ank/ES3011
1
Fork 0
Code Releases Activity

Compare commits

base: ank:95c821664c84e7cc88f560f94ac2112471c8f641
Branches Tags
ank:master
ank:velff
ank:cad
ank:submission
..
compare: ank:0ef16df511ca7d474bef9bf3d6f59723c6958b61
Branches Tags
ank:velff
ank:master
ank:cad
ank:submission

No commits in common. "95c821664c84e7cc88f560f94ac2112471c8f641" and "0ef16df511ca7d474bef9bf3d6f59723c6958b61" have entirely different histories.
95c821664c ... 0ef16df511

2 changed files with 23 additions and 26 deletions
Show stats Expand all files Collapse all files

10
robot_controller/constants.h
View file

@ -14,15 +14,15 @@ const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
const float DEG_CM = (PI*WHEEL_DIAMETER) / 360.0; const float DEG_CM = (PI*WHEEL_DIAMETER) / 360.0;
const float FORWARD_DISTANCE = 10.0; // cm const float FORWARD_DISTANCE = 10.0; // cm
const float TURN_AMOUNT = 175.0; // degrees const float TURN_AMOUNT = 180.0; // degrees
const float TURN_DISTANCE = (TURN_AMOUNT / 360.0) * WHEEL_TO_WHEEL * PI; const float TURN_DISTANCE = (TURN_AMOUNT / 360.0) * WHEEL_TO_WHEEL * PI;
const float RETURN_DISTANCE = 100.0; // cm const float RETURN_DISTANCE = 100.0; // cm
const float FF_ACCEL = 2.3; // motor acceleration feedforward const float FF_ACCEL = 0.7; // motor acceleration feedforward
const float FF_VEL = 0.7; // motor velocity feedforward const float FF_VEL = 0.6; // motor velocity feedforward
const float FF_STAT = 16.4; // motor static friction const float FF_STAT = 15.4; // motor static friction
const float KP = 2.0; // proportional const float KP = 2.0; // proportional
const float KI = 0.5; // integral const float KI = 0.5; // integral
const float KD = 0.05; // derivative const float KD = 0.05; // derivative
const float KPP = 0.25; // position proportional const float KPP = 0.15; // position proportional
const float KPI = 0.05; // position integral const float KPI = 0.05; // position integral

39
robot_controller/robot_controller.ino
View file

@ -62,59 +62,56 @@ void setup() {
void loop() { void loop() {
float time = (float)millis() / 1000.0 - phase_start; float time = (float)millis() / 1000.0 - phase_start;
float end_time;
switch (robot_state) { switch (robot_state) {
case FORWARD: case FORWARD:
case RETURN: case RETURN:
setpoint = trapezoidal_planner(&forward, time); setpoint = trapezoidal_planner(&forward, time);
end_time = trapezoidal_time(&forward);
break; break;
case TURN: case TURN:
setpoint = trapezoidal_planner(&turn, time); setpoint = trapezoidal_planner(&turn, time);
end_time = trapezoidal_time(&turn);
break; break;
} }
float turnsig = (robot_state == TURN ? 1.0 : -1.0); // direction of travel for right motor float turnsig = (robot_state == TURN ? 1.0 : -1.0); // direction of travel for right motor
// position PI controller // position PI controller
float pos_err_right = ((setpoint.position / DEG_CM) + right_home) - right_motor.read_angle(); float pos_err_left = ((setpoint.position / DEG_CM) + left_home) - left_motor.read_angle();
float pos_err_left = ((turnsig * setpoint.position / DEG_CM) + left_home) - left_motor.read_angle(); float pos_err_right = ((turnsig * setpoint.position / DEG_CM) + right_home) - right_motor.read_angle();
// delta should be ~4ms // delta should be ~4ms
if (time > time_p) { if (time > time_p) {
float delta = time - time_p; float delta = time - time_p;
left_iaccum += pos_err_left * delta;
right_iaccum += pos_err_right * delta; right_iaccum += pos_err_right * delta;
left_iaccum += pos_err_left * delta;
} }
time_p = time; time_p = time;
float right_effort = pos_err_right * KPP + right_iaccum * KPI;
float left_effort = pos_err_left * KPP + left_iaccum * KPI; float left_effort = pos_err_left * KPP + left_iaccum * KPI;
float right_effort = pos_err_right * KPP + right_iaccum * KPI;
float right_velocity = setpoint.velocity + right_effort; float left_velocity = setpoint.velocity + left_effort;
float left_velocity = (turnsig * setpoint.velocity) + left_effort; float right_velocity = (turnsig * setpoint.velocity) + right_effort;
// calculate feedforward from motion profile and position PI data // calculate feedforward from motion profile and position PI data
float feedforward_right =
setpoint.acceleration * CMS_RPM * FF_ACCEL +
right_velocity * CMS_RPM * FF_VEL +
((right_velocity > 0.0) ? FF_STAT : -FF_STAT);
float feedforward_left = float feedforward_left =
setpoint.acceleration * CMS_RPM * FF_ACCEL + setpoint.acceleration * CMS_RPM * FF_ACCEL +
left_velocity * CMS_RPM * FF_VEL + left_velocity * CMS_RPM * FF_VEL +
((left_velocity > 0.0) ? FF_STAT : -FF_STAT); ((left_velocity > 0.0) ? FF_STAT : -FF_STAT);
float feedforward_right =
setpoint.acceleration * CMS_RPM * FF_ACCEL +
right_velocity * CMS_RPM * FF_VEL +
((right_velocity > 0.0) ? FF_STAT : -FF_STAT);
// arbitrary feedforward with on-board velocity PID // arbitrary feedforward with on-board velocity PID
write_rpm_ff(&right_motor, right_velocity * CMS_RPM , feedforward_right); write_rpm_ff(&left_motor, left_velocity * CMS_RPM , feedforward_left);
write_rpm_ff(&left_motor, left_velocity * CMS_RPM , feedforward_left); write_rpm_ff(&right_motor, right_velocity * CMS_RPM , feedforward_right);
// send telemetry // send telemetry
int32_t rpm = left_motor.read_rpm(); int32_t rpm = -right_motor.read_rpm();
int32_t pos = left_motor.read_angle(); int32_t pos = -right_motor.read_angle();
int32_t error = setpoint.velocity * CMS_RPM - rpm; int32_t error = setpoint.velocity * CMS_RPM - rpm;
Serial.print("ff:"); Serial.print("ff:");
Serial.print(left_effort); Serial.print(right_effort);
Serial.print(",time:"); Serial.print(",time:");
Serial.print(time); Serial.print(time);
Serial.print(",distgoal:"); Serial.print(",distgoal:");
@ -134,10 +131,10 @@ void loop() {
// total wheel offness in degrees // total wheel offness in degrees
float total_pos_error = abs(pos_err_left) + abs(pos_err_right); float total_pos_error = abs(pos_err_left) + abs(pos_err_right);
// move on if at setpoint TODO: give up after timeout // move on if at setpoint TODO: give up after timeout
if (setpoint.complete && (total_pos_error < 3.0 || time - end_time > 3.0)) { if (setpoint.complete && total_pos_error < 10.0) {
// rehome // rehome
right_home += (setpoint.position / DEG_CM); left_home += (setpoint.position / DEG_CM);
left_home += (turnsig * setpoint.position / DEG_CM); right_home += (turnsig * setpoint.position / DEG_CM);
// zero accumulators // zero accumulators
right_iaccum = 0.0; left_iaccum = 0.0; right_iaccum = 0.0; left_iaccum = 0.0;