PID.java

package org.usfirst.frc.team236.robot;

import org.usfirst.frc.team236.robot.RobotMap.PIDConstants;

import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
/**
 * A PID controller that limits integral windup and accurately reports if the
 * setpoint has been reached.
 * @author Jared
 *
 */
public class PID implements Updatable {
	
	//variables that may be accessed from a separate thread should be volatile.
	public volatile double max_derror_dt, max_error, max_I, setPoint;
	public volatile boolean reset_I_whenOnTarget = false, enabled = false;
	private double i, lastError;
	private volatile double kP, kI, kD;
	//sensor value
	private PIDSource source;
	//output power
	private PIDOutput output;
	
	/**
	 * Construct a new PID controller that uses the given source and output for calculations.
	 * The PID controller will not run until enabled is set to true.
	 * @param source
	 * @param output
	 */
	public PID(PIDSource source, PIDOutput output) {
		this.source = source;
		this.output = output;
	}
    //synchronized to allow multiple threads to use this method without problems
	public synchronized void setGains(PIDConstants constants) {
		kP = constants.kP;
		kI = constants.kI;
		kD = constants.kD;
	}

	@Override
	public void update() {
		// PID calculations
		double dt = .02; // length of time between each calculation
		// calculate how far from our target we are
		double error = setPoint - source.pidGet();
		// p is proportional to error and our kP gain
		double p = error;
		// integrate error
		i += error * dt;
		// make sure it isn't too big
		coerce(i, -max_I, max_I);
		// differentiate error
		double dError_dt = (error - lastError) / dt;
		double d = dError_dt;
		//output is sum of all terms multiplied by gains
		double result = kP * p + kI * i + kD * d;

		// check to see if we're at our target
		if ((Math.abs(dError_dt) < max_derror_dt)
				&& (Math.abs(error) < max_error)) {
			// reset I if we want to
			//possibly add decay?
			i = (reset_I_whenOnTarget) ? 0 : i;
		}
		
		//set output
		if(enabled){
			output.pidWrite(result);
		}
		
		lastError = error;
		
		
	}

	/**
	 * Constrains a number between min and max. Like the LabVIEW
	 * "Coerce/In-range" VI.
	 * 
	 * @param a
	 * @param min
	 * @param max
	 * @return
	 */
	public static double coerce(double a, double min, double max) {
		return Math.min(max, Math.max(a, min));
	}
	@Override
	public boolean isOld() {
		
		return false;
	}

}