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