ank/ES3011
1
Fork 0
Code Releases Activity

don't forget accumulated position error

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

6
robot_controller/constants.h
View file

@ -15,7 +15,7 @@ const float DEG_CM = (PI*WHEEL_DIAMETER) / 360.0;
const float FORWARD_DISTANCE = 100.0; // cm const float FORWARD_DISTANCE = 100.0; // cm
const float TURN_AMOUNT = 180.0; // degrees const float TURN_AMOUNT = 180.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 * 0.5;
const float RETURN_DISTANCE = 100.0; // cm const float RETURN_DISTANCE = 100.0; // cm
const float FF_ACCEL = 0.7; // motor acceleration feedforward const float FF_ACCEL = 0.7; // motor acceleration feedforward
@ -24,5 +24,5 @@ const float FF_STAT = 15.4; // motor static friction
const float KP = 2.0; // proportional const float KP = 2.0; // 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.55; // position proportional const float KPP = 0.15; // position proportional
const float KPI = 1.50; // position integral const float KPI = 0.05; // position integral

18
robot_controller/robot_controller.ino
View file

@ -30,6 +30,9 @@ float left_iaccum = 0.0;
float right_iaccum = 0.0; float right_iaccum = 0.0;
float time_p = 0.0; float time_p = 0.0;
float left_home = 0.0;
float right_home = 0.0;
void write_rpm_ff(SmartMotor* motor, int32_t rpm, float ff) { void write_rpm_ff(SmartMotor* motor, int32_t rpm, float ff) {
if (rpm == 0) {rpm = 1;}; if (rpm == 0) {rpm = 1;};
float kV = ff / (float) rpm; // calculate velocity feedforward that causes desired absolute feedforward float kV = ff / (float) rpm; // calculate velocity feedforward that causes desired absolute feedforward
@ -73,8 +76,8 @@ void loop() {
float turnsig = (robot_state == TURN ? 1.0 : -1.0); // direction of travel for right motor float turnsig = (robot_state == TURN ? 1.0 : -1.0); // direction of travel for right motor
// position PI controller // position PI controller
float pos_err_left = (setpoint.position / DEG_CM) - left_motor.read_angle(); float pos_err_left = ((setpoint.position / DEG_CM) + left_home) - left_motor.read_angle();
float pos_err_right = (turnsig * setpoint.position / DEG_CM) - right_motor.read_angle(); float pos_err_right = ((turnsig * setpoint.position / DEG_CM) + right_home) - right_motor.read_angle();
// delta should be ~4ms // delta should be ~4ms
if (time > time_p) { if (time > time_p) {
@ -130,18 +133,19 @@ void loop() {
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 < 10.0) { if (setpoint.complete && total_pos_error < 10.0) {
// rehome
left_home += (setpoint.position / DEG_CM);
right_home += (turnsig * setpoint.position / DEG_CM);
// zero accumulators
right_iaccum = 0.0; left_iaccum = 0.0;
switch (robot_state) { switch (robot_state) {
case FORWARD: case FORWARD:
robot_state = TURN; 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; phase_start = (float)millis() / 1000.0;
break; break;
case TURN: case TURN:
robot_state = RETURN; robot_state = RETURN;
left_motor.home();
right_motor.home();
phase_start = (float)millis() / 1000.0; phase_start = (float)millis() / 1000.0;
break; break;