From 0788fd3df6f9297dd1d22d41acb4f6c23ac9e8c4 Mon Sep 17 00:00:00 2001 From: Cindy Zhang Date: Wed, 25 Jan 2017 20:50:01 -0800 Subject: [PATCH] add PIDController --- .../usfirst/frc/team3501/robot/MathLib.java | 30 ++- .../team3501/robot/utils/PIDController.java | 173 ++++++++++++++++++ 2 files changed, 200 insertions(+), 3 deletions(-) create mode 100644 src/org/usfirst/frc/team3501/robot/utils/PIDController.java diff --git a/src/org/usfirst/frc/team3501/robot/MathLib.java b/src/org/usfirst/frc/team3501/robot/MathLib.java index a57b5ce..e870d58 100644 --- 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 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; + } +} -- 2.30.2