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@55:ee5257fb73df, 2015-10-09 (annotated)

Committer:
annesteenbeek
Date:
Fri Oct 09 12:59:00 2015 +0000
Revision:
55:ee5257fb73df
Parent:
54:c14c3bc48b8a
Child:
57:43f707648f2b
bug fixes for PID filter

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 54:c14c3bc48b8a 6 // PID is called with interrupt caller, so no need to keep track of time since dT is constant
annesteenbeek 54:c14c3bc48b8a 7 //Timer t; // create Timer object t
annesteenbeek 54:c14c3bc48b8a 8 //t.start(); // start the timer
annesteenbeek 51:1e6334b993a3 9
annesteenbeek 51:1e6334b993a3 10
annesteenbeek 51:1e6334b993a3 11 PID::PID(float* Input, float* Output, float* Setpoint, float Kp, float Ki, float Kd){
annesteenbeek 51:1e6334b993a3 12 myOutput = Output;
annesteenbeek 51:1e6334b993a3 13 myInput = Input;
annesteenbeek 51:1e6334b993a3 14 mySetpoint = Setpoint;
annesteenbeek 51:1e6334b993a3 15 inAuto = false;
annesteenbeek 51:1e6334b993a3 16
annesteenbeek 51:1e6334b993a3 17 PID::SetOutputLimits(-1.0,1.0); // default output limits for FRDM board
annesteenbeek 51:1e6334b993a3 18
annesteenbeek 51:1e6334b993a3 19 SampleTime = 0.1;
annesteenbeek 51:1e6334b993a3 20
annesteenbeek 51:1e6334b993a3 21 PID::SetTunings(Kp, Ki, Kd);
annesteenbeek 54:c14c3bc48b8a 22 // prevTime = t.read();
annesteenbeek 51:1e6334b993a3 23 }
annesteenbeek 51:1e6334b993a3 24
annesteenbeek 54:c14c3bc48b8a 25 bool PID::Compute(){
annesteenbeek 51:1e6334b993a3 26 if(!inAuto) return false;
annesteenbeek 51:1e6334b993a3 27
annesteenbeek 54:c14c3bc48b8a 28 // curTime = t.read() // read time in seconds (no need since called by interrupt)
annesteenbeek 54:c14c3bc48b8a 29 // timeChange = curTime - prevTime;
annesteenbeek 51:1e6334b993a3 30
annesteenbeek 51:1e6334b993a3 31 // calculate using sample time frequency (regular intervals)
annesteenbeek 51:1e6334b993a3 32 // no more need to modify derivative/int sicne sample time is constant
annesteenbeek 54:c14c3bc48b8a 33 // if(timeChange <=SampleTime){
annesteenbeek 51:1e6334b993a3 34 float input = *myInput;
annesteenbeek 54:c14c3bc48b8a 35 error = *mySetpoint - input; // get error for proportional
annesteenbeek 51:1e6334b993a3 36 errSum += error; // Error for integral
annesteenbeek 51:1e6334b993a3 37 // check for integrator windup
annesteenbeek 51:1e6334b993a3 38 if(errSum> outMax) errSum=outMax;
annesteenbeek 51:1e6334b993a3 39 else if(errSum<outMin) errSum=outMin;
annesteenbeek 51:1e6334b993a3 40 dErr = (error - lastErr); // get error for derivative
annesteenbeek 51:1e6334b993a3 41 // (could still be tuned for derivative kick)
annesteenbeek 51:1e6334b993a3 42
annesteenbeek 51:1e6334b993a3 43 // compute PID output
annesteenbeek 55:ee5257fb73df 44 float output = kp * error + ki * errSum + kd * dErr;
annesteenbeek 51:1e6334b993a3 45 // make sure output is allso within min/max
annesteenbeek 51:1e6334b993a3 46 if(output> outMax) output=outMax;
annesteenbeek 51:1e6334b993a3 47 else if(output<outMin) output=outMin;
annesteenbeek 51:1e6334b993a3 48 *myOutput = output;
annesteenbeek 51:1e6334b993a3 49 lastErr = error;
annesteenbeek 54:c14c3bc48b8a 50 // prevTime = curTime;
annesteenbeek 51:1e6334b993a3 51 return true;
annesteenbeek 54:c14c3bc48b8a 52 // }
annesteenbeek 54:c14c3bc48b8a 53 // else return false;
annesteenbeek 51:1e6334b993a3 54 }
annesteenbeek 51:1e6334b993a3 55
annesteenbeek 54:c14c3bc48b8a 56 void PID::SetTunings(float Kp, float Ki, float Kd)
annesteenbeek 51:1e6334b993a3 57 {
annesteenbeek 55:ee5257fb73df 58 kp = Kp;
annesteenbeek 55:ee5257fb73df 59 ki = Ki * SampleTime; // change PID parameters according to sample time
annesteenbeek 55:ee5257fb73df 60 kd = Kd / SampleTime;
annesteenbeek 51:1e6334b993a3 61 }
annesteenbeek 51:1e6334b993a3 62
annesteenbeek 51:1e6334b993a3 63
annesteenbeek 51:1e6334b993a3 64 void PID::SetSampleTime(float NewSampleTime){
annesteenbeek 51:1e6334b993a3 65 if (NewSampleTime > 0 ){
annesteenbeek 51:1e6334b993a3 66 // change ratios for parameters for better computation in compute()
annesteenbeek 51:1e6334b993a3 67 float ratio = NewSampleTime/SampleTime;
annesteenbeek 55:ee5257fb73df 68 ki *= ratio;
annesteenbeek 55:ee5257fb73df 69 kd /= ratio;
annesteenbeek 51:1e6334b993a3 70 SampleTime = NewSampleTime;
annesteenbeek 51:1e6334b993a3 71 }
annesteenbeek 51:1e6334b993a3 72 }
annesteenbeek 51:1e6334b993a3 73
annesteenbeek 51:1e6334b993a3 74 void PID::SetOutputLimits(float min, float max){
annesteenbeek 51:1e6334b993a3 75 if (min > max) return;
annesteenbeek 51:1e6334b993a3 76 outMin = min;
annesteenbeek 51:1e6334b993a3 77 outMax = max;
annesteenbeek 51:1e6334b993a3 78
annesteenbeek 54:c14c3bc48b8a 79 if(inAuto){
annesteenbeek 54:c14c3bc48b8a 80 if(*myOutput > outMax) *myOutput = outMax;
annesteenbeek 54:c14c3bc48b8a 81 else if(*myOutput < outMin) *myOutput = outMin;
annesteenbeek 54:c14c3bc48b8a 82
annesteenbeek 55:ee5257fb73df 83 if(errSum > outMax) errSum= outMax;
annesteenbeek 55:ee5257fb73df 84 else if(errSum < outMin) errSum= outMin;
annesteenbeek 54:c14c3bc48b8a 85 }
annesteenbeek 51:1e6334b993a3 86 }
annesteenbeek 51:1e6334b993a3 87
annesteenbeek 51:1e6334b993a3 88 void PID::SetMode(int Mode){
annesteenbeek 51:1e6334b993a3 89 // Turn PID on(AUTOMATIC) or off(MANUAL)
annesteenbeek 51:1e6334b993a3 90 bool newAuto = (Mode == AUTOMATIC);
annesteenbeek 51:1e6334b993a3 91 if(newAuto && !inAuto){
annesteenbeek 55:ee5257fb73df 92 PID::Initialize(); // avoid bump because of stored values
annesteenbeek 51:1e6334b993a3 93 }
annesteenbeek 51:1e6334b993a3 94 inAuto = newAuto;
annesteenbeek 51:1e6334b993a3 95 }
annesteenbeek 51:1e6334b993a3 96
annesteenbeek 51:1e6334b993a3 97 void PID::Initialize(){
annesteenbeek 54:c14c3bc48b8a 98 lastInput = *myInput;
annesteenbeek 54:c14c3bc48b8a 99 errSum = *myOutput;
annesteenbeek 54:c14c3bc48b8a 100 lastErr = 0;
annesteenbeek 51:1e6334b993a3 101 if(errSum> outMax) errSum=outMax;
annesteenbeek 51:1e6334b993a3 102 else if(errSum<outMin) errSum=outMin;
annesteenbeek 55:ee5257fb73df 103 }
annesteenbeek 55:ee5257fb73df 104
annesteenbeek 55:ee5257fb73df 105 float PID::getOutput(){
annesteenbeek 55:ee5257fb73df 106 return output;
annesteenbeek 55:ee5257fb73df 107 }
annesteenbeek 55:ee5257fb73df 108