From 4ca507605a84af46180191c28c102ba9d2a11492 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Wed, 23 Oct 2024 11:49:39 -0400
Subject: [PATCH] naive pid impl

---
 lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
index 3a27781..c065b29 100644
--- a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
+++ b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
@@ -122,8 +122,14 @@ protected:
             // Calculate the error in speed
             float error = targetSpeed - speed;
 
+            sumError += error; 
+
+            float errorDiff = error - prevError;
+
+            prevError = error;
+
             // Calculate the effort from the PID gains
-            int16_t effort = Kp * error;
+            int16_t effort = Kp * error + Ki * sumError + Kd * errorDiff;
 
             // Set the effort for the motor
             SetEffort(effort);