ank/ES3011
1
Fork 0
Code Releases Activity

feedforward tuning first round

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

10
robot_controller/constants.h
View file

@ -17,9 +17,9 @@ const float FORWARD_DISTANCE = 30.0; // cm
const float TURN_AMOUNT = 180.0; // degrees const float TURN_AMOUNT = 180.0; // degrees
const float RETURN_DISTANCE = 100.0; // cm const float RETURN_DISTANCE = 100.0; // cm
const float FF_ACCEL = 100.0; // motor acceleration feedforward const float FF_ACCEL = 3.0; // motor acceleration feedforward
const float FF_VEL = 0.9; // motor velocity feedforward const float FF_VEL = 3.9; // motor velocity feedforward
const float FF_STAT = 100.0; // motor static friction const float FF_STAT = 2.4; // motor static friction
const float KP = 3.7; // proportional const float KP = 3.0; // proportional
const float KI = 0.3; // integral const float KI = 0.0; // integral
const float KD = 0.0; // derivative const float KD = 0.0; // derivative

27
robot_controller/robot_controller.ino
View file

@ -41,9 +41,9 @@ void setup() {
Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
// TUNE VELOCITY PID // TUNE VELOCITY PID
left_motor.tune_vel_pid(0.9, 3.7,0.3,0.0); //left_motor.tune_vel_pid(0.9, 3.7,0.3,0.0);
delay(10); //delay(10);
right_motor.tune_vel_pid(0.9, 3.7,0.3,0.0); //right_motor.tune_vel_pid(0.9, 3.7,0.3,0.0);
delay(10); delay(10);
//right_motor.set_direction(PIDDirection::DIRECT); //right_motor.set_direction(PIDDirection::DIRECT);
@ -71,31 +71,34 @@ void loop() {
break; break;
} }
float feedforward = float feedforward =
setpoint.acceleration * CMS_RPM * FF_ACCEL + setpoint.acceleration * CMS_RPM * FF_ACCEL +
setpoint.velocity * CMS_RPM * FF_VEL + setpoint.velocity * CMS_RPM * FF_VEL +
FF_STAT; ((setpoint.velocity > 0.0) ? FF_STAT : -FF_STAT);
left_motor.write_rpm_ff(setpoint.velocity * CMS_RPM, feedforward); write_rpm_ff(&left_motor, setpoint.velocity * CMS_RPM, feedforward);
delay(1); delay(1);
right_motor.write_rpm_ff((robot_state == TURN ? velocity : -velocity) * CMS_RPM, feedforward); //write_rpm_ff(&right_motor, (robot_state == TURN ? velocity : -velocity) * CMS_RPM, feedforward);
delay(19); delay(19);
// READ MOTOR POSITION // READ MOTOR POSITION
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("time:"); Serial.print("ff:");
Serial.print(feedforward);
Serial.print(",time:");
Serial.print(time); Serial.print(time);
Serial.print(",dist goal:"); Serial.print(",distgoal:");
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(",cms/s:"); Serial.print(",cms/s:");
Serial.print(rpm / CMS_RPM); Serial.print(rpm / CMS_RPM);
Serial.print(",Setpoint:"); Serial.print(",setvel:");
Serial.print(setpoint.velocity); Serial.print(setpoint.velocity);
Serial.print(",setacc:");
Serial.print(setpoint.acceleration);
Serial.print(",Err:"); Serial.print(",Err:");
Serial.print(error / CMS_RPM); Serial.print(error / CMS_RPM);
Serial.println(""); Serial.println("");