diff --git a/custom/motioncontrollers/CustomPIDController.java b/custom/motioncontrollers/CustomPIDController.java index 502e62f1..af00918a 100644 --- a/custom/motioncontrollers/CustomPIDController.java +++ b/custom/motioncontrollers/CustomPIDController.java @@ -24,6 +24,7 @@ public class CustomPIDController extends MotionController { protected double lastError; protected long lastTime; protected double minimumNominalOutput = 0.0; + protected double deadband = 0.0; /** * An extremely basic PID controller. @@ -160,7 +161,6 @@ public double getF() { } /** - * * @return * The current minimumNominalOutput (minimum nominal output) value */ @@ -168,6 +168,14 @@ public double getMinimumNominalOutput() { return minimumNominalOutput; } + /** + * @return + * The current deadband value + */ + public double getDeadband() { + return deadband; + } + /** * Sets the parameters of the PID loop * @@ -210,7 +218,6 @@ public void setPIDF(double P, double I, double D, double F) { } /** - * * @param minimumNominalOutput * Minimum Nominal Output * result will be set to @@ -220,9 +227,25 @@ public void setPIDF(double P, double I, double D, double F) { * the motor can only run well above a value. */ public void setMinimumNominalOutput(double minimumNominalOutput) { + if (deadband > 0) { + LogKitten.w("Removing deadband in favor of minimum nominal output"); + } + deadband = 0; this.minimumNominalOutput = minimumNominalOutput; } + /** + * @param deadband + * PID output will be set to 0 if it is lower than the deadband + */ + public void setDeadband(double deadband) { + if (minimumNominalOutput > 0) { + LogKitten.w("Removing minimum nominal output in favor of deadband"); + } + minimumNominalOutput = 0; + this.deadband = deadband; + } + /** * Sets the threshold below which the I term becomes active. * When the I term is active, the error sum increases. When @@ -329,6 +352,8 @@ public double getSafely() throws InvalidSensorException { } if (Math.abs(result) < minimumNominalOutput) { result = Math.signum(result) * minimumNominalOutput; + } else if (Math.abs(result) < deadband) { + result = 0; } return result; }