ank/ES3011
1
Fork 0
Code Releases Activity

Compare commits

base: ank:96a81eb2d192399b44945c9cd5882693661088d0
Branches Tags
ank:master
ank:velff
ank:cad
ank:submission
..
compare: ank:acec3623dd9b5bd69bfd75f2dfcbc5412e35b4a6
Branches Tags
ank:velff
ank:master
ank:cad
ank:submission

No commits in common. "96a81eb2d192399b44945c9cd5882693661088d0" and "acec3623dd9b5bd69bfd75f2dfcbc5412e35b4a6" have entirely different histories.
96a81eb2d1 ... acec3623dd

3 changed files with 44 additions and 85 deletions
Show stats Expand all files Collapse all files

39
robot_controller/constants.h
View file

@ -1,9 +1,11 @@
#pragma once #pragma once
const float ACCEL_LIMIT = 6.0; // cm/s/s const float ACCEL_LIMIT = 5.0; // cm/s/s
const float VEL_LIMIT = 12.0; // cm/s const float VEL_LIMIT = 16.0; // cm/s
const float WHEEL_DIAMETER = 6.61; // cm const float BACKLASH_RIGHT = 0.0; // degrees
const float WHEEL_TO_WHEEL = 8.6; // cm const float BACKLASH_LEFT = 0.0; // degrees
const float WHEEL_DIAMETER = 10.25; // cm
const float WHEEL_TO_WHEEL = 9.0; // cm
// centimeters per second to rpm // centimeters per second to rpm
const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER); const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
@ -11,22 +13,17 @@ const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
// degrees to centimeters // degrees to centimeters
const float DEG_CM = (PI*WHEEL_DIAMETER) / 360.0; const float DEG_CM = (PI*WHEEL_DIAMETER) / 360.0;
const float FORWARD_DISTANCE = 100.0; // cm const float FORWARD_DISTANCE = 73.0; // cm
const float TURN_AMOUNT = 190.0; // degrees //const float FORWARD_DISTANCE = 10.0; // cm
const float TURN_AMOUNT = 146.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 KP = 1.50; // proportional const float FF_ACCEL = 2.5; // motor acceleration feedforward
const float KI = 0.12; // integral const float FF_VEL = 0.9; // motor velocity feedforward
const float KD = 0.00; // derivative const float FF_STAT = 16.4; // motor static friction
const float Kv = 1.85; // onboard velocity feedforward const float KP = 2.7; // proportional
const float KI = 0.5; // integral
const float TURN_KP = 1.50; // proportional const float KD = 0.05; // derivative
const float TURN_KI = 0.10; // integral const float KPP = 0.25; // position proportional
const float TURN_KD = 0.00; // derivative const float KPI = 0.07; // position integral
const float TURN_Kv = 2.6; // onboard velocity feedforward
const float KPP = 0.14; // position proportional
const float KPI = 0.12; // position integral
const float KRP = 0.0; // rotation proportional
const float V_MIN = 5.5; // velocity minimum
const float V_KMIN = 0.7; // velocity minimum coefficient

86
robot_controller/robot_controller.ino
View file

@ -48,26 +48,11 @@ void setup() {
while(!Serial); while(!Serial);
Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
left_motor.tune_pos_pid(0.4, 0.024, 0.008); left_motor.write_rpm(0.0);
delay(1); delay(1);
right_motor.tune_pos_pid(0.4, 0.024, 0.008); right_motor.write_rpm(0.0);
delay(1); delay(999);
left_motor.tune_vel_pid(Kv, KP,KI,KD);
delay(1);
right_motor.tune_vel_pid(Kv, KP,KI,KD);
delay(1);
delay(10);
// runs to mend bad state
left_motor.home();
delay(1);
left_motor.write_angle(0.0);
delay(1);
right_motor.home();
delay(1);
right_motor.write_angle(0.0);
delay(3000);
phase_start = (float)millis() / 1000.0; phase_start = (float)millis() / 1000.0;
robot_state = FORWARD; robot_state = FORWARD;
@ -96,9 +81,6 @@ void loop() {
float pos_err_right = ((setpoint.position / DEG_CM) + right_home) - right_motor.read_angle(); float pos_err_right = ((setpoint.position / DEG_CM) + right_home) - right_motor.read_angle();
float pos_err_left = ((turnsig * 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();
// total wheel offness in degrees
float total_pos_error = abs(pos_err_left) + abs(pos_err_right);
// 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;
@ -110,41 +92,36 @@ void loop() {
float right_effort = pos_err_right * KPP + right_iaccum * KPI; 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;
#define sgn(x) ((x) < 0 ? -1 : ((x) > 0 ? 1 : 0))
// boost on when slowing down
//if (setpoint.velocity < V_MIN && setpoint.acceleration < 0.0 && total_pos_error > 4.0) {
// right_effort += (float )sgn(right_effort) * (setpoint.velocity - V_MIN) * V_KMIN;
// left_effort += (float )sgn(left_effort) * (setpoint.velocity - V_MIN) * V_KMIN;
//}
float right_velocity = setpoint.velocity + right_effort; float right_velocity = setpoint.velocity + right_effort;
float left_velocity = (turnsig * setpoint.velocity) + left_effort; float left_velocity = (turnsig * setpoint.velocity) + left_effort;
// 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 =
setpoint.acceleration * CMS_RPM * FF_ACCEL +
left_velocity * CMS_RPM * FF_VEL +
((left_velocity > 0.0) ? FF_STAT : -FF_STAT);
// arbitrary feedforward with on-board velocity PID // arbitrary feedforward with on-board velocity PID
if (setpoint.complete && robot_state == TURN) { if (setpoint.complete) {
right_motor.write_angle((setpoint.position / DEG_CM) + right_home); right_motor.write_angle((setpoint.position / DEG_CM) + right_home);
left_motor.write_angle((turnsig * setpoint.position / DEG_CM) + left_home); left_motor.write_angle((turnsig * setpoint.position / DEG_CM) + left_home);
} else { } else {
right_motor.write_rpm(right_velocity * CMS_RPM); write_rpm_ff(&right_motor, right_velocity * CMS_RPM , feedforward_right);
delay(1); write_rpm_ff(&left_motor, left_velocity * CMS_RPM , feedforward_left);
left_motor.write_rpm(left_velocity * CMS_RPM);
delay(1);
} }
// send telemetry // send telemetry
int32_t rpm = right_motor.read_rpm(); int32_t rpm = left_motor.read_rpm();
int32_t pos = right_motor.read_angle(); int32_t pos = left_motor.read_angle();
int32_t error = setpoint.velocity * CMS_RPM - rpm; int32_t error = setpoint.velocity * CMS_RPM - rpm;
Serial.print("el:"); Serial.print("el:");
Serial.print(pos_err_left); Serial.print(pos_err_left);
Serial.print(",er:"); Serial.print(",er:");
Serial.print(pos_err_right); Serial.print(pos_err_right);
Serial.print(",il:");
Serial.print(left_iaccum);
Serial.print(",ir:");
Serial.print(right_iaccum);
Serial.print(",effr:");
Serial.print(right_effort);
//Serial.print("ff:"); //Serial.print("ff:");
//Serial.print(left_effort); //Serial.print(left_effort);
//Serial.print(",time:"); //Serial.print(",time:");
@ -153,20 +130,20 @@ void loop() {
//Serial.print(setpoint.position); //Serial.print(setpoint.position);
//Serial.print(",dist:"); //Serial.print(",dist:");
//Serial.print(pos * DEG_CM); //Serial.print(pos * DEG_CM);
Serial.print(",vel:"); //Serial.print(",cms/s:");
Serial.print(rpm / CMS_RPM); //Serial.print(rpm / CMS_RPM);
Serial.print(",setvel:"); //Serial.print(",setvel:");
Serial.print(setpoint.velocity); //Serial.print(setpoint.velocity);
Serial.print(",rvel:");
Serial.print(right_velocity);
//Serial.print(",setacc:"); //Serial.print(",setacc:");
//Serial.print(setpoint.acceleration); //Serial.print(setpoint.acceleration);
//Serial.print(",Err:"); //Serial.print(",Err:");
//Serial.print(error / CMS_RPM); //Serial.print(error / CMS_RPM);
Serial.println(""); Serial.println("");
// total wheel offness in degrees
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 > 5.0)) { if (setpoint.complete && (total_pos_error < 3.0 || time - end_time > 7.0)) {
// rehome // rehome
right_home += (setpoint.position / DEG_CM); right_home += (setpoint.position / DEG_CM);
left_home += (turnsig * setpoint.position / DEG_CM); left_home += (turnsig * setpoint.position / DEG_CM);
@ -175,21 +152,10 @@ void loop() {
switch (robot_state) { switch (robot_state) {
case FORWARD: case FORWARD:
// turn tuning
left_motor.tune_vel_pid(TURN_Kv, TURN_KP,TURN_KI,TURN_KD);
delay(1);
right_motor.tune_vel_pid(TURN_Kv,TURN_KP,TURN_KI,TURN_KD);
robot_state = TURN; robot_state = TURN;
right_iaccum = left_iaccum = 60.0;
phase_start = (float)millis() / 1000.0; phase_start = (float)millis() / 1000.0;
break; break;
case TURN: case TURN:
// straight line tuning
left_motor.tune_vel_pid(Kv, KP,KI,KD);
delay(1);
right_motor.tune_vel_pid(Kv, KP,KI,KD);
robot_state = RETURN; robot_state = RETURN;
phase_start = (float)millis() / 1000.0; phase_start = (float)millis() / 1000.0;

4
robot_controller/trapezoidal.ino
View file

@ -11,10 +11,6 @@ struct Setpoint trapezoidal_planner(struct Trapezoidal* trapezoidal, float time)
float time_accelerating = max_vel / max_acc; float time_accelerating = max_vel / max_acc;
float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating; float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating;
if (time < 0.0) {
return setpoint;
}
if (2.0 * distance_while_accelerating > dist) { if (2.0 * distance_while_accelerating > dist) {
// triangular // triangular
float peak_velocity_time = sqrt(dist/max_acc); float peak_velocity_time = sqrt(dist/max_acc);