ank/ES3011
1
Fork 0
Code Releases Activity

works quite well

Browse source
This commit is contained in:
Andy Killorin 2025-12-12 14:37:52 -05:00
parent be3c8a359b
commit 697487a0dd
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
2 changed files with 32 additions and 14 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

23
robot_controller/constants.h
View file

@ -1,6 +1,6 @@
#pragma once #pragma once
const float ACCEL_LIMIT = 7.0; // cm/s/s const float ACCEL_LIMIT = 6.0; // cm/s/s
const float VEL_LIMIT = 12.0; // cm/s const float VEL_LIMIT = 12.0; // cm/s
const float WHEEL_DIAMETER = 6.61; // cm const float WHEEL_DIAMETER = 6.61; // cm
const float WHEEL_TO_WHEEL = 8.6; // cm const float WHEEL_TO_WHEEL = 8.6; // cm
@ -11,17 +11,22 @@ const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
// degrees to centimeters // degrees to centimeters
const float DEG_CM = (PI*WHEEL_DIAMETER) / 360.0; const float DEG_CM = (PI*WHEEL_DIAMETER) / 360.0;
//const float FORWARD_DISTANCE = 73.0; // cm const float FORWARD_DISTANCE = 100.0; // cm
const float FORWARD_DISTANCE = 50.0; // cm const float TURN_AMOUNT = 200.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;
const float KP = 1.40; // proportional const float KP = 1.50; // proportional
const float KI = 0.10; // integral const float KI = 0.12; // integral
const float KD = 0.00; // derivative const float KD = 0.00; // derivative
const float Kv = 1.8; // onboard velocity feedforward const float Kv = 1.85; // onboard velocity feedforward
const float KPP = 0.12; // position proportional
const float KPI = 0.06; // position integral const float TURN_KP = 1.50; // proportional
const float TURN_KI = 0.10; // integral
const float TURN_KD = 0.00; // derivative
const float TURN_Kv = 2.6; // onboard velocity feedforward
const float KPP = 0.14; // position proportional
const float KPI = 0.12; // position integral
const float KRP = 0.0; // rotation proportional const float KRP = 0.0; // rotation proportional
const float V_MIN = 5.5; // velocity minimum const float V_MIN = 5.5; // velocity minimum
const float V_KMIN = 0.7; // velocity minimum coefficient const float V_KMIN = 0.7; // velocity minimum coefficient

23
robot_controller/robot_controller.ino
View file

@ -96,6 +96,9 @@ void loop() {
float pos_err_right = ((setpoint.position / DEG_CM) + right_home) - right_motor.read_angle(); float pos_err_right = ((setpoint.position / DEG_CM) + right_home) - right_motor.read_angle();
float pos_err_left = ((turnsig * setpoint.position / DEG_CM) + left_home) - left_motor.read_angle(); float pos_err_left = ((turnsig * setpoint.position / DEG_CM) + left_home) - left_motor.read_angle();
// total wheel offness in degrees
float total_pos_error = abs(pos_err_left) + abs(pos_err_right);
// delta should be ~4ms // delta should be ~4ms
if (time > time_p) { if (time > time_p) {
float delta = time - time_p; float delta = time - time_p;
@ -109,8 +112,8 @@ void loop() {
#define sgn(x) ((x) < 0 ? -1 : ((x) > 0 ? 1 : 0)) #define sgn(x) ((x) < 0 ? -1 : ((x) > 0 ? 1 : 0))
//// boost on slowdowns // boost on when slowing down
//if (setpoint.velocity < V_MIN && setpoint.acceleration < 0.0) { //if (setpoint.velocity < V_MIN && setpoint.acceleration < 0.0 && total_pos_error > 4.0) {
// right_effort += (float )sgn(right_effort) * (setpoint.velocity - V_MIN) * V_KMIN; // right_effort += (float )sgn(right_effort) * (setpoint.velocity - V_MIN) * V_KMIN;
// left_effort += (float )sgn(left_effort) * (setpoint.velocity - V_MIN) * V_KMIN; // left_effort += (float )sgn(left_effort) * (setpoint.velocity - V_MIN) * V_KMIN;
//} //}
@ -140,6 +143,8 @@ void loop() {
Serial.print(left_iaccum); Serial.print(left_iaccum);
Serial.print(",ir:"); Serial.print(",ir:");
Serial.print(right_iaccum); Serial.print(right_iaccum);
Serial.print(",effr:");
Serial.print(right_effort);
//Serial.print("ff:"); //Serial.print("ff:");
//Serial.print(left_effort); //Serial.print(left_effort);
//Serial.print(",time:"); //Serial.print(",time:");
@ -160,8 +165,6 @@ void loop() {
//Serial.print(error / CMS_RPM); //Serial.print(error / CMS_RPM);
Serial.println(""); Serial.println("");
// 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 // move on if at setpoint TODO: give up after timeout
if (setpoint.complete && (total_pos_error < 3.0 || time - end_time > 7.0)) { if (setpoint.complete && (total_pos_error < 3.0 || time - end_time > 7.0)) {
// rehome // rehome
@ -172,11 +175,21 @@ void loop() {
switch (robot_state) { switch (robot_state) {
case FORWARD: case FORWARD:
// turn tuning
left_motor.tune_vel_pid(TURN_Kv, TURN_KP,TURN_KI,TURN_KD);
delay(1);
right_motor.tune_vel_pid(TURN_Kv,TURN_KP,TURN_KI,TURN_KD);
robot_state = TURN; robot_state = TURN;
//right_iaccum = left_iaccum = 8.0; right_iaccum = left_iaccum = 60.0;
phase_start = (float)millis() / 1000.0; phase_start = (float)millis() / 1000.0;
break; break;
case TURN: case TURN:
// straight line tuning
left_motor.tune_vel_pid(Kv, KP,KI,KD);
delay(1);
right_motor.tune_vel_pid(Kv, KP,KI,KD);
robot_state = RETURN; robot_state = RETURN;
phase_start = (float)millis() / 1000.0; phase_start = (float)millis() / 1000.0;