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@81:71e7e98deb2c, 2015-10-20 (annotated)

Committer:
annesteenbeek
Date:
Tue Oct 20 14:25:31 2015 +0200
Revision:
81:71e7e98deb2c
Parent:
62:6c566e6f9664
Child:
82:4cc8f9ad3fec
added PID direction

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 62:6c566e6f9664 11 PID::PID(double* Input, double* Output, double* Setpoint, double Kp, double Ki, double 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 62:6c566e6f9664 34 double input = *myInput;
annesteenbeek 54:c14c3bc48b8a 35 error = *mySetpoint - input; // get error for proportional
annesteenbeek 57:43f707648f2b 36 ITerm += (ki*error); // Error for integral (with ki included)
annesteenbeek 51:1e6334b993a3 37 // check for integrator windup
annesteenbeek 58:0ef7bb34fe29 38 if(ITerm> outMax) ITerm=outMax;
annesteenbeek 58:0ef7bb34fe29 39 else if(ITerm<outMin) ITerm=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 62:6c566e6f9664 44 double output = kp * error + ITerm + 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 62:6c566e6f9664 56 void PID::SetTunings(double Kp, double Ki, double Kd)
annesteenbeek 51:1e6334b993a3 57 {
annesteenbeek 81:71e7e98deb2c 58 if (Kp<0 || Ki<0|| Kd<0) return;
annesteenbeek 55:ee5257fb73df 59 kp = Kp;
annesteenbeek 81:71e7e98deb2c 60 ki = Ki * SampleTime; // change PID parameters according to sample time (in seconds)
annesteenbeek 55:ee5257fb73df 61 kd = Kd / SampleTime;
annesteenbeek 81:71e7e98deb2c 62
annesteenbeek 81:71e7e98deb2c 63 if(controllerDirection ==REVERSE){
annesteenbeek 81:71e7e98deb2c 64 kp = (0 - kp);
annesteenbeek 81:71e7e98deb2c 65 ki = (0 - ki);
annesteenbeek 81:71e7e98deb2c 66 kd = (0 - kd);
annesteenbeek 81:71e7e98deb2c 67 }
annesteenbeek 51:1e6334b993a3 68 }
annesteenbeek 51:1e6334b993a3 69
annesteenbeek 51:1e6334b993a3 70
annesteenbeek 81:71e7e98deb2c 71
annesteenbeek 62:6c566e6f9664 72 void PID::SetSampleTime(double NewSampleTime){
annesteenbeek 51:1e6334b993a3 73 if (NewSampleTime > 0 ){
annesteenbeek 51:1e6334b993a3 74 // change ratios for parameters for better computation in compute()
annesteenbeek 62:6c566e6f9664 75 double ratio = NewSampleTime/SampleTime;
annesteenbeek 55:ee5257fb73df 76 ki *= ratio;
annesteenbeek 55:ee5257fb73df 77 kd /= ratio;
annesteenbeek 51:1e6334b993a3 78 SampleTime = NewSampleTime;
annesteenbeek 51:1e6334b993a3 79 }
annesteenbeek 51:1e6334b993a3 80 }
annesteenbeek 51:1e6334b993a3 81
annesteenbeek 62:6c566e6f9664 82 void PID::SetOutputLimits(double min, double max){
annesteenbeek 51:1e6334b993a3 83 if (min > max) return;
annesteenbeek 51:1e6334b993a3 84 outMin = min;
annesteenbeek 51:1e6334b993a3 85 outMax = max;
annesteenbeek 51:1e6334b993a3 86
annesteenbeek 54:c14c3bc48b8a 87 if(inAuto){
annesteenbeek 54:c14c3bc48b8a 88 if(*myOutput > outMax) *myOutput = outMax;
annesteenbeek 54:c14c3bc48b8a 89 else if(*myOutput < outMin) *myOutput = outMin;
annesteenbeek 54:c14c3bc48b8a 90
annesteenbeek 58:0ef7bb34fe29 91 if(ITerm > outMax) ITerm= outMax;
annesteenbeek 58:0ef7bb34fe29 92 else if(ITerm < outMin) ITerm= outMin;
annesteenbeek 54:c14c3bc48b8a 93 }
annesteenbeek 51:1e6334b993a3 94 }
annesteenbeek 51:1e6334b993a3 95
annesteenbeek 51:1e6334b993a3 96 void PID::SetMode(int Mode){
annesteenbeek 51:1e6334b993a3 97 // Turn PID on(AUTOMATIC) or off(MANUAL)
annesteenbeek 51:1e6334b993a3 98 bool newAuto = (Mode == AUTOMATIC);
annesteenbeek 51:1e6334b993a3 99 if(newAuto && !inAuto){
annesteenbeek 55:ee5257fb73df 100 PID::Initialize(); // avoid bump because of stored values
annesteenbeek 51:1e6334b993a3 101 }
annesteenbeek 51:1e6334b993a3 102 inAuto = newAuto;
annesteenbeek 51:1e6334b993a3 103 }
annesteenbeek 51:1e6334b993a3 104
annesteenbeek 51:1e6334b993a3 105 void PID::Initialize(){
annesteenbeek 54:c14c3bc48b8a 106 lastInput = *myInput;
annesteenbeek 57:43f707648f2b 107 ITerm = *myOutput;
annesteenbeek 54:c14c3bc48b8a 108 lastErr = 0;
annesteenbeek 58:0ef7bb34fe29 109 if(ITerm> outMax) ITerm=outMax;
annesteenbeek 58:0ef7bb34fe29 110 else if(ITerm<outMin) ITerm=outMin;
annesteenbeek 55:ee5257fb73df 111 }
annesteenbeek 60:20945383ad1b 112
annesteenbeek 81:71e7e98deb2c 113 void PID::SetControllerDirection(int Direction){
annesteenbeek 81:71e7e98deb2c 114 controllerDirection = Direction;
annesteenbeek 81:71e7e98deb2c 115 }
annesteenbeek 81:71e7e98deb2c 116
annesteenbeek 62:6c566e6f9664 117 double PID::getKp(){
annesteenbeek 60:20945383ad1b 118 return kp;
annesteenbeek 60:20945383ad1b 119 }
annesteenbeek 60:20945383ad1b 120
annesteenbeek 62:6c566e6f9664 121 double PID::getKi(){
annesteenbeek 60:20945383ad1b 122 return ki/SampleTime;
annesteenbeek 60:20945383ad1b 123 }
annesteenbeek 60:20945383ad1b 124
annesteenbeek 62:6c566e6f9664 125 double PID::getKd(){
annesteenbeek 60:20945383ad1b 126 return kd*SampleTime;
annesteenbeek 60:20945383ad1b 127 }