From 043bb622fba66aa4350f687dfb95ee55b8d2c27a Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Thu, 24 Oct 2024 13:40:57 -0400
Subject: [PATCH] added constant term

---
 lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h | 18 +++++++++++++++---
 1 file changed, 15 insertions(+), 3 deletions(-)

diff --git a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
index ac56a29..2a773dd 100644
--- a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
+++ b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
@@ -24,11 +24,11 @@ protected:
     enum CTRL_MODE : uint8_t {CTRL_DIRECT, CTRL_SPEED};
     volatile CTRL_MODE ctrlMode = CTRL_DIRECT;
 
-    // TODO: After you tune your motors, set the gains here.
-    float Kp = 2.50;
+    float Kp = 2.50; // proportional to error
     float Ki = 0.03;
     float Kd = 0;
-    float Kf = 3.00;
+    float Kf = 3.00; // proportional to setpoint
+    int16_t C = 22; // constant added to effort
 
     // Used to keep track of the target speed, in counts / interval.
     float targetSpeed = 0;
@@ -140,6 +140,18 @@ protected:
             // Calculate the effort from the PID gains
             int16_t effort = Kp * error + Ki * sumError + Kd * errorDiff + Kf * targetSpeed;
 
+            // increase effort to match static friction
+            if (effort > 0) {
+                effort += C;
+            } else if (effort < 0) {
+                effort -= C;
+            }
+
+            // stop motor if halted and stopped
+            if (abs(error) < 3. && targetSpeed == 0) {
+                effort = 0;
+            }
+
             // Set the effort for the motor
             SetEffort(effort);