ported pid library from imu
Dependents: wheelchaircontrol wheelchaircontrolRos
PID_v1.cpp@1:45383b49536c, 2018-08-17 (annotated)
- Committer:
- ryanlin97
- Date:
- Fri Aug 17 20:46:39 2018 +0000
- Revision:
- 1:45383b49536c
- Parent:
- 0:539e3b3b91e1
adjust for jesus;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ryanlin97 | 0:539e3b3b91e1 | 1 | /********************************************************************************************** |
ryanlin97 | 0:539e3b3b91e1 | 2 | * Arduino PID Library - Version 1.2.1 |
ryanlin97 | 0:539e3b3b91e1 | 3 | * by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com |
ryanlin97 | 0:539e3b3b91e1 | 4 | * |
ryanlin97 | 0:539e3b3b91e1 | 5 | * This Library is licensed under the MIT License |
ryanlin97 | 0:539e3b3b91e1 | 6 | **********************************************************************************************/ |
ryanlin97 | 0:539e3b3b91e1 | 7 | /* |
ryanlin97 | 0:539e3b3b91e1 | 8 | #if ARDUINO >= 100 |
ryanlin97 | 0:539e3b3b91e1 | 9 | #include "Arduino.h" |
ryanlin97 | 0:539e3b3b91e1 | 10 | #else |
ryanlin97 | 0:539e3b3b91e1 | 11 | #include "WProgram.h" |
ryanlin97 | 0:539e3b3b91e1 | 12 | #endif*/ |
ryanlin97 | 0:539e3b3b91e1 | 13 | |
ryanlin97 | 0:539e3b3b91e1 | 14 | #include "mbed.h" |
ryanlin97 | 0:539e3b3b91e1 | 15 | #include "PID_v1.h" |
ryanlin97 | 0:539e3b3b91e1 | 16 | |
ryanlin97 | 0:539e3b3b91e1 | 17 | Timer PIDtimer; |
ryanlin97 | 0:539e3b3b91e1 | 18 | /*Constructor (...)********************************************************* |
ryanlin97 | 0:539e3b3b91e1 | 19 | * The parameters specified here are those for for which we can't set up |
ryanlin97 | 0:539e3b3b91e1 | 20 | * reliable defaults, so we need to have the user set them. |
ryanlin97 | 0:539e3b3b91e1 | 21 | ***************************************************************************/ |
ryanlin97 | 1:45383b49536c | 22 | PID::PID(double* Input, double* Output, double* Setpoint, double Kp, double Ki, double Kd, int POn, int ControllerDirection){ |
ryanlin97 | 1:45383b49536c | 23 | PIDtimer.start(); |
ryanlin97 | 1:45383b49536c | 24 | PIDtimer.reset(); |
ryanlin97 | 0:539e3b3b91e1 | 25 | myOutput = Output; |
ryanlin97 | 0:539e3b3b91e1 | 26 | myInput = Input; |
ryanlin97 | 0:539e3b3b91e1 | 27 | mySetpoint = Setpoint; |
ryanlin97 | 1:45383b49536c | 28 | inAuto = true; |
ryanlin97 | 1:45383b49536c | 29 | PID::SetOutputLimits(0, 1); //default output limit corresponds to |
ryanlin97 | 1:45383b49536c | 30 | //the arduino pwm limits |
ryanlin97 | 1:45383b49536c | 31 | SampleTime = .1; //default Controller Sample Time is 0.1 seconds |
ryanlin97 | 0:539e3b3b91e1 | 32 | |
ryanlin97 | 0:539e3b3b91e1 | 33 | PID::SetControllerDirection(ControllerDirection); |
ryanlin97 | 0:539e3b3b91e1 | 34 | PID::SetTunings(Kp, Ki, Kd, POn); |
ryanlin97 | 0:539e3b3b91e1 | 35 | |
ryanlin97 | 0:539e3b3b91e1 | 36 | lastTime = PIDtimer.read()-SampleTime; |
ryanlin97 | 1:45383b49536c | 37 | // printf("the values are %f, %f, %f, %f, %f, %f \r\n", *myOutput, *myInput, *mySetpoint, kp, ki, kd); |
ryanlin97 | 1:45383b49536c | 38 | } |
ryanlin97 | 1:45383b49536c | 39 | |
ryanlin97 | 0:539e3b3b91e1 | 40 | /* Compute() ********************************************************************** |
ryanlin97 | 0:539e3b3b91e1 | 41 | * This, as they say, is where the magic happens. this function should be called |
ryanlin97 | 0:539e3b3b91e1 | 42 | * every time "void loop()" executes. the function will decide for itself whether a new |
ryanlin97 | 0:539e3b3b91e1 | 43 | * pid Output needs to be computed. returns true when the output is computed, |
ryanlin97 | 0:539e3b3b91e1 | 44 | * false when nothing has been done. |
ryanlin97 | 0:539e3b3b91e1 | 45 | **********************************************************************************/ |
ryanlin97 | 0:539e3b3b91e1 | 46 | bool PID::Compute() |
ryanlin97 | 0:539e3b3b91e1 | 47 | { |
ryanlin97 | 0:539e3b3b91e1 | 48 | if(!inAuto) return false; |
ryanlin97 | 1:45383b49536c | 49 | double now = PIDtimer.read(); |
ryanlin97 | 1:45383b49536c | 50 | double timeChange = (now - lastTime); |
ryanlin97 | 0:539e3b3b91e1 | 51 | if(timeChange>=SampleTime) |
ryanlin97 | 0:539e3b3b91e1 | 52 | { |
ryanlin97 | 0:539e3b3b91e1 | 53 | /*Compute all the working error variables*/ |
ryanlin97 | 0:539e3b3b91e1 | 54 | double input = *myInput; |
ryanlin97 | 0:539e3b3b91e1 | 55 | double error = *mySetpoint - input; |
ryanlin97 | 0:539e3b3b91e1 | 56 | double dInput = (input - lastInput); |
ryanlin97 | 0:539e3b3b91e1 | 57 | outputSum+= (ki * error); |
ryanlin97 | 1:45383b49536c | 58 | |
ryanlin97 | 0:539e3b3b91e1 | 59 | /*Add Proportional on Measurement, if P_ON_M is specified*/ |
ryanlin97 | 1:45383b49536c | 60 | if(!pOnE) outputSum -= kp * dInput; |
ryanlin97 | 1:45383b49536c | 61 | printf("outputSum = %f \r\n", outputSum); |
ryanlin97 | 0:539e3b3b91e1 | 62 | if(outputSum > outMax) outputSum= outMax; |
ryanlin97 | 0:539e3b3b91e1 | 63 | else if(outputSum < outMin) outputSum= outMin; |
ryanlin97 | 0:539e3b3b91e1 | 64 | |
ryanlin97 | 0:539e3b3b91e1 | 65 | /*Add Proportional on Error, if P_ON_E is specified*/ |
ryanlin97 | 1:45383b49536c | 66 | double output; |
ryanlin97 | 0:539e3b3b91e1 | 67 | if(pOnE) output = kp * error; |
ryanlin97 | 0:539e3b3b91e1 | 68 | else output = 0; |
ryanlin97 | 0:539e3b3b91e1 | 69 | /*Compute Rest of PID Output*/ |
ryanlin97 | 0:539e3b3b91e1 | 70 | output += outputSum - kd * dInput; |
ryanlin97 | 0:539e3b3b91e1 | 71 | |
ryanlin97 | 1:45383b49536c | 72 | if(output > outMax) output = outMax; |
ryanlin97 | 0:539e3b3b91e1 | 73 | else if(output < outMin) output = outMin; |
ryanlin97 | 1:45383b49536c | 74 | *myOutput = output; |
ryanlin97 | 0:539e3b3b91e1 | 75 | |
ryanlin97 | 0:539e3b3b91e1 | 76 | /*Remember some variables for next time*/ |
ryanlin97 | 0:539e3b3b91e1 | 77 | lastInput = input; |
ryanlin97 | 0:539e3b3b91e1 | 78 | lastTime = now; |
ryanlin97 | 1:45383b49536c | 79 | return true; |
ryanlin97 | 0:539e3b3b91e1 | 80 | } |
ryanlin97 | 1:45383b49536c | 81 | else |
ryanlin97 | 1:45383b49536c | 82 | { |
ryanlin97 | 1:45383b49536c | 83 | return false; |
ryanlin97 | 1:45383b49536c | 84 | } |
ryanlin97 | 0:539e3b3b91e1 | 85 | } |
ryanlin97 | 0:539e3b3b91e1 | 86 | |
ryanlin97 | 0:539e3b3b91e1 | 87 | /* SetTunings(...)************************************************************* |
ryanlin97 | 0:539e3b3b91e1 | 88 | * This function allows the controller's dynamic performance to be adjusted. |
ryanlin97 | 0:539e3b3b91e1 | 89 | * it's called automatically from the constructor, but tunings can also |
ryanlin97 | 0:539e3b3b91e1 | 90 | * be adjusted on the fly during normal operation |
ryanlin97 | 0:539e3b3b91e1 | 91 | ******************************************************************************/ |
ryanlin97 | 0:539e3b3b91e1 | 92 | void PID::SetTunings(double Kp, double Ki, double Kd, int POn) |
ryanlin97 | 0:539e3b3b91e1 | 93 | { |
ryanlin97 | 0:539e3b3b91e1 | 94 | if (Kp<0 || Ki<0 || Kd<0) return; |
ryanlin97 | 0:539e3b3b91e1 | 95 | |
ryanlin97 | 0:539e3b3b91e1 | 96 | pOn = POn; |
ryanlin97 | 0:539e3b3b91e1 | 97 | pOnE = POn == P_ON_E; |
ryanlin97 | 0:539e3b3b91e1 | 98 | |
ryanlin97 | 0:539e3b3b91e1 | 99 | dispKp = Kp; dispKi = Ki; dispKd = Kd; |
ryanlin97 | 0:539e3b3b91e1 | 100 | |
ryanlin97 | 0:539e3b3b91e1 | 101 | double SampleTimeInSec = ((double)SampleTime)/1000; |
ryanlin97 | 0:539e3b3b91e1 | 102 | kp = Kp; |
ryanlin97 | 0:539e3b3b91e1 | 103 | ki = Ki * SampleTimeInSec; |
ryanlin97 | 0:539e3b3b91e1 | 104 | kd = Kd / SampleTimeInSec; |
ryanlin97 | 0:539e3b3b91e1 | 105 | |
ryanlin97 | 0:539e3b3b91e1 | 106 | if(controllerDirection ==REVERSE) |
ryanlin97 | 0:539e3b3b91e1 | 107 | { |
ryanlin97 | 0:539e3b3b91e1 | 108 | kp = (0 - kp); |
ryanlin97 | 0:539e3b3b91e1 | 109 | ki = (0 - ki); |
ryanlin97 | 0:539e3b3b91e1 | 110 | kd = (0 - kd); |
ryanlin97 | 0:539e3b3b91e1 | 111 | } |
ryanlin97 | 0:539e3b3b91e1 | 112 | } |
ryanlin97 | 0:539e3b3b91e1 | 113 | |
ryanlin97 | 0:539e3b3b91e1 | 114 | /* SetTunings(...)************************************************************* |
ryanlin97 | 0:539e3b3b91e1 | 115 | * Set Tunings using the last-rembered POn setting |
ryanlin97 | 0:539e3b3b91e1 | 116 | ******************************************************************************/ |
ryanlin97 | 0:539e3b3b91e1 | 117 | void PID::SetTunings(double Kp, double Ki, double Kd){ |
ryanlin97 | 0:539e3b3b91e1 | 118 | SetTunings(Kp, Ki, Kd, pOn); |
ryanlin97 | 0:539e3b3b91e1 | 119 | } |
ryanlin97 | 0:539e3b3b91e1 | 120 | |
ryanlin97 | 0:539e3b3b91e1 | 121 | /* SetSampleTime(...) ********************************************************* |
ryanlin97 | 0:539e3b3b91e1 | 122 | * sets the period, in Milliseconds, at which the calculation is performed |
ryanlin97 | 0:539e3b3b91e1 | 123 | ******************************************************************************/ |
ryanlin97 | 0:539e3b3b91e1 | 124 | void PID::SetSampleTime(int NewSampleTime) |
ryanlin97 | 0:539e3b3b91e1 | 125 | { |
ryanlin97 | 0:539e3b3b91e1 | 126 | if (NewSampleTime > 0) |
ryanlin97 | 0:539e3b3b91e1 | 127 | { |
ryanlin97 | 0:539e3b3b91e1 | 128 | double ratio = (double)NewSampleTime |
ryanlin97 | 0:539e3b3b91e1 | 129 | / (double)SampleTime; |
ryanlin97 | 0:539e3b3b91e1 | 130 | ki *= ratio; |
ryanlin97 | 0:539e3b3b91e1 | 131 | kd /= ratio; |
ryanlin97 | 0:539e3b3b91e1 | 132 | SampleTime = (unsigned long)NewSampleTime; |
ryanlin97 | 0:539e3b3b91e1 | 133 | } |
ryanlin97 | 0:539e3b3b91e1 | 134 | } |
ryanlin97 | 0:539e3b3b91e1 | 135 | |
ryanlin97 | 0:539e3b3b91e1 | 136 | /* SetOutputLimits(...)**************************************************** |
ryanlin97 | 0:539e3b3b91e1 | 137 | * This function will be used far more often than SetInputLimits. while |
ryanlin97 | 0:539e3b3b91e1 | 138 | * the input to the controller will generally be in the 0-1023 range (which is |
ryanlin97 | 0:539e3b3b91e1 | 139 | * the default already,) the output will be a little different. maybe they'll |
ryanlin97 | 0:539e3b3b91e1 | 140 | * be doing a time window and will need 0-8000 or something. or maybe they'll |
ryanlin97 | 0:539e3b3b91e1 | 141 | * want to clamp it from 0-125. who knows. at any rate, that can all be done |
ryanlin97 | 0:539e3b3b91e1 | 142 | * here. |
ryanlin97 | 0:539e3b3b91e1 | 143 | **************************************************************************/ |
ryanlin97 | 0:539e3b3b91e1 | 144 | void PID::SetOutputLimits(double Min, double Max) |
ryanlin97 | 0:539e3b3b91e1 | 145 | { |
ryanlin97 | 0:539e3b3b91e1 | 146 | if(Min >= Max) return; |
ryanlin97 | 0:539e3b3b91e1 | 147 | outMin = Min; |
ryanlin97 | 0:539e3b3b91e1 | 148 | outMax = Max; |
ryanlin97 | 0:539e3b3b91e1 | 149 | |
ryanlin97 | 0:539e3b3b91e1 | 150 | if(inAuto) |
ryanlin97 | 0:539e3b3b91e1 | 151 | { |
ryanlin97 | 1:45383b49536c | 152 | if(*myOutput > outMax) *myOutput = outMax; |
ryanlin97 | 1:45383b49536c | 153 | else if(*myOutput < outMin) *myOutput = outMin; |
ryanlin97 | 0:539e3b3b91e1 | 154 | |
ryanlin97 | 1:45383b49536c | 155 | if(outputSum > outMax) outputSum= outMax; |
ryanlin97 | 1:45383b49536c | 156 | else if(outputSum < outMin) outputSum= outMin; |
ryanlin97 | 0:539e3b3b91e1 | 157 | } |
ryanlin97 | 0:539e3b3b91e1 | 158 | } |
ryanlin97 | 0:539e3b3b91e1 | 159 | |
ryanlin97 | 0:539e3b3b91e1 | 160 | /* SetMode(...)**************************************************************** |
ryanlin97 | 0:539e3b3b91e1 | 161 | * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) |
ryanlin97 | 0:539e3b3b91e1 | 162 | * when the transition from manual to auto occurs, the controller is |
ryanlin97 | 0:539e3b3b91e1 | 163 | * automatically initialized |
ryanlin97 | 0:539e3b3b91e1 | 164 | ******************************************************************************/ |
ryanlin97 | 0:539e3b3b91e1 | 165 | void PID::SetMode(int Mode) |
ryanlin97 | 0:539e3b3b91e1 | 166 | { |
ryanlin97 | 0:539e3b3b91e1 | 167 | bool newAuto = (Mode == AUTOMATIC); |
ryanlin97 | 0:539e3b3b91e1 | 168 | if(newAuto && !inAuto) |
ryanlin97 | 0:539e3b3b91e1 | 169 | { /*we just went from manual to auto*/ |
ryanlin97 | 0:539e3b3b91e1 | 170 | PID::Initialize(); |
ryanlin97 | 0:539e3b3b91e1 | 171 | } |
ryanlin97 | 0:539e3b3b91e1 | 172 | inAuto = newAuto; |
ryanlin97 | 0:539e3b3b91e1 | 173 | } |
ryanlin97 | 0:539e3b3b91e1 | 174 | |
ryanlin97 | 0:539e3b3b91e1 | 175 | /* Initialize()**************************************************************** |
ryanlin97 | 1:45383b49536c | 176 | * does all the things that need to happen to ensure a bumpless transfer |
ryanlin97 | 0:539e3b3b91e1 | 177 | * from manual to automatic mode. |
ryanlin97 | 0:539e3b3b91e1 | 178 | ******************************************************************************/ |
ryanlin97 | 0:539e3b3b91e1 | 179 | void PID::Initialize() |
ryanlin97 | 0:539e3b3b91e1 | 180 | { |
ryanlin97 | 0:539e3b3b91e1 | 181 | outputSum = *myOutput; |
ryanlin97 | 0:539e3b3b91e1 | 182 | lastInput = *myInput; |
ryanlin97 | 0:539e3b3b91e1 | 183 | if(outputSum > outMax) outputSum = outMax; |
ryanlin97 | 0:539e3b3b91e1 | 184 | else if(outputSum < outMin) outputSum = outMin; |
ryanlin97 | 0:539e3b3b91e1 | 185 | } |
ryanlin97 | 0:539e3b3b91e1 | 186 | |
ryanlin97 | 0:539e3b3b91e1 | 187 | /* SetControllerDirection(...)************************************************* |
ryanlin97 | 0:539e3b3b91e1 | 188 | * The PID will either be connected to a DIRECT acting process (+Output leads |
ryanlin97 | 0:539e3b3b91e1 | 189 | * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to |
ryanlin97 | 0:539e3b3b91e1 | 190 | * know which one, because otherwise we may increase the output when we should |
ryanlin97 | 0:539e3b3b91e1 | 191 | * be decreasing. This is called from the constructor. |
ryanlin97 | 0:539e3b3b91e1 | 192 | ******************************************************************************/ |
ryanlin97 | 0:539e3b3b91e1 | 193 | void PID::SetControllerDirection(int Direction) |
ryanlin97 | 0:539e3b3b91e1 | 194 | { |
ryanlin97 | 0:539e3b3b91e1 | 195 | if(inAuto && Direction !=controllerDirection) |
ryanlin97 | 0:539e3b3b91e1 | 196 | { |
ryanlin97 | 1:45383b49536c | 197 | kp = (0 - kp); |
ryanlin97 | 0:539e3b3b91e1 | 198 | ki = (0 - ki); |
ryanlin97 | 0:539e3b3b91e1 | 199 | kd = (0 - kd); |
ryanlin97 | 0:539e3b3b91e1 | 200 | } |
ryanlin97 | 0:539e3b3b91e1 | 201 | controllerDirection = Direction; |
ryanlin97 | 0:539e3b3b91e1 | 202 | } |
ryanlin97 | 0:539e3b3b91e1 | 203 | |
ryanlin97 | 0:539e3b3b91e1 | 204 | /* Status Funcions************************************************************* |
ryanlin97 | 0:539e3b3b91e1 | 205 | * Just because you set the Kp=-1 doesn't mean it actually happened. these |
ryanlin97 | 0:539e3b3b91e1 | 206 | * functions query the internal state of the PID. they're here for display |
ryanlin97 | 0:539e3b3b91e1 | 207 | * purposes. this are the functions the PID Front-end uses for example |
ryanlin97 | 0:539e3b3b91e1 | 208 | ******************************************************************************/ |
ryanlin97 | 0:539e3b3b91e1 | 209 | double PID::GetKp(){ return dispKp; } |
ryanlin97 | 0:539e3b3b91e1 | 210 | double PID::GetKi(){ return dispKi;} |
ryanlin97 | 0:539e3b3b91e1 | 211 | double PID::GetKd(){ return dispKd;} |
ryanlin97 | 0:539e3b3b91e1 | 212 | int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} |
ryanlin97 | 1:45383b49536c | 213 | int PID::GetDirection(){ return controllerDirection;} |