From 0b0a656bf31569c4cb9154af0961894c49c6e195 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Wed, 10 Dec 2025 12:09:07 -0500
Subject: [PATCH] basic position PI tuning

---
 robot_controller/constants.h          |  2 +-
 robot_controller/robot_controller.ino | 30 +++++++++++++++++----------
 2 files changed, 20 insertions(+), 12 deletions(-)

diff --git a/robot_controller/constants.h b/robot_controller/constants.h
index 886ec19..2335cfc 100644
--- a/robot_controller/constants.h
+++ b/robot_controller/constants.h
@@ -24,5 +24,5 @@ const float FF_STAT = 15.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 = 3.55; // position proportional
+const float KPP = 0.55; // position proportional
 const float KPI = 1.50; // position integral
diff --git a/robot_controller/robot_controller.ino b/robot_controller/robot_controller.ino
index ebdf947..4ef0028 100644
--- a/robot_controller/robot_controller.ino
+++ b/robot_controller/robot_controller.ino
@@ -69,17 +69,12 @@ void loop() {
       break;
   }
 
-  // calculate feedforward from motion profile
-  float feedforward = 
-    setpoint.acceleration * CMS_RPM * FF_ACCEL + 
-    setpoint.velocity * CMS_RPM * FF_VEL + 
-    ((setpoint.velocity > 0.0) ? FF_STAT : -FF_STAT);
 
   float turnsig = (robot_state == TURN ? 1.0 : -1.0); // direction of travel for right motor
 
   // position PI controller
   float pos_err_left = (setpoint.position / DEG_CM) - left_motor.read_angle();
-  float pos_err_right = (turnsig * setpoint.position / DEG_CM) - left_motor.read_angle();
+  float pos_err_right = (turnsig * setpoint.position / DEG_CM) - right_motor.read_angle();
 
   // delta should be ~4ms
   if (time > time_p) {
@@ -92,16 +87,29 @@ void loop() {
   float left_effort = pos_err_left * KPP + left_iaccum * KPI;
   float right_effort = pos_err_right * KPP + right_iaccum * KPI;
 
+  float left_velocity = setpoint.velocity + left_effort;
+  float right_velocity = (turnsig * setpoint.velocity) + right_effort;
+  
+  // calculate feedforward from motion profile and position PI data
+  float feedforward_left = 
+    setpoint.acceleration * CMS_RPM * FF_ACCEL + 
+    left_velocity * CMS_RPM * FF_VEL + 
+    ((left_velocity > 0.0) ? FF_STAT : -FF_STAT);
+  float feedforward_right = 
+    setpoint.acceleration * CMS_RPM * FF_ACCEL + 
+    right_velocity * CMS_RPM * FF_VEL + 
+    ((right_velocity > 0.0) ? FF_STAT : -FF_STAT);
+
   // arbitrary feedforward with on-board velocity PID
-  write_rpm_ff(&left_motor, setpoint.velocity * CMS_RPM + left_effort, feedforward);
-  //write_rpm_ff(&right_motor, turnsig * setpoint.velocity * CMS_RPM  , feedforward + right_effort);
+  write_rpm_ff(&left_motor, left_velocity * CMS_RPM , feedforward_left);
+  write_rpm_ff(&right_motor, right_velocity * CMS_RPM  , feedforward_right);
 
   // send telemetry
-  int32_t rpm = left_motor.read_rpm();
-  int32_t pos = left_motor.read_angle();
+  int32_t rpm = -right_motor.read_rpm();
+  int32_t pos = -right_motor.read_angle();
   int32_t error = setpoint.velocity * CMS_RPM - rpm;
   Serial.print("ff:");
-  Serial.print(left_effort);
+  Serial.print(right_effort);
   Serial.print(",time:");
   Serial.print(time);
   Serial.print(",distgoal:");