Dammenisvoorcasuals / Mbed 2 deprecated chessRobot

control for robotic arm that can play chess using a granular gripper

Dependencies:   Encoder mbed HIDScope Servo MODSERIAL

Fork of chessRobot by a steenbeek

PID/PID.cpp@51:1e6334b993a3, 2015-10-09 (annotated)

Committer:
annesteenbeek
Date:
Fri Oct 09 13:32:29 2015 +0200
Revision:
51:1e6334b993a3
Child:
54:c14c3bc48b8a
wrote own PID library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
annesteenbeek 51:1e6334b993a3 1 // sources used: http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
annesteenbeek 51:1e6334b993a3 2
annesteenbeek 51:1e6334b993a3 3 #include "PID.h"
annesteenbeek 51:1e6334b993a3 4 #include "mbed.h"
annesteenbeek 51:1e6334b993a3 5
annesteenbeek 51:1e6334b993a3 6 Timer t; // create Timer object t
annesteenbeek 51:1e6334b993a3 7 t.start(); // start the timer
annesteenbeek 51:1e6334b993a3 8
annesteenbeek 51:1e6334b993a3 9
annesteenbeek 51:1e6334b993a3 10 PID::PID(float* Input, float* Output, float* Setpoint, float Kp, float Ki, float Kd){
annesteenbeek 51:1e6334b993a3 11 myOutput = Output;
annesteenbeek 51:1e6334b993a3 12 myInput = Input;
annesteenbeek 51:1e6334b993a3 13 mySetpoint = Setpoint;
annesteenbeek 51:1e6334b993a3 14 inAuto = false;
annesteenbeek 51:1e6334b993a3 15
annesteenbeek 51:1e6334b993a3 16 PID::SetOutputLimits(-1.0,1.0); // default output limits for FRDM board
annesteenbeek 51:1e6334b993a3 17
annesteenbeek 51:1e6334b993a3 18 SampleTime = 0.1;
annesteenbeek 51:1e6334b993a3 19
annesteenbeek 51:1e6334b993a3 20 PID::SetTunings(Kp, Ki, Kd);
annesteenbeek 51:1e6334b993a3 21 prevTime = t.read();
annesteenbeek 51:1e6334b993a3 22 }
annesteenbeek 51:1e6334b993a3 23
annesteenbeek 51:1e6334b993a3 24 bool PID::compute(){
annesteenbeek 51:1e6334b993a3 25 if(!inAuto) return false;
annesteenbeek 51:1e6334b993a3 26
annesteenbeek 51:1e6334b993a3 27 curTime = t.read() // read time in seconds
annesteenbeek 51:1e6334b993a3 28 timeChange = curTime - prevTime;
annesteenbeek 51:1e6334b993a3 29
annesteenbeek 51:1e6334b993a3 30 // calculate using sample time frequency (regular intervals)
annesteenbeek 51:1e6334b993a3 31 // no more need to modify derivative/int sicne sample time is constant
annesteenbeek 51:1e6334b993a3 32 if(timeChange <=SampleTime){
annesteenbeek 51:1e6334b993a3 33 float input = *myInput;
annesteenbeek 51:1e6334b993a3 34 error = Setpoint - input; // get error for proportional
annesteenbeek 51:1e6334b993a3 35 errSum += error; // Error for integral
annesteenbeek 51:1e6334b993a3 36 // check for integrator windup
annesteenbeek 51:1e6334b993a3 37 if(errSum> outMax) errSum=outMax;
annesteenbeek 51:1e6334b993a3 38 else if(errSum<outMin) errSum=outMin;
annesteenbeek 51:1e6334b993a3 39 dErr = (error - lastErr); // get error for derivative
annesteenbeek 51:1e6334b993a3 40 // (could still be tuned for derivative kick)
annesteenbeek 51:1e6334b993a3 41
annesteenbeek 51:1e6334b993a3 42 // compute PID output
annesteenbeek 51:1e6334b993a3 43 float output = Kp * error + Ki * errSum + Kd * dErr;
annesteenbeek 51:1e6334b993a3 44 // make sure output is allso within min/max
annesteenbeek 51:1e6334b993a3 45 if(output> outMax) output=outMax;
annesteenbeek 51:1e6334b993a3 46 else if(output<outMin) output=outMin;
annesteenbeek 51:1e6334b993a3 47 *myOutput = output;
annesteenbeek 51:1e6334b993a3 48 lastErr = error;
annesteenbeek 51:1e6334b993a3 49 prevTime = curTime;
annesteenbeek 51:1e6334b993a3 50 return true;
annesteenbeek 51:1e6334b993a3 51 }
annesteenbeek 51:1e6334b993a3 52 else return false;
annesteenbeek 51:1e6334b993a3 53 }
annesteenbeek 51:1e6334b993a3 54
annesteenbeek 51:1e6334b993a3 55 void PID::SetTunigns(float Kp, float Ki, float Kd)
annesteenbeek 51:1e6334b993a3 56 {
annesteenbeek 51:1e6334b993a3 57 Kp = Kp;
annesteenbeek 51:1e6334b993a3 58 Ki = Ki * SampleTime; // change PID parameters according to sample time
annesteenbeek 51:1e6334b993a3 59 Kd = Kd / SampleTime;
annesteenbeek 51:1e6334b993a3 60 }
annesteenbeek 51:1e6334b993a3 61
annesteenbeek 51:1e6334b993a3 62
annesteenbeek 51:1e6334b993a3 63 void PID::SetSampleTime(float NewSampleTime){
annesteenbeek 51:1e6334b993a3 64 if (NewSampleTime > 0 ){
annesteenbeek 51:1e6334b993a3 65 // change ratios for parameters for better computation in compute()
annesteenbeek 51:1e6334b993a3 66 float ratio = NewSampleTime/SampleTime;
annesteenbeek 51:1e6334b993a3 67 Ki *= ratio;
annesteenbeek 51:1e6334b993a3 68 Kd /= ratio;
annesteenbeek 51:1e6334b993a3 69 SampleTime = NewSampleTime;
annesteenbeek 51:1e6334b993a3 70 }
annesteenbeek 51:1e6334b993a3 71 }
annesteenbeek 51:1e6334b993a3 72
annesteenbeek 51:1e6334b993a3 73 void PID::SetOutputLimits(float min, float max){
annesteenbeek 51:1e6334b993a3 74 if (min > max) return;
annesteenbeek 51:1e6334b993a3 75 outMin = min;
annesteenbeek 51:1e6334b993a3 76 outMax = max;
annesteenbeek 51:1e6334b993a3 77
annesteenbeek 51:1e6334b993a3 78 if(Output > outMax) Output = outMax;
annesteenbeek 51:1e6334b993a3 79 else if(Output < outMin) Output = outMin;
annesteenbeek 51:1e6334b993a3 80
annesteenbeek 51:1e6334b993a3 81 if(ITerm> outMax) ITerm= outMax;
annesteenbeek 51:1e6334b993a3 82 else if(ITerm< outMin) ITerm= outMin;
annesteenbeek 51:1e6334b993a3 83 }
annesteenbeek 51:1e6334b993a3 84
annesteenbeek 51:1e6334b993a3 85 void PID::SetMode(int Mode){
annesteenbeek 51:1e6334b993a3 86 // Turn PID on(AUTOMATIC) or off(MANUAL)
annesteenbeek 51:1e6334b993a3 87 bool newAuto = (Mode == AUTOMATIC);
annesteenbeek 51:1e6334b993a3 88 if(newAuto && !inAuto){
annesteenbeek 51:1e6334b993a3 89 Initialize(); // avoid bump because of stored values
annesteenbeek 51:1e6334b993a3 90 }
annesteenbeek 51:1e6334b993a3 91 inAuto = newAuto;
annesteenbeek 51:1e6334b993a3 92 }
annesteenbeek 51:1e6334b993a3 93
annesteenbeek 51:1e6334b993a3 94 void PID::Initialize(){
annesteenbeek 51:1e6334b993a3 95 lastInput = Input;
annesteenbeek 51:1e6334b993a3 96 errSum = Output;
annesteenbeek 51:1e6334b993a3 97 if(errSum> outMax) errSum=outMax;
annesteenbeek 51:1e6334b993a3 98 else if(errSum<outMin) errSum=outMin;
annesteenbeek 51:1e6334b993a3 99 }