ank/ES3011
1
Fork 0
Code Releases Activity

position PI loop

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

4
robot_controller/constants.h
View file

@ -4,7 +4,7 @@ const float ACCEL_LIMIT = 5.0; // cm/s/s
const float VEL_LIMIT = 10.0; // cm/s
const float BACKLASH_RIGHT = 0.0; // degrees
const float BACKLASH_LEFT = 0.0; // degrees
const float WHEEL_DIAMETER = 10.00; // cm
const float WHEEL_DIAMETER = 9.25; // cm
const float WHEEL_TO_WHEEL = 10.0; // cm
// centimeters per second to rpm
@ -24,3 +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 KPI = 1.50; // position integral

62
robot_controller/robot_controller.ino
View file

@ -19,7 +19,6 @@ enum ROBOT_STATE {
float CURRENT_POSITION = 0.0;
float CURRENT_ROTATION = 0.0;
struct Setpoint setpoint;
float velocity = 40.0;
// start of current state
float phase_start;
enum ROBOT_STATE robot_state;
@ -27,6 +26,10 @@ enum ROBOT_STATE robot_state;
struct Trapezoidal forward = {VEL_LIMIT, ACCEL_LIMIT, FORWARD_DISTANCE};
struct Trapezoidal turn = {VEL_LIMIT, ACCEL_LIMIT, TURN_DISTANCE};
float left_iaccum = 0.0;
float right_iaccum = 0.0;
float time_p = 0.0;
void write_rpm_ff(SmartMotor* motor, int32_t rpm, float ff) {
if (rpm == 0) {rpm = 1;};
float kV = ff / (float) rpm; // calculate velocity feedforward that causes desired absolute feedforward
@ -43,15 +46,15 @@ void setup() {
Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
// TUNE VELOCITY PID
//left_motor.tune_vel_pid(0.9, 3.7,0.3,0.0);
//delay(10);
//right_motor.tune_vel_pid(0.9, 3.7,0.3,0.0);
delay(10);
//right_motor.set_direction(PIDDirection::DIRECT);
left_motor.write_rpm(0.0);
delay(1);
right_motor.write_rpm(0.0);
delay(999);
phase_start = (float)millis() / 1000.0;
robot_state = FORWARD;
left_motor.home();
right_motor.home();
}
void loop() {
@ -66,25 +69,39 @@ 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 feedforward_right =
setpoint.acceleration * CMS_RPM * 9.0 +
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
write_rpm_ff(&left_motor, setpoint.velocity * CMS_RPM, feedforward);
write_rpm_ff(&right_motor, (robot_state == TURN ? setpoint.velocity : -setpoint.velocity) * CMS_RPM, feedforward);
// 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();
// READ MOTOR POSITION
int32_t rpm = -right_motor.read_rpm();
int32_t pos = -right_motor.read_angle();
// delta should be ~4ms
if (time > time_p) {
float delta = time - time_p;
right_iaccum += pos_err_right * delta;
left_iaccum += pos_err_left * delta;
}
time_p = time;
float left_effort = pos_err_left * KPP + left_iaccum * KPI;
float right_effort = pos_err_right * KPP + right_iaccum * KPI;
// 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);
// send telemetry
int32_t rpm = left_motor.read_rpm();
int32_t pos = left_motor.read_angle();
int32_t error = setpoint.velocity * CMS_RPM - rpm;
Serial.print("ff:");
Serial.print(feedforward_right);
Serial.print(left_effort);
Serial.print(",time:");
Serial.print(time);
Serial.print(",distgoal:");
@ -101,15 +118,22 @@ void loop() {
Serial.print(error / CMS_RPM);
Serial.println("");
// move on if at setpoint TODO: check that the error is low as well
if (setpoint.complete) {
// 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
if (setpoint.complete && total_pos_error < 10.0) {
switch (robot_state) {
case FORWARD:
robot_state = TURN;
// TODO: don't forget about remaining err (add previous setpoint to global offset?)
left_motor.home();
right_motor.home();
phase_start = (float)millis() / 1000.0;
break;
case TURN:
robot_state = RETURN;
left_motor.home();
right_motor.home();
phase_start = (float)millis() / 1000.0;
break;