add PIDController

diff --git a/src/org/usfirst/frc/team3501/robot/MathLib.java b/src/org/usfirst/frc/team3501/robot/MathLib.java
index a57b5ce7efa4aa3f8d02f040652dd450485f3d4a..e870d58756b2ee7e69112b4ca9ddaf01c5ac0fca 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/MathLib.java
+++ b/src/org/usfirst/frc/team3501/robot/MathLib.java
@@ -38,8 +38,8 @@ public class MathLib {
    */
   public static double getSpeedForConstantAccel(double minSpeed,
       double maxSpeed, double percentComplete) {
-    return maxSpeed + 2 * (minSpeed - maxSpeed)
-        * Math.abs(percentComplete - 0.5);
+    return maxSpeed
+        + 2 * (minSpeed - maxSpeed) * Math.abs(percentComplete - 0.5);
   }
 
   /***
@@ -65,7 +65,7 @@ public class MathLib {
 
   /***
    * Returns true if val is in the range [low, high]
-   * 
+   *
    * @param val
    *          value to test
    * @param low
@@ -77,4 +77,28 @@ public class MathLib {
   public static boolean inRange(double val, double low, double high) {
     return (val <= high) && (val >= low);
   }
+
+  public static double limitValue(double val) {
+    return limitValue(val, 1.0);
+  }
+
+  public static double limitValue(double val, double max) {
+    if (val > max) {
+      return max;
+    } else if (val < -max) {
+      return -max;
+    } else {
+      return val;
+    }
+  }
+
+  public static double limitValue(double val, double max, double min) {
+    if (val > max) {
+      return max;
+    } else if (val < min) {
+      return min;
+    } else {
+      return val;
+    }
+  }
 }

diff --git a/src/org/usfirst/frc/team3501/robot/utils/PIDController.java b/src/org/usfirst/frc/team3501/robot/utils/PIDController.java
new file mode 100644 (file)
index 0000000..b8ec465
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/utils/PIDController.java
@@ -0,0 +1,173 @@
+package org.usfirst.frc.team3501.robot.utils;
+
+import org.usfirst.frc.team3501.robot.MathLib;
+
+public class PIDController {
+
+  private double pConst;
+  private double iConst;
+  private double dConst;
+  private double setPoint;
+  private double previousError;
+  private double errorSum;
+  private double errorIncrement;
+  private double iRange;
+  private double doneRange;
+  private boolean firstCycle;
+  private double maxOutput;
+  private int minDoneCycleCount;
+  private int doneCycleCount;
+
+  public PIDController() {
+    this(0.0, 0.0, 0.0, 0.0, 0.0);
+  }
+
+  public PIDController(double p, double i, double d, double eps,
+      double iRange) {
+    this.pConst = p;
+    this.iConst = i;
+    this.dConst = d;
+    this.setPoint = 0.0;
+
+    this.iRange = iRange;
+    this.errorIncrement = 1.0;
+
+    this.doneRange = eps;
+    this.doneCycleCount = 0;
+    this.minDoneCycleCount = 5;
+
+    this.firstCycle = true;
+
+    this.maxOutput = 1.0;
+
+  }
+
+  public PIDController(double p, double i, double d, double eps) {
+    this(p, i, d, eps, eps * 0.8);
+  }
+
+  public PIDController(double p, double i, double d) {
+    this(p, i, d, 1.0);
+  }
+
+  public void setConstants(double p, double i, double d) {
+    this.pConst = p;
+    this.iConst = i;
+    this.dConst = d;
+  }
+
+  public void setConstants(double p, double i, double d, double eps,
+      double iRange) {
+    this.pConst = p;
+    this.iConst = i;
+    this.dConst = d;
+    this.doneRange = eps; // range of error
+    this.iRange = iRange;
+  }
+
+  public void setConstants(double p, double i, double d, double eps) {
+    setConstants(p, i, d, eps, eps * 0.8);
+  }
+
+  public void setDoneRange(double range) {
+    this.doneRange = range;
+  }
+
+  public void setIRange(double eps) {
+    this.iRange = eps;
+  }
+
+  public void setSetPoint(double val) {
+    this.setPoint = val;
+  }
+
+  public void setMaxOutput(double max) {
+    if (max < 0.0) {
+      this.maxOutput = 0.0;
+    } else if (max > 1.0) {
+      this.maxOutput = 1.0;
+    } else {
+      this.maxOutput = max;
+    }
+  }
+
+  public void setMinDoneCycles(int num) {
+    this.minDoneCycleCount = num;
+  }
+
+  public void resetErrorSum() {
+    this.errorSum = 0.0;
+  }
+
+  public double getDesiredVal() {
+    return this.setPoint;
+  }
+
+  public double calcPID(double current) {
+    return calcPIDError(this.setPoint - current);
+  }
+
+  public double calcPIDError(double error) {
+    double pVal = 0.0;
+    double iVal = 0.0;
+    double dVal = 0.0;
+
+    if (this.firstCycle) {
+      this.previousError = error;
+      this.firstCycle = false;
+    }
+
+    pVal = this.pConst * error;
+
+    // + error outside of acceptable range which might distort the sum calc
+    if (error > this.iRange) {
+      // check if error sum was in the wrong direction
+      if (this.errorSum < 0.0) {
+        this.errorSum = 0.0;
+      }
+      // only allow up to the max contribution per cycle
+      this.errorSum += Math.min(error, this.errorIncrement);
+    } // - error outside of acceptable range
+    else if (error < -1.0 * this.iRange) {
+      // error sum was in the wrong direction
+      if (this.errorSum > 0.0) {
+        this.errorSum = 0.0;
+      }
+      // add either the full error or the max allowable amount to sum
+      this.errorSum += Math.max(error, -1.0 * this.errorIncrement);
+    }
+
+    // i contribution (final) calculation
+    iVal = this.iConst * this.errorSum;
+
+    // /////D Calc///////
+    double deriv = error - this.previousError;
+    dVal = this.dConst * deriv;
+
+    // overal PID calc
+    double output = pVal + iVal + dVal;
+
+    // limit the output
+    output = MathLib.limitValue(output, this.maxOutput);
+
+    // store current value as previous for next cycle
+    this.previousError = error;
+    return output;
+  }
+
+  public boolean isDone() {
+    double currError = Math.abs(this.previousError);
+
+    // close enough to target
+    if (currError <= this.doneRange) {
+      this.doneCycleCount++;
+      System.out.println(doneCycleCount);
+    }
+    // not close enough to target
+    else {
+      this.doneCycleCount = 0;
+    }
+
+    return this.doneCycleCount >= this.minDoneCycleCount;
+  }
+}