From fb475f2ecf01bce0c3a5b08f32543fe3a4e07652 Mon Sep 17 00:00:00 2001
From: kellharpole <kgh926@eid.utexas.edu>
Date: Sun, 16 Nov 2025 12:11:03 -0600
Subject: [PATCH] integral shi

---
 DualYokeControl (Previous Version).ino | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/DualYokeControl (Previous Version).ino b/DualYokeControl (Previous Version).ino
index 29a879a..2807030 100644
--- a/DualYokeControl (Previous Version).ino	
+++ b/DualYokeControl (Previous Version).ino	
@@ -130,6 +130,11 @@ void loop() {
 
     ecurr2 = ref2 - dist2;
     derror2 = ecurr2 - eprior2;
+    
+    ecum += ecurr * (telapsed / 1e6);
+    ecum = constrain(ecum, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM);
+    ecum2 += ecurr2 * (telapsed / 1e6);
+    ecum2 = constrain(ecum2, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM);
 
     
     if (ON) {