ank/ES3011
1
Fork 0
Code Releases Activity

goes and turns around

Browse source
This commit is contained in:
Andy Killorin 2025-12-10 14:43:01 -05:00
parent 95c821664c
commit fb1667b1e2
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
2 changed files with 35 additions and 25 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

13
robot_controller/constants.h
View file

@ -13,16 +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 = 10.0; // cm const float FORWARD_DISTANCE = 73.0; // cm
const float TURN_AMOUNT = 175.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 RETURN_DISTANCE = 100.0; // cm
const float FF_ACCEL = 2.3; // motor acceleration feedforward const float FF_ACCEL = 2.5; // motor acceleration feedforward
const float FF_VEL = 0.7; // motor velocity feedforward const float FF_VEL = 0.9; // motor velocity feedforward
const float FF_STAT = 16.4; // motor static friction const float FF_STAT = 16.4; // motor static friction
const float KP = 2.0; // proportional const float KP = 2.7; // 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.25; // position proportional
const float KPI = 0.05; // position integral const float KPI = 0.07; // position integral

47
robot_controller/robot_controller.ino
View file

@ -106,35 +106,44 @@ void loop() {
((left_velocity > 0.0) ? FF_STAT : -FF_STAT); ((left_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); if (setpoint.complete) {
write_rpm_ff(&left_motor, left_velocity * CMS_RPM , feedforward_left); right_motor.write_angle((setpoint.position / DEG_CM) + right_home);
left_motor.write_angle((turnsig * setpoint.position / DEG_CM) + left_home);
} else {
write_rpm_ff(&right_motor, right_velocity * CMS_RPM , feedforward_right);
write_rpm_ff(&left_motor, left_velocity * CMS_RPM , feedforward_left);
}
// send telemetry // send telemetry
int32_t rpm = left_motor.read_rpm(); int32_t rpm = left_motor.read_rpm();
int32_t pos = left_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("ff:"); Serial.print("el:");
Serial.print(left_effort); Serial.print(pos_err_left);
Serial.print(",time:"); Serial.print(",er:");
Serial.print(time); Serial.print(pos_err_right);
Serial.print(",distgoal:"); //Serial.print("ff:");
Serial.print(setpoint.position); //Serial.print(left_effort);
Serial.print(",dist:"); //Serial.print(",time:");
Serial.print(pos * DEG_CM); //Serial.print(time);
Serial.print(",cms/s:"); //Serial.print(",distgoal:");
Serial.print(rpm / CMS_RPM); //Serial.print(setpoint.position);
Serial.print(",setvel:"); //Serial.print(",dist:");
Serial.print(setpoint.velocity); //Serial.print(pos * DEG_CM);
Serial.print(",setacc:"); //Serial.print(",cms/s:");
Serial.print(setpoint.acceleration); //Serial.print(rpm / CMS_RPM);
Serial.print(",Err:"); //Serial.print(",setvel:");
Serial.print(error / CMS_RPM); //Serial.print(setpoint.velocity);
//Serial.print(",setacc:");
//Serial.print(setpoint.acceleration);
//Serial.print(",Err:");
//Serial.print(error / CMS_RPM);
Serial.println(""); Serial.println("");
// 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 < 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);