BangBangController.java

package org.usfirst.frc4904.standard.custom.motioncontrollers;


import org.usfirst.frc4904.standard.LogKitten;
import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException;
import org.usfirst.frc4904.standard.custom.sensors.PIDSensor;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * A bang bang controller.
 * The bang bang controller increases the value of the output
 * if it is below the setpoint or decreases the value of the
 * output if it is above the setpoint.
 *
 */
public class BangBangController extends MotionController {
	protected double error;
	protected double A;
	protected double F;
	protected double threshold;
	protected static final String DEFAULT_SMARTDASHBOARD_PREFIX = "!!";

	/**
	 * BangBang controller
	 * A bang bang controller.
	 * The bang bang controller increases the value of the output
	 * if it is below the setpoint or decreases the value of the
	 * output if it is above the setpoint.
	 *
	 * @param sensor
	 *        Sensor
	 * @param A
	 *        Adjustment term
	 *        The is the amount the setpoint is increase
	 *        or decrease by.
	 * @param F
	 *        Feedforward term
	 *        The scalar on the input.
	 * @param threshold
	 *        The threshold for the bangbang to start doing something.
	 */
	public BangBangController(PIDSensor sensor, double A, double F, double threshold) {
		super(sensor);
		this.A = A;
		this.F = F;
		this.threshold = threshold;
		reset();
	}

	/**
	 * BangBang controller
	 * A bang bang controller.
	 * The bang bang controller increases the value of the output
	 * if it is below the setpoint or decreases the value of the
	 * output if it is above the setpoint.
	 *
	 * @param source
	 *        Sensor
	 * @param A
	 *        Adjustment term
	 *        The is the amount the setpoint is increase
	 *        or decrease by.
	 * @param F
	 *        Feedforward term
	 *        The scalar on the input.
	 * @param threshold
	 *        The threshold for the bangbang to start doing something.
	 */
	public BangBangController(PIDSource source, double A, double F, double threshold) {
		super(source);
		this.A = A;
		this.F = F;
		this.threshold = threshold;
		reset();
	}

	/**
	 * BangBang controller
	 * A bang bang controller.
	 * The bang bang controller increases the value of the output
	 * if it is below the setpoint or decreases the value of the
	 * output if it is above the setpoint.
	 *
	 * @param sensor
	 *        Sensor
	 * @param A
	 *        Adjustment term
	 *        The is the amount the setpoint is increase
	 *        or decrease by.
	 * @param F
	 *        Feedforward term
	 *        The scalar on the input.
	 */
	public BangBangController(PIDSensor sensor, double A, double F) {
		this(sensor, A, F, Double.MIN_VALUE);
	}

	/**
	 * BangBang controller
	 * A bang bang controller.
	 * The bang bang controller increases the value of the output
	 * if it is below the setpoint or decreases the value of the
	 * output if it is above the setpoint.
	 *
	 * @param source
	 *        Sensor
	 * @param A
	 *        Adjustment term
	 *        The is the amount the setpoint is increase
	 *        or decrease by.
	 * @param F
	 *        Feedforward term
	 *        The scalar on the input.
	 */
	public BangBangController(PIDSource source, double A, double F) {
		this(source, A, F, Double.MIN_VALUE);
	}

	/**
	 * @return
	 * 		The current A value
	 */
	public double getA() {
		return A;
	}

	/**
	 * @return
	 * 		The current F (feed forward) value
	 */
	public double getF() {
		return F;
	}

	/**
	 * @return
	 * 		The current D value
	 */
	public double getThreshold() {
		return threshold;
	}

	/**
	 * Set the value of the A constant
	 * 
	 * @param the
	 *        desired A value
	 */
	public void setA(double A) {
		this.A = A;
	}

	/**
	 * Set the value of the F (feed forward) constant
	 * 
	 * @param the
	 *        desired F value
	 */
	public void setF(double F) {
		this.F = F;
	}

	/**
	 * Set the value of the bang-bang threshold
	 * 
	 * @param the
	 *        desired threshold value
	 */
	public void setThreshold(double threshold) {
		this.threshold = threshold;
	}

	/**
	 * Sets the stored error value to zero (0)
	 */
	@Override
	public void resetErrorToZero() {
		error = 0;
	}

	/**
	 * Get the current output of the bang bang controller.
	 * This should be used to set the output.
	 *
	 * @return
	 * 		The current output of the bang bang controller.
	 * @throws InvalidSensorException
	 */
	@Override
	public double getSafely() throws InvalidSensorException {
		if (!isEnabled()) {
			return F * setpoint;
		}
		double input = 0.0;
		input = sensor.pidGetSafely();
		error = setpoint - input;
		LogKitten.v(input + " " + setpoint + " " + error);
		if (continuous) {
			double range = inputMax - inputMin;
			// If the error is more than half of the range, it is faster to increase the error and loop around the boundary
			if (Math.abs(error) > range / 2) {
				if (error > 0) {
					error -= range;
				} else {
					error += range;
				}
			}
		}
		if (error < 0.0 && Math.abs(error) > threshold) {
			LogKitten.v("+A " + error);
			return A + F * setpoint;
		} else if (error > 0.0 && Math.abs(error) > threshold) {
			LogKitten.v("-A" + error);
			return -1.0 * A + F * setpoint;
		}
		return F * setpoint;
	}

	/**
	 * Get the current output of the bang bang controller.
	 * This should be used to set the output.
	 *
	 * @return
	 * 		The current output of the bang bang controller.
	 * @warning does not indicate sensor error
	 */
	@Override
	public double get() {
		try {
			return getSafely();
		}
		catch (Exception e) {
			LogKitten.ex(e);
			return 0;
		}
	}

	/**
	 * @return
	 * 		The most recent error.
	 */
	@Override
	public double getError() {
		return error;
	}

	/**
	 * Put the PID controller to SmartDashboard for tuning.
	 * Puts the error, setpoint, sensor value, and output.
	 * This method adds a very small random value to everything
	 * so that graphs show properly on SmartDashboard.
	 * 
	 * @param prefix
	 *        The prefix to use when putting things on SmartDashboard.
	 */
	@Override
	public void putToSmartDashboard(String prefix) {
		double noise = (Math.random() - 0.5) * 0.0000001; // Generate very small noise centered at zero
		SmartDashboard.putNumber(prefix + "_Error", getError() + noise);
		SmartDashboard.putNumber(prefix + "_Setpoint", getSetpoint() + noise);
		SmartDashboard.putNumber(prefix + "_Sensor", getSensorValue() + noise);
		SmartDashboard.putNumber(prefix + "_Output", get() + noise);
	}

	/**
	 * Put the PID controller to SmartDashboard for tuning.
	 * Puts the error, setpoint, sensor value, and output.
	 * This method adds a very small random value to everything
	 * so that graphs show properly on SmartDashboard.
	 */
	@Override
	public void putToSmartDashboard() {
		putToSmartDashboard(BangBangController.DEFAULT_SMARTDASHBOARD_PREFIX);
	}

	/**
	 * Update the PID controller from SmartDashboard for tuning.
	 * Gets the P, I, D, and F constants from SmartDashboard,
	 * or puts the current ones there if there's no constants.
	 * 
	 * @param prefix
	 *        The prefix to use when putting things on SmartDashboard.
	 */
	@Override
	public void updateFromSmartDashboard(String prefix) {
		// If we're missing any constants on SmartDashboard, put the current ones
		if (!(SmartDashboard.containsKey(prefix + "_A") && SmartDashboard.containsKey(prefix + "_F")
			&& SmartDashboard.containsKey(prefix + "_threshold"))) {
			SmartDashboard.putNumber(prefix + "_A", getA());
			SmartDashboard.putNumber(prefix + "_F", getF());
			SmartDashboard.putNumber(prefix + "_threshold", getThreshold());
			return;
		}
		setA(SmartDashboard.getNumber(prefix + "_A", getA()));
		setF(SmartDashboard.getNumber(prefix + "_F", getF()));
		setThreshold(SmartDashboard.getNumber(prefix + "_threshold", getThreshold()));
	}

	/**
	 * Update the PID controller from SmartDashboard for tuning.
	 * Gets the P, I, D, and F constants from SmartDashboard,
	 * or puts the current ones there if there's no constants.
	 */
	@Override
	public void updateFromSmartDashboard() {
		updateFromSmartDashboard(BangBangController.DEFAULT_SMARTDASHBOARD_PREFIX);
	}
}