From 697487a0dd912b3aae1ab18c3af725bcac878654 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Fri, 12 Dec 2025 14:37:52 -0500
Subject: [PATCH] works quite well

---
 robot_controller/constants.h          | 23 ++++++++++++++---------
 robot_controller/robot_controller.ino | 23 ++++++++++++++++++-----
 2 files changed, 32 insertions(+), 14 deletions(-)

diff --git a/robot_controller/constants.h b/robot_controller/constants.h
index 03548ff..042f664 100644
--- a/robot_controller/constants.h
+++ b/robot_controller/constants.h
@@ -1,6 +1,6 @@
 #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 WHEEL_DIAMETER = 6.61; // 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
 const float DEG_CM = (PI*WHEEL_DIAMETER) / 360.0;
 
-//const float FORWARD_DISTANCE = 73.0; // cm
-const float FORWARD_DISTANCE = 50.0; // cm
-const float TURN_AMOUNT = 180.0; // degrees
+const float FORWARD_DISTANCE = 100.0; // cm
+const float TURN_AMOUNT = 200.0; // degrees
 const float TURN_DISTANCE = (TURN_AMOUNT / 360.0) * WHEEL_TO_WHEEL * PI;
 
-const float KP = 1.40; // proportional
-const float KI = 0.10; // integral
+const float KP = 1.50; // proportional
+const float KI = 0.12; // integral
 const float KD = 0.00; // derivative
-const float Kv = 1.8; // onboard velocity feedforward
-const float KPP = 0.12; // position proportional
-const float KPI = 0.06; // position integral
+const float Kv = 1.85; // onboard velocity feedforward
+
+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 V_MIN = 5.5; // velocity minimum
 const float V_KMIN = 0.7; // velocity minimum coefficient
diff --git a/robot_controller/robot_controller.ino b/robot_controller/robot_controller.ino
index f38ba3d..446fa25 100644
--- a/robot_controller/robot_controller.ino
+++ b/robot_controller/robot_controller.ino
@@ -96,6 +96,9 @@ void loop() {
   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();
 
+  // total wheel offness in degrees
+  float total_pos_error = abs(pos_err_left) + abs(pos_err_right);
+
   // delta should be ~4ms
   if (time > time_p) {
     float delta = time - time_p;
@@ -109,8 +112,8 @@ void loop() {
 
   
 #define sgn(x) ((x) < 0 ? -1 : ((x) > 0 ? 1 : 0))
-  //// boost on slowdowns
-  //if (setpoint.velocity < V_MIN && setpoint.acceleration < 0.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;
   //}
@@ -140,6 +143,8 @@ void loop() {
   Serial.print(left_iaccum);
   Serial.print(",ir:");
   Serial.print(right_iaccum);
+  Serial.print(",effr:");
+  Serial.print(right_effort);
   //Serial.print("ff:");
   //Serial.print(left_effort);
   //Serial.print(",time:");
@@ -160,8 +165,6 @@ void loop() {
   //Serial.print(error / CMS_RPM);
   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
   if (setpoint.complete && (total_pos_error < 3.0 || time - end_time > 7.0)) {
     // rehome
@@ -172,11 +175,21 @@ void loop() {
 
     switch (robot_state) {
       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;
-				//right_iaccum = left_iaccum = 8.0;
+				right_iaccum = left_iaccum = 60.0;
         phase_start = (float)millis() / 1000.0;
         break;
       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;
         phase_start = (float)millis() / 1000.0;