ank/ES3011
1
Fork 0
Code Releases Activity

further tuning (without cubes)

Browse source
This commit is contained in:
Andy Killorin 2025-12-12 13:29:50 -05:00
parent 1e070076f4
commit 1a388de3fe
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
2 changed files with 18 additions and 14 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

12
robot_controller/constants.h
View file

@ -1,7 +1,7 @@
#pragma once #pragma once
const float ACCEL_LIMIT = 7.0; // cm/s/s const float ACCEL_LIMIT = 7.0; // cm/s/s
const float VEL_LIMIT = 8.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
@ -12,14 +12,16 @@ const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
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 = 73.0; // cm
const float FORWARD_DISTANCE = 40.0; // cm const float FORWARD_DISTANCE = 50.0; // cm
const float TURN_AMOUNT = 146.0; // degrees const float TURN_AMOUNT = 210.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.40; // proportional
const float KI = 0.10; // integral const float KI = 0.10; // 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.8; // onboard velocity feedforward
const float KPP = 0.08; // position proportional const float KPP = 0.12; // position proportional
const float KPI = 0.04; // position integral const float KPI = 0.06; // 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_KMIN = 0.7; // velocity minimum coefficient

20
robot_controller/robot_controller.ino
View file

@ -49,9 +49,9 @@ void setup() {
Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
left_motor.tune_pos_pid(0.4, 0.024, 0.008); left_motor.tune_pos_pid(0.05, 0.014, 0.008);
delay(1); delay(1);
right_motor.tune_pos_pid(0.4, 0.024, 0.008); right_motor.tune_pos_pid(0.05, 0.014, 0.008);
delay(1); delay(1);
left_motor.tune_vel_pid(Kv, KP,KI,KD); left_motor.tune_vel_pid(Kv, KP,KI,KD);
delay(1); delay(1);
@ -107,11 +107,18 @@ void loop() {
float right_effort = pos_err_right * KPP + right_iaccum * KPI; float right_effort = pos_err_right * KPP + right_iaccum * KPI;
float left_effort = pos_err_left * KPP + left_iaccum * KPI; float left_effort = pos_err_left * KPP + left_iaccum * KPI;
#define sgn(x) ((x) < 0 ? -1 : ((x) > 0 ? 1 : 0))
//// boost on slowdowns
//if (setpoint.velocity < V_MIN && setpoint.acceleration < 0.0) {
// right_effort += (float )sgn(right_effort) * (setpoint.velocity - V_MIN) * V_KMIN;
// left_effort += (float )sgn(left_effort) * (setpoint.velocity - V_MIN) * V_KMIN;
//}
float right_velocity = setpoint.velocity + right_effort; float right_velocity = setpoint.velocity + right_effort;
float left_velocity = (turnsig * setpoint.velocity) + left_effort; float left_velocity = (turnsig * setpoint.velocity) + left_effort;
// arbitrary feedforward with on-board velocity PID // arbitrary feedforward with on-board velocity PID
if (setpoint.complete && 0) { if (setpoint.complete && robot_state == TURN) {
right_motor.write_angle((setpoint.position / DEG_CM) + right_home); right_motor.write_angle((setpoint.position / DEG_CM) + right_home);
left_motor.write_angle((turnsig * setpoint.position / DEG_CM) + left_home); left_motor.write_angle((turnsig * setpoint.position / DEG_CM) + left_home);
} else { } else {
@ -166,13 +173,8 @@ void loop() {
switch (robot_state) { switch (robot_state) {
case FORWARD: case FORWARD:
robot_state = TURN; robot_state = TURN;
right_iaccum = left_iaccum = 8.0; //right_iaccum = left_iaccum = 8.0;
phase_start = (float)millis() / 1000.0; phase_start = (float)millis() / 1000.0;
// end
left_motor.write_rpm(0.0);
delay(1);
right_motor.write_rpm(0.0);
for (;;) {};
break; break;
case TURN: case TURN:
robot_state = RETURN; robot_state = RETURN;