ank/ES3011
1
Fork 0
Code Releases Activity

basic position PI tuning

Browse source
This commit is contained in:
Andy Killorin 2025-12-10 12:09:07 -05:00
parent a1b4dfce79
commit 0b0a656bf3
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
2 changed files with 20 additions and 12 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
robot_controller/constants.h
View file

@ -24,5 +24,5 @@ const float FF_STAT = 15.4; // motor static friction
const float KP = 2.0; // proportional
const float KI = 0.5; // integral
const float KD = 0.05; // derivative
const float KPP = 3.55; // position proportional
const float KPP = 0.55; // position proportional
const float KPI = 1.50; // position integral

30
robot_controller/robot_controller.ino
View file

@ -69,17 +69,12 @@ void loop() {
break;
}
// calculate feedforward from motion profile
float feedforward =
setpoint.acceleration * CMS_RPM * FF_ACCEL +
setpoint.velocity * CMS_RPM * FF_VEL +
((setpoint.velocity > 0.0) ? FF_STAT : -FF_STAT);
float turnsig = (robot_state == TURN ? 1.0 : -1.0); // direction of travel for right motor
// position PI controller
float pos_err_left = (setpoint.position / DEG_CM) - left_motor.read_angle();
float pos_err_right = (turnsig * setpoint.position / DEG_CM) - left_motor.read_angle();
float pos_err_right = (turnsig * setpoint.position / DEG_CM) - right_motor.read_angle();
// delta should be ~4ms
if (time > time_p) {
@ -92,16 +87,29 @@ void loop() {
float left_effort = pos_err_left * KPP + left_iaccum * KPI;
float right_effort = pos_err_right * KPP + right_iaccum * KPI;
float left_velocity = setpoint.velocity + left_effort;
float right_velocity = (turnsig * setpoint.velocity) + right_effort;
// calculate feedforward from motion profile and position PI data
float feedforward_left =
setpoint.acceleration * CMS_RPM * FF_ACCEL +
left_velocity * CMS_RPM * FF_VEL +
((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
write_rpm_ff(&left_motor, setpoint.velocity * CMS_RPM + left_effort, feedforward);
//write_rpm_ff(&right_motor, turnsig * setpoint.velocity * CMS_RPM , feedforward + right_effort);
write_rpm_ff(&left_motor, left_velocity * CMS_RPM , feedforward_left);
write_rpm_ff(&right_motor, right_velocity * CMS_RPM , feedforward_right);
// send telemetry
int32_t rpm = left_motor.read_rpm();
int32_t pos = left_motor.read_angle();
int32_t rpm = -right_motor.read_rpm();
int32_t pos = -right_motor.read_angle();
int32_t error = setpoint.velocity * CMS_RPM - rpm;
Serial.print("ff:");
Serial.print(left_effort);
Serial.print(right_effort);
Serial.print(",time:");
Serial.print(time);
Serial.print(",distgoal:");