Li Weiyi / Mbed 2 deprecated BalanceCar

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

Dependencies:   mbed

PID_v2.cpp@3:c6caae712d5d, 2016-06-07 (annotated)

Committer:
lixianyu
Date:
Tue Jun 07 08:14:15 2016 +0000
Revision:
3:c6caae712d5d
Parent:
2:99785a1007a4 
??????????work? ?????

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 3:c6caae712d5d 60 //if(dt>=SampleTime) {
lixianyu 3:c6caae712d5d 61 if (1) {
lixianyu 0:a4d8f5b3c546 62 /*Compute all the working error variables*/
lixianyu 0:a4d8f5b3c546 63 float input = Input;
lixianyu 0:a4d8f5b3c546 64 float error = Setpoint - input;
lixianyu 0:a4d8f5b3c546 65 ITerm += error * dt;
lixianyu 0:a4d8f5b3c546 66 ITerm = constrain(ITerm, errorMin, errorMax);
lixianyu 0:a4d8f5b3c546 67 float dInput = (input - lastInput)/dt;
lixianyu 0:a4d8f5b3c546 68
lixianyu 0:a4d8f5b3c546 69 /*Compute PID Output*/
lixianyu 0:a4d8f5b3c546 70 output = kp * error + ki * ITerm - kd * dInput;
lixianyu 0:a4d8f5b3c546 71
lixianyu 0:a4d8f5b3c546 72 output = constrain(output, outMin, outMax);
lixianyu 0:a4d8f5b3c546 73 // *myOutput = output;
lixianyu 0:a4d8f5b3c546 74
lixianyu 0:a4d8f5b3c546 75 /*Remember some variables for next time*/
lixianyu 0:a4d8f5b3c546 76 lastInput = input;
lixianyu 0:a4d8f5b3c546 77 lastTime = now;
lixianyu 0:a4d8f5b3c546 78 // return true;
lixianyu 0:a4d8f5b3c546 79 }
lixianyu 0:a4d8f5b3c546 80 return output;
lixianyu 0:a4d8f5b3c546 81 }
lixianyu 0:a4d8f5b3c546 82
lixianyu 0:a4d8f5b3c546 83
lixianyu 0:a4d8f5b3c546 84 /* SetTunings(...)*************************************************************
lixianyu 0:a4d8f5b3c546 85 * This function allows the controller's dynamic performance to be adjusted.
lixianyu 0:a4d8f5b3c546 86 * it's called automatically from the constructor, but tunings can also
lixianyu 0:a4d8f5b3c546 87 * be adjusted on the fly during normal operation
lixianyu 0:a4d8f5b3c546 88 ******************************************************************************/
lixianyu 0:a4d8f5b3c546 89 void PID::SetTunings(double Kp, double Ki, double Kd)
lixianyu 0:a4d8f5b3c546 90 {
lixianyu 0:a4d8f5b3c546 91 if (Kp<0 || Ki<0 || Kd<0) return;
lixianyu 0:a4d8f5b3c546 92
lixianyu 2:99785a1007a4 93 //double SampleTimeInSec = ((double)SampleTime)/1000;
lixianyu 0:a4d8f5b3c546 94 kp = Kp;
lixianyu 0:a4d8f5b3c546 95 ki = Ki;
lixianyu 0:a4d8f5b3c546 96 kd = Kd;
lixianyu 0:a4d8f5b3c546 97
lixianyu 0:a4d8f5b3c546 98 dispKp = Kp;
lixianyu 0:a4d8f5b3c546 99 dispKi = Ki;
lixianyu 0:a4d8f5b3c546 100 dispKd = Kd;
lixianyu 0:a4d8f5b3c546 101
lixianyu 0:a4d8f5b3c546 102 if(controllerDirection ==REVERSE) {
lixianyu 0:a4d8f5b3c546 103 kp = (0 - kp);
lixianyu 0:a4d8f5b3c546 104 ki = (0 - ki);
lixianyu 0:a4d8f5b3c546 105 kd = (0 - kd);
lixianyu 0:a4d8f5b3c546 106 }
lixianyu 0:a4d8f5b3c546 107 }
lixianyu 0:a4d8f5b3c546 108
lixianyu 0:a4d8f5b3c546 109 /* SetSampleTime(...) *********************************************************
lixianyu 0:a4d8f5b3c546 110 * sets the period, in Milliseconds, at which the calculation is performed
lixianyu 0:a4d8f5b3c546 111 ******************************************************************************/
lixianyu 0:a4d8f5b3c546 112 void PID::SetSampleTime(int NewSampleTime)
lixianyu 0:a4d8f5b3c546 113 {
lixianyu 0:a4d8f5b3c546 114 if (NewSampleTime > 0) {
lixianyu 0:a4d8f5b3c546 115 double ratio = (double)NewSampleTime
lixianyu 0:a4d8f5b3c546 116 / (double)SampleTime;
lixianyu 0:a4d8f5b3c546 117 ki *= ratio;
lixianyu 0:a4d8f5b3c546 118 kd /= ratio;
lixianyu 0:a4d8f5b3c546 119 SampleTime = (unsigned long)NewSampleTime;
lixianyu 0:a4d8f5b3c546 120 }
lixianyu 0:a4d8f5b3c546 121 }
lixianyu 0:a4d8f5b3c546 122
lixianyu 0:a4d8f5b3c546 123 /* SetOutputLimits(...)****************************************************
lixianyu 0:a4d8f5b3c546 124 * This function will be used far more often than SetInputLimits. while
lixianyu 0:a4d8f5b3c546 125 * the input to the controller will generally be in the 0-1023 range (which is
lixianyu 0:a4d8f5b3c546 126 * the default already,) the output will be a little different. maybe they'll
lixianyu 0:a4d8f5b3c546 127 * be doing a time window and will need 0-8000 or something. or maybe they'll
lixianyu 0:a4d8f5b3c546 128 * want to clamp it from 0-125. who knows. at any rate, that can all be done
lixianyu 0:a4d8f5b3c546 129 * here.
lixianyu 0:a4d8f5b3c546 130 **************************************************************************/
lixianyu 0:a4d8f5b3c546 131 void PID::SetOutputLimits(double Min, double Max)
lixianyu 0:a4d8f5b3c546 132 {
lixianyu 0:a4d8f5b3c546 133 if(Min >= Max) return;
lixianyu 0:a4d8f5b3c546 134 outMin = Min;
lixianyu 0:a4d8f5b3c546 135 outMax = Max;
lixianyu 0:a4d8f5b3c546 136
lixianyu 0:a4d8f5b3c546 137 if(inAuto)
lixianyu 0:a4d8f5b3c546 138 *myOutput = constrain(*myOutput, outMin, outMax);
lixianyu 0:a4d8f5b3c546 139 }
lixianyu 0:a4d8f5b3c546 140
lixianyu 0:a4d8f5b3c546 141 void PID::SetITermLimits(double Min, double Max)
lixianyu 0:a4d8f5b3c546 142 {
lixianyu 0:a4d8f5b3c546 143 if(Min >= Max) return;
lixianyu 0:a4d8f5b3c546 144 errorMin = Min;
lixianyu 0:a4d8f5b3c546 145 errorMax = Max;
lixianyu 0:a4d8f5b3c546 146
lixianyu 0:a4d8f5b3c546 147 if(inAuto)
lixianyu 0:a4d8f5b3c546 148 ITerm = constrain(ITerm, errorMin, errorMax);
lixianyu 0:a4d8f5b3c546 149 }
lixianyu 0:a4d8f5b3c546 150
lixianyu 0:a4d8f5b3c546 151 /* SetMode(...)****************************************************************
lixianyu 0:a4d8f5b3c546 152 * Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
lixianyu 0:a4d8f5b3c546 153 * when the transition from manual to auto occurs, the controller is
lixianyu 0:a4d8f5b3c546 154 * automatically initialized
lixianyu 0:a4d8f5b3c546 155 ******************************************************************************/
lixianyu 0:a4d8f5b3c546 156 void PID::SetMode(int Mode)
lixianyu 0:a4d8f5b3c546 157 {
lixianyu 0:a4d8f5b3c546 158 bool newAuto = (Mode == AUTOMATIC);
lixianyu 0:a4d8f5b3c546 159 if(newAuto == !inAuto) {
lixianyu 0:a4d8f5b3c546 160 /*we just went from manual to auto*/
lixianyu 0:a4d8f5b3c546 161 PID::Initialize();
lixianyu 0:a4d8f5b3c546 162 }
lixianyu 0:a4d8f5b3c546 163 inAuto = newAuto;
lixianyu 0:a4d8f5b3c546 164 }
lixianyu 0:a4d8f5b3c546 165
lixianyu 0:a4d8f5b3c546 166 /* Initialize()****************************************************************
lixianyu 0:a4d8f5b3c546 167 * does all the things that need to happen to ensure a bumpless transfer
lixianyu 0:a4d8f5b3c546 168 * from manual to automatic mode.
lixianyu 0:a4d8f5b3c546 169 ******************************************************************************/
lixianyu 0:a4d8f5b3c546 170 void PID::Initialize()
lixianyu 0:a4d8f5b3c546 171 {
lixianyu 0:a4d8f5b3c546 172 ITerm = *myOutput;
lixianyu 0:a4d8f5b3c546 173 lastInput = *myInput;
lixianyu 0:a4d8f5b3c546 174 ITerm = constrain(ITerm, errorMin, errorMax);
lixianyu 0:a4d8f5b3c546 175 }
lixianyu 0:a4d8f5b3c546 176
lixianyu 0:a4d8f5b3c546 177 /* SetControllerDirection(...)*************************************************
lixianyu 0:a4d8f5b3c546 178 * The PID will either be connected to a DIRECT acting process (+Output leads
lixianyu 0:a4d8f5b3c546 179 * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to
lixianyu 0:a4d8f5b3c546 180 * know which one, because otherwise we may increase the output when we should
lixianyu 0:a4d8f5b3c546 181 * be decreasing. This is called from the constructor.
lixianyu 0:a4d8f5b3c546 182 ******************************************************************************/
lixianyu 0:a4d8f5b3c546 183 void PID::SetControllerDirection(int Direction)
lixianyu 0:a4d8f5b3c546 184 {
lixianyu 0:a4d8f5b3c546 185 if(inAuto && Direction !=controllerDirection) {
lixianyu 0:a4d8f5b3c546 186 kp = (0 - kp);
lixianyu 0:a4d8f5b3c546 187 ki = (0 - ki);
lixianyu 0:a4d8f5b3c546 188 kd = (0 - kd);
lixianyu 0:a4d8f5b3c546 189 }
lixianyu 0:a4d8f5b3c546 190 controllerDirection = Direction;
lixianyu 0:a4d8f5b3c546 191 }
lixianyu 0:a4d8f5b3c546 192
lixianyu 0:a4d8f5b3c546 193 /* Status Funcions*************************************************************
lixianyu 0:a4d8f5b3c546 194 * Just because you set the Kp=-1 doesn't mean it actually happened. these
lixianyu 0:a4d8f5b3c546 195 * functions query the internal state of the PID. they're here for display
lixianyu 0:a4d8f5b3c546 196 * purposes. this are the functions the PID Front-end uses for example
lixianyu 0:a4d8f5b3c546 197 ******************************************************************************/
lixianyu 0:a4d8f5b3c546 198 double PID::GetKp()
lixianyu 0:a4d8f5b3c546 199 {
lixianyu 0:a4d8f5b3c546 200 return dispKp;
lixianyu 0:a4d8f5b3c546 201 }
lixianyu 0:a4d8f5b3c546 202 double PID::GetKi()
lixianyu 0:a4d8f5b3c546 203 {
lixianyu 0:a4d8f5b3c546 204 return dispKi;
lixianyu 0:a4d8f5b3c546 205 }
lixianyu 0:a4d8f5b3c546 206 double PID::GetKd()
lixianyu 0:a4d8f5b3c546 207 {
lixianyu 0:a4d8f5b3c546 208 return dispKd;
lixianyu 0:a4d8f5b3c546 209 }
lixianyu 0:a4d8f5b3c546 210 int PID::GetMode()
lixianyu 0:a4d8f5b3c546 211 {
lixianyu 0:a4d8f5b3c546 212 return inAuto ? AUTOMATIC : MANUAL;
lixianyu 0:a4d8f5b3c546 213 }
lixianyu 0:a4d8f5b3c546 214 int PID::GetDirection()
lixianyu 0:a4d8f5b3c546 215 {
lixianyu 0:a4d8f5b3c546 216 return controllerDirection;
lixianyu 0:a4d8f5b3c546 217 }
lixianyu 0:a4d8f5b3c546 218
lixianyu 0:a4d8f5b3c546 219