From d1ee6baccf6918f8498ce5566a84e4f4f6394da3 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Thu, 24 Oct 2024 13:03:12 -0400
Subject: [PATCH] integral windup limit

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

diff --git a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
index 9bdfdb3..afdb82a 100644
--- a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
+++ b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
@@ -125,6 +125,11 @@ protected:
 
             sumError += error; 
 
+            float maxSumError = maxEffort/Ki;
+
+            sumError = max(sumError, -maxSumError);
+            sumError = min(sumError, maxSumError);
+
             float errorDiff = error - prevError;
 
             prevError = error;