ank/ES3011
1
Fork 0
Code Releases Activity

tuning

Browse source
This commit is contained in:
Andy Killorin 2025-12-10 16:24:44 -05:00
parent acec3623dd
commit 7e6ba786eb
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
3 changed files with 15 additions and 5 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

9
robot_controller/constants.h
View file

@ -19,11 +19,12 @@ const float TURN_AMOUNT = 146.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;
const float RETURN_DISTANCE = 100.0; // cm const float RETURN_DISTANCE = 100.0; // cm
const float FF_ACCEL = 2.5; // motor acceleration feedforward const float FF_ACCEL = 2.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 = 16.4; // motor static friction const float FF_STAT = 16.4; // motor static friction
const float KP = 2.7; // proportional const float KP = 2.9; // proportional
const float KI = 0.5; // integral const float KI = 0.8; // integral
const float KD = 0.05; // derivative const float KD = 0.05; // derivative
const float KPP = 0.25; // position proportional const float KPP = 0.25; // position proportional
const float KPI = 0.07; // position integral const float KPI = 0.17; // position integral
const float KRP = 0.07; // rotation proportional

7
robot_controller/robot_controller.ino
View file

@ -52,7 +52,7 @@ void setup() {
left_motor.write_rpm(0.0); left_motor.write_rpm(0.0);
delay(1); delay(1);
right_motor.write_rpm(0.0); right_motor.write_rpm(0.0);
delay(999); delay(2999);
phase_start = (float)millis() / 1000.0; phase_start = (float)millis() / 1000.0;
robot_state = FORWARD; robot_state = FORWARD;
@ -122,6 +122,10 @@ void loop() {
Serial.print(pos_err_left); Serial.print(pos_err_left);
Serial.print(",er:"); Serial.print(",er:");
Serial.print(pos_err_right); Serial.print(pos_err_right);
Serial.print(",il:");
Serial.print(left_iaccum);
Serial.print(",ir:");
Serial.print(right_iaccum);
//Serial.print("ff:"); //Serial.print("ff:");
//Serial.print(left_effort); //Serial.print(left_effort);
//Serial.print(",time:"); //Serial.print(",time:");
@ -153,6 +157,7 @@ void loop() {
switch (robot_state) { switch (robot_state) {
case FORWARD: case FORWARD:
robot_state = TURN; robot_state = TURN;
right_iaccum = left_iaccum = 8.0;
phase_start = (float)millis() / 1000.0; phase_start = (float)millis() / 1000.0;
break; break;
case TURN: case TURN:

4
robot_controller/trapezoidal.ino
View file

@ -11,6 +11,10 @@ struct Setpoint trapezoidal_planner(struct Trapezoidal* trapezoidal, float time)
float time_accelerating = max_vel / max_acc; float time_accelerating = max_vel / max_acc;
float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating; float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating;
if (time < 0.0) {
return setpoint;
}
if (2.0 * distance_while_accelerating > dist) { if (2.0 * distance_while_accelerating > dist) {
// triangular // triangular
float peak_velocity_time = sqrt(dist/max_acc); float peak_velocity_time = sqrt(dist/max_acc);