Fixed bug in pid controller

Team3555 · Feb 18, 2020 · c621826 · c621826
1 parent 982bcef
commit c621826
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 3 deletions.
diff --git a/src/org/aluminati3555/lib/data/AluminatiData.java b/src/org/aluminati3555/lib/data/AluminatiData.java
@@ -33,7 +33,7 @@
  */
 public final class AluminatiData {
     // Library
-    public static final int LIBRARY_VERSION = 14;
+    public static final int LIBRARY_VERSION = 15;
 
     // Robot delay
     public static double robotDelay = 0.02; // Seconds

diff --git a/src/org/aluminati3555/lib/pid/AluminatiPIDController.java b/src/org/aluminati3555/lib/pid/AluminatiPIDController.java
@@ -47,6 +47,7 @@ public String toString() {
 
     private double integral;
     private double lastError;
+    private double currentError;
     private double lastTimestamp;
 
     /**
@@ -68,6 +69,7 @@ protected void setPID(double kP, double kI, double kD) {
     public void reset(double timestamp) {
         integral = 0;
         lastError = 0;
+        currentError = Double.POSITIVE_INFINITY;
         lastTimestamp = timestamp;
     }
 
@@ -78,9 +80,9 @@ public void reset(double timestamp) {
      */
     public boolean isComplete() {
         if (allowableErrorEnabled) {
-            return (Math.abs(lastError) <= allowableError);
+            return (Math.abs(currentError) <= allowableError);
         } else {
-            return (lastError == 0);
+            return (currentError == 0);
         }
     }
 
@@ -107,6 +109,7 @@ public double update(double setpoint, double currentPoint, double timestamp) {
             double i = kI * integral;
             double d = kD * (error - lastError) / dt;
             lastError = error;
+            currentError = error;
 
             double output = p + i + d;
             if (output < -maxOutput) {