Li Weiyi / Mbed 2 deprecated BalanceCar

虽然移植完毕，但是不work。需要细调……

Dependencies:   mbed

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?

UserRevisionLine numberNew 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