ank/ES3011
1
Fork 0
Code Releases Activity

switch to onboard feedforward

Browse source
This commit is contained in:
Andy Killorin 2025-12-12 12:17:45 -05:00
parent 0a09c4b785
commit 7b7cd3d8e7
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
2 changed files with 8 additions and 15 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

3
robot_controller/constants.h
View file

@ -22,9 +22,10 @@ const float RETURN_DISTANCE = 100.0; // cm
const float FF_ACCEL = 5.4; // motor acceleration feedforward const float FF_ACCEL = 5.4; // motor acceleration feedforward
const float FF_VEL = 0.9; // motor velocity feedforward const float FF_VEL = 0.9; // motor velocity feedforward
const float FF_STAT = 18.4; // motor static friction const float FF_STAT = 18.4; // motor static friction
const float KP = 9.13; // proportional const float KP = 0.00; // proportional
const float KI = 0.0; // integral const float KI = 0.0; // integral
const float KD = 0.0; // derivative const float KD = 0.0; // derivative
const float Kv = 1.0; // onboard velocity feedforward
const float KPP = 0.0; // position proportional const float KPP = 0.0; // position proportional
const float KPI = 0.0; // position integral const float KPI = 0.0; // position integral
const float KRP = 0.0; // rotation proportional const float KRP = 0.0; // rotation proportional

20
robot_controller/robot_controller.ino
View file

@ -49,8 +49,10 @@ void setup() {
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.tune_pos_pid(0.4, 0.024, 0.008);
//right_motor.tune_pos_pid(0.4, 0.024, 0.008); right_motor.tune_pos_pid(0.4, 0.024, 0.008);
left_motor.tune_vel_pid(Kv, KP,KI,KD);
left_motor.tune_vel_pid(Kv, KP,KI,KD);
//delay(1000); //delay(1000);
//left_motor.home(); //left_motor.home();
@ -103,23 +105,13 @@ void loop() {
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 && 0) { if (setpoint.complete && 0) {
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 {
write_rpm_ff(&right_motor, right_velocity * CMS_RPM , feedforward_right); right_motor.write_rpm(right_velocity * CMS_RPM);
write_rpm_ff(&left_motor, left_velocity * CMS_RPM , feedforward_left); left_motor.write_rpm(left_velocity * CMS_RPM);
} }
// send telemetry // send telemetry