Pid deadband

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,14 +161,21 @@ public double getF() {
 	}
 
 	/**
-	 *
 	 * @return
 	 * 		The current minimumNominalOutput (minimum nominal output) value
 	 */
 	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;
 	}