PID control

gistfile1.java

package com.bitgrind.control;

public class PID {
	
	/**
	 *  Low pass filter cutoff frequency for the derivative term.
	 */
	private static final int LOWPASS_CUTOFF_HZ = 20;
	private static final double RC = 1 / (2 * Math.PI * LOWPASS_CUTOFF_HZ);

	private double pgain;
	private double igain;
	private double dgain;

	private long ilimit;
	private double integrator;
	private double derivative;

	private int lastError;
	private double lastDerivative;
	
	public PID() {
		this(1, 0, 0, 0);
	}

	public PID(double pgain, double igain, double dgain, double ilimit) {
		this.pgain = pgain;
		this.igain = igain;
		this.dgain = dgain;
		this.ilimit = (long) Math.abs(ilimit);
	}

	public void reset() {
		integrator = 0;
		lastError = 0;
		lastDerivative = 0;
	}
	
	private int p(int error) {
		return (int) (error * pgain);
	}

	private int i(int error, double timeDelta) {
		if (igain != 0 && timeDelta != 0) {
			integrator += ((error * igain) * timeDelta);
			if (integrator < -ilimit) {
				integrator = -ilimit;
			} else if (integrator > ilimit) {
				integrator = ilimit;
			}
			return (int) integrator;
		}
		return 0;
	}

	private int d(int error, double timeDelta) {
		if (dgain != 0 && timeDelta != 0) {
			derivative = (error - lastError) / timeDelta;

			// low-pass filter
			derivative = lastDerivative + (timeDelta / (timeDelta + RC)) * 
					(derivative - lastDerivative);

			lastError = error;
			lastDerivative = derivative;
			return (int) (dgain * derivative);
		}
		return 0;
	}

	public int pi(int error, double timeDelta) {
		return p(error) + i(error, timeDelta);
	}

	public int pid(int error, double timeDelta) {
		return p(error) + i(error, timeDelta) + d(error, timeDelta);
	}
}