From 60ce3219067ff961a54d12a6871342d37e09374b Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Wed, 10 Dec 2025 13:22:34 -0500
Subject: [PATCH] continue until timeout

---
 robot_controller/constants.h          |  8 ++++----
 robot_controller/robot_controller.ino | 11 +++++++----
 2 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/robot_controller/constants.h b/robot_controller/constants.h
index 1b01356..381a8e1 100644
--- a/robot_controller/constants.h
+++ b/robot_controller/constants.h
@@ -18,11 +18,11 @@ const float TURN_AMOUNT = 180.0; // degrees
 const float TURN_DISTANCE = (TURN_AMOUNT / 360.0) * WHEEL_TO_WHEEL * PI;
 const float RETURN_DISTANCE = 100.0; // cm
 
-const float FF_ACCEL = 0.7; // motor acceleration feedforward
-const float FF_VEL = 0.6; // motor velocity feedforward
-const float FF_STAT = 15.4; // motor static friction
+const float FF_ACCEL = 2.7; // motor acceleration feedforward
+const float FF_VEL = 0.7; // motor velocity feedforward
+const float FF_STAT = 16.4; // motor static friction
 const float KP = 2.0; // proportional
 const float KI = 0.5; // integral
 const float KD = 0.05; // derivative
-const float KPP = 0.15; // position proportional
+const float KPP = 0.25; // position proportional
 const float KPI = 0.05; // position integral
diff --git a/robot_controller/robot_controller.ino b/robot_controller/robot_controller.ino
index 19e4061..28a2a52 100644
--- a/robot_controller/robot_controller.ino
+++ b/robot_controller/robot_controller.ino
@@ -62,13 +62,16 @@ void setup() {
 
 void loop() {
   float time = (float)millis() / 1000.0 - phase_start;
+  float end_time;
   switch (robot_state) {
     case FORWARD:
     case RETURN:
       setpoint = trapezoidal_planner(&forward, time);
+      end_time = trapezoidal_time(&forward);
       break;
     case TURN:
       setpoint = trapezoidal_planner(&turn, time);
+      end_time = trapezoidal_time(&turn);
       break;
   }
 
@@ -107,11 +110,11 @@ void loop() {
   write_rpm_ff(&right_motor, right_velocity * CMS_RPM  , feedforward_right);
 
   // send telemetry
-  int32_t rpm = -right_motor.read_rpm();
-  int32_t pos = -right_motor.read_angle();
+  int32_t rpm = left_motor.read_rpm();
+  int32_t pos = left_motor.read_angle();
   int32_t error = setpoint.velocity * CMS_RPM - rpm;
   Serial.print("ff:");
-  Serial.print(right_effort);
+  Serial.print(left_effort);
   Serial.print(",time:");
   Serial.print(time);
   Serial.print(",distgoal:");
@@ -131,7 +134,7 @@ void loop() {
   // 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
-  if (setpoint.complete && total_pos_error < 10.0) {
+  if (setpoint.complete && (total_pos_error < 3.0 || time - end_time > 3.0)) {
     // rehome
     left_home += (setpoint.position / DEG_CM);
     right_home += (turnsig * setpoint.position / DEG_CM);