File content as of revision 60:20945383ad1b:

// sources used: http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/

#include "PID.h"
#include "mbed.h"

// PID is called with interrupt caller, so no need to keep track of time since dT is constant
//Timer t;  // create Timer object t
//t.start(); // start the timer 


PID::PID(float* Input, float* Output, float* Setpoint, float Kp, float Ki, float Kd){
	myOutput = Output;
	myInput = Input;
	mySetpoint = Setpoint;
	inAuto = false;

	PID::SetOutputLimits(-1.0,1.0); // default output limits for FRDM board

	SampleTime = 0.1; 

	PID::SetTunings(Kp, Ki, Kd);
//	prevTime = t.read();
}

bool PID::Compute(){
	if(!inAuto) return false;

//	curTime = t.read()  // read time in seconds (no need since called by interrupt)
//	timeChange = curTime - prevTime;

	// calculate using sample time frequency (regular intervals)
	// no more need to modify derivative/int sicne sample time is constant
//	if(timeChange <=SampleTime){ 
		float input = *myInput;
		error = *mySetpoint - input;  // get error for proportional
		ITerm += (ki*error);  // Error for integral (with ki included)
		// check for integrator windup
		if(ITerm> outMax) ITerm=outMax;
		else if(ITerm<outMin) ITerm=outMin;
		dErr = (error - lastErr); // get error for derivative
		// (could still be tuned for derivative kick)

		// compute PID output
		float output  = kp * error + ITerm + kd * dErr;
		// make sure output is allso within min/max
		if(output> outMax) output=outMax;
		else if(output<outMin) output=outMin;
		*myOutput = output;
		lastErr = error;
//		prevTime = curTime;
		return true;
//	}
//	else return false;
}

void PID::SetTunings(float Kp, float Ki, float Kd)
{
	kp = Kp;
	ki = Ki * SampleTime; // change PID parameters according to sample time
	kd = Kd / SampleTime;
}


void PID::SetSampleTime(float NewSampleTime){
	if (NewSampleTime > 0 ){
		// change ratios for parameters for better computation in compute()
		float ratio = NewSampleTime/SampleTime;
		ki *= ratio;
		kd /= ratio;
		SampleTime = NewSampleTime;
	}
}

void PID::SetOutputLimits(float min, float max){
	if (min > max) return;
	outMin = min;
	outMax = max;

	if(inAuto){
		if(*myOutput > outMax) *myOutput = outMax;
		else if(*myOutput < outMin) *myOutput = outMin;
	
		if(ITerm > outMax) ITerm= outMax;
		else if(ITerm < outMin) ITerm= outMin;
	}
}

void PID::SetMode(int Mode){
	// Turn PID on(AUTOMATIC) or off(MANUAL)
	bool newAuto = (Mode == AUTOMATIC);
	if(newAuto && !inAuto){
		PID::Initialize(); // avoid bump because of stored values
	}
	inAuto = newAuto;
}

void PID::Initialize(){
	lastInput = *myInput;
	ITerm = *myOutput;
	lastErr = 0;
	if(ITerm> outMax) ITerm=outMax;
	else if(ITerm<outMin) ITerm=outMin;
}

float PID::getKp(){
	return kp;
}
	
float PID::getKi(){
	return ki/SampleTime;
}

float PID::getKd(){
	return kd*SampleTime;
}