ank/ES3011
1
Fork 0
Code Releases Activity

dead code removal

Browse source
This commit is contained in:
Andy Killorin 2025-12-12 17:01:45 -05:00
parent fa00d397de
commit 5ecc8066a2
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
1 changed files with 7 additions and 26 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

33
robot_controller/robot_controller.ino
View file

@ -16,8 +16,6 @@ enum ROBOT_STATE {
COMPLETE,
};
float CURRENT_POSITION = 0.0;
float CURRENT_ROTATION = 0.0;
struct Setpoint setpoint;
// start of current state
float phase_start;
@ -33,15 +31,6 @@ 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) {
if (rpm == 0) {rpm = 1;};
float kV = ff / (float) rpm; // calculate velocity feedforward that causes desired absolute feedforward
motor->tune_vel_pid(kV, KP,KI,KD);
delay(1);
motor->write_rpm(rpm);
delay(1);
}
void setup() {
// INIT SERIAL
Serial.begin(115200);
@ -58,16 +47,16 @@ void setup() {
right_motor.tune_vel_pid(Kv, KP,KI,KD);
delay(1);
delay(10);
delay(10);
// runs to mend bad state
left_motor.home();
left_motor.home();
delay(1);
left_motor.write_angle(0.0);
delay(1);
right_motor.home();
right_motor.home();
delay(1);
right_motor.write_angle(0.0);
delay(3000);
delay(3000);
phase_start = (float)millis() / 1000.0;
robot_state = FORWARD;
@ -110,17 +99,9 @@ void loop() {
float right_effort = pos_err_right * KPP + right_iaccum * KPI;
float left_effort = pos_err_left * KPP + left_iaccum * KPI;
#define sgn(x) ((x) < 0 ? -1 : ((x) > 0 ? 1 : 0))
// boost on when slowing down
//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;
// left_effort += (float )sgn(left_effort) * (setpoint.velocity - V_MIN) * V_KMIN;
//}
float right_velocity = setpoint.velocity + right_effort;
float left_velocity = (turnsig * setpoint.velocity) + left_effort;
// arbitrary feedforward with on-board velocity PID
if (setpoint.complete && robot_state == TURN) {
right_motor.write_angle((setpoint.position / DEG_CM) + right_home);
left_motor.write_angle((turnsig * setpoint.position / DEG_CM) + left_home);
@ -165,7 +146,7 @@ void loop() {
//Serial.print(error / CMS_RPM);
Serial.println("");
// move on if at setpoint TODO: give up after timeout
// move on if at setpoint or timeout
if (setpoint.complete && (total_pos_error < 3.0 || time - end_time > 5.0)) {
// rehome
right_home += (setpoint.position / DEG_CM);
@ -175,7 +156,7 @@ void loop() {
switch (robot_state) {
case FORWARD:
// turn tuning
// turn tuning (more aggressive)
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);
@ -185,7 +166,7 @@ void loop() {
phase_start = (float)millis() / 1000.0;
break;
case TURN:
// straight line tuning
// straight line tuning (less aggressive)
left_motor.tune_vel_pid(Kv, KP,KI,KD);
delay(1);
right_motor.tune_vel_pid(Kv, KP,KI,KD);