Diff: PID/PID.cpp

Revision:

51:1e6334b993a3

Child:

54:c14c3bc48b8a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID/PID.cpp	Fri Oct 09 13:32:29 2015 +0200
@@ -0,0 +1,99 @@
+// sources used: http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
+
+#include "PID.h"
+#include "mbed.h"
+
+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
+	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 = Setpoint - input;  // get error for proportional
+		errSum += error;  // Error for integral
+		// check for integrator windup
+		if(errSum> outMax) errSum=outMax;
+		else if(errSum<outMin) errSum=outMin;
+		dErr = (error - lastErr); // get error for derivative
+		// (could still be tuned for derivative kick)
+
+		// compute PID output
+		float output  = Kp * error + Ki * errSum + 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::SetTunigns(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(Output > outMax) Output = outMax;
+	else if(Output < outMin) Output = 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){
+		Initialize(); // avoid bump because of stored values
+	}
+	inAuto = newAuto;
+}
+
+void PID::Initialize(){
+	lastInput = Input;
+	errSum = Output;
+	if(errSum> outMax) errSum=outMax;
+	else if(errSum<outMin) errSum=outMin;
+}
\ No newline at end of file