control for robotic arm that can play chess using a granular gripper
Dependencies: Encoder mbed HIDScope Servo MODSERIAL
Fork of chessRobot by
PID/PID.cpp@130:2542f844ba1e, 2015-10-30 (annotated)
- Committer:
- annesteenbeek
- Date:
- Fri Oct 30 10:32:49 2015 +0100
- Revision:
- 130:2542f844ba1e
- Parent:
- 82:4cc8f9ad3fec
changes
Who changed what in which revision?
User | Revision | Line number | New 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 | 82:4cc8f9ad3fec | 113 | void PID::SetDirection(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 | } |