pid.h

#ifndef PID_H_
#define PID_H_

#include <arduino--.h>

template<typename clock_, typename float_, float_ min_, float_ max_, void (*set_output_)(float_),
	float_ kp_, float_ ki_, float_ kd_,
	unsigned long interval_=100, bool proportionalOnError_=true>
class PID
{
public:

	static const float_ kp = kp_;
	static const float_ ki = ki_;
	static const float_ kd = kd_;

	PID(const float_ setpoint) : _setpoint(setpoint), _outputSum(0.0), _lastInput(0.0), _lastTime(clock_::millis()) {}

	bool compute(const float_ input)
	{
		unsigned long now = clock_::millis();
		unsigned long dTime = now - _lastTime;

		if (dTime >= interval_)
		{
			// Compute all the working error variables
			float_ error = _setpoint - input;
			float_ dInput = (input - _lastInput);
			_outputSum += (ki_ * error);

			// Add Proportional on Measurement, proportionalOnError_ is not set
			if (!proportionalOnError_) {
				_outputSum -= kp_ * dInput;
			}

			if (_outputSum > max_) {
				_outputSum = max_;
			}
			else if (_outputSum < min_) {
				_outputSum = min_;
			}

			// Add Proportional on Error, if proportionalOnError_ is specified
			float_ output = 0.0;
			if (proportionalOnError_) {
				output = kp_ * error;
			}

			// Compute Rest of PID Output
			output += _outputSum - kd_ * dInput;

			if (output > max_) {
				output = max_;
			}
			else if (output < min_) {
				output = min_;
			}

			// Remember some variables for next time
			_lastInput = input;
			_lastTime = now;

			set_output_(output);
			return true;
		}

		return false;
	}

private:

	float_ _kp;				  // * (P)roportional Tuning Parameter
	float_ _ki;				  // * (I)ntegral Tuning Parameter
	float_ _kd;				  // * (D)erivative Tuning Parameter

	float_ _setpoint;

	unsigned long _lastTime;
	float_ _outputSum;
	float_ _lastInput;
};

#endif // PID_H_