From a1b4dfce793c759c63f774f051f0067da3d2910b Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Wed, 10 Dec 2025 12:00:13 -0500
Subject: [PATCH] position PI loop

---
 robot_controller/constants.h          |  4 +-
 robot_controller/robot_controller.ino | 62 +++++++++++++++++++--------
 2 files changed, 46 insertions(+), 20 deletions(-)

diff --git a/robot_controller/constants.h b/robot_controller/constants.h
index 4956d8d..886ec19 100644
--- a/robot_controller/constants.h
+++ b/robot_controller/constants.h
@@ -4,7 +4,7 @@ const float ACCEL_LIMIT = 5.0; // cm/s/s
 const float VEL_LIMIT = 10.0; // cm/s
 const float BACKLASH_RIGHT = 0.0; // degrees
 const float BACKLASH_LEFT = 0.0; // degrees
-const float WHEEL_DIAMETER = 10.00; // cm
+const float WHEEL_DIAMETER = 9.25; // cm
 const float WHEEL_TO_WHEEL = 10.0; // cm
 
 // centimeters per second to rpm
@@ -24,3 +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 KPI = 1.50; // position integral
diff --git a/robot_controller/robot_controller.ino b/robot_controller/robot_controller.ino
index e5251ad..ebdf947 100644
--- a/robot_controller/robot_controller.ino
+++ b/robot_controller/robot_controller.ino
@@ -19,7 +19,6 @@ enum ROBOT_STATE {
 float CURRENT_POSITION = 0.0;
 float CURRENT_ROTATION = 0.0;
 struct Setpoint setpoint; 
-float velocity = 40.0;
 // start of current state
 float phase_start;
 enum ROBOT_STATE robot_state;
@@ -27,6 +26,10 @@ enum ROBOT_STATE robot_state;
 struct Trapezoidal forward = {VEL_LIMIT, ACCEL_LIMIT, FORWARD_DISTANCE};
 struct Trapezoidal turn = {VEL_LIMIT, ACCEL_LIMIT, TURN_DISTANCE};
 
+float left_iaccum = 0.0;
+float right_iaccum = 0.0;
+float time_p = 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
@@ -43,15 +46,15 @@ void setup() {
 
     Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
     
-    // TUNE VELOCITY PID
-    //left_motor.tune_vel_pid(0.9, 3.7,0.3,0.0); 
-    //delay(10);
-    //right_motor.tune_vel_pid(0.9, 3.7,0.3,0.0); 
-    delay(10);
-    //right_motor.set_direction(PIDDirection::DIRECT);
+    left_motor.write_rpm(0.0);
+    delay(1);
+    right_motor.write_rpm(0.0);
+    delay(999);
     
     phase_start = (float)millis() / 1000.0;
     robot_state = FORWARD;
+    left_motor.home();
+    right_motor.home();
 }
 
 void loop() {
@@ -66,25 +69,39 @@ 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 feedforward_right = 
-    setpoint.acceleration * CMS_RPM * 9.0 + 
-    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
 
-  write_rpm_ff(&left_motor, setpoint.velocity * CMS_RPM, feedforward);
-  write_rpm_ff(&right_motor, (robot_state == TURN ? setpoint.velocity : -setpoint.velocity) * CMS_RPM, feedforward);
+  // 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();
 
-  // READ MOTOR POSITION
-  int32_t rpm = -right_motor.read_rpm();
-  int32_t pos = -right_motor.read_angle();
+  // delta should be ~4ms
+  if (time > time_p) {
+    float delta = time - time_p;
+    right_iaccum += pos_err_right * delta;
+    left_iaccum += pos_err_left * delta;
+  }
+  time_p = time;
+
+  float left_effort = pos_err_left * KPP + left_iaccum * KPI;
+  float right_effort = pos_err_right * KPP + right_iaccum * KPI;
+
+  // 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);
+
+  // send telemetry
+  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(feedforward_right);
+  Serial.print(left_effort);
   Serial.print(",time:");
   Serial.print(time);
   Serial.print(",distgoal:");
@@ -101,15 +118,22 @@ void loop() {
   Serial.print(error / CMS_RPM);
   Serial.println("");
 
-  // move on if at setpoint TODO: check that the error is low as well
-  if (setpoint.complete) {
+  // 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) {
     switch (robot_state) {
       case FORWARD:
         robot_state = TURN;
+        // TODO: don't forget about remaining err (add previous setpoint to global offset?)
+        left_motor.home();
+        right_motor.home();
         phase_start = (float)millis() / 1000.0;
         break;
       case TURN:
         robot_state = RETURN;
+        left_motor.home();
+        right_motor.home();
         phase_start = (float)millis() / 1000.0;
 
         break;