Li Weiyi
/
BalanceCar
虽然移植完毕,但是不work。需要细调……
Diff: PID_v2.cpp
- Revision:
- 0:a4d8f5b3c546
- Child:
- 2:99785a1007a4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID_v2.cpp Sat Jun 04 03:16:52 2016 +0000 @@ -0,0 +1,220 @@ +/********************************************************************************************** + * Arduino PID Library - Version 1.1.1 + * by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com + * + * This Library is licensed under a GPLv3 License + **********************************************************************************************/ +#if 0 +#if ARDUINO >= 100 +#include "Arduino.h" +#else +#include "WProgram.h" +#endif +#endif +#include "mbed.h" +#include <PID_v2.h> + +extern Timer g_Timer; +#define constrain(x,a,b) ((x) < (a) ? (a) : ((x) > (b) ? (b) : (x))) + +/*Constructor (...)********************************************************* + * The parameters specified here are those for for which we can't set up + * reliable defaults, so we need to have the user set them. + ***************************************************************************/ +PID::PID(double Kp, double Ki, double Kd, int ControllerDirection) +{ + +// myOutput = Output; +// myInput = Input; +// mySetpoint = Setpoint; + inAuto = false; + + PID::SetOutputLimits(0, 255); //default output limit corresponds to + PID::SetITermLimits(0, 255); + //the arduino pwm limits + + SampleTime = 1; //default Controller Sample Time is 0.1 seconds + + PID::SetControllerDirection(ControllerDirection); + PID::SetTunings(Kp, Ki, Kd); + + //lastTime = micros()/1000 - SampleTime; + lastTime = g_Timer.read_us() / 1000 - SampleTime; +} + + +/* Compute() ********************************************************************** + * This, as they say, is where the magic happens. this function should be called + * every time "void loop()" executes. the function will decide for itself whether a new + * pid Output needs to be computed. returns true when the output is computed, + * false when nothing has been done. + **********************************************************************************/ +float PID::Compute(float Input, float Setpoint) +{ + float output = 0; + if(!inAuto) return output; + //unsigned long now = micros(); + unsigned long now = g_Timer.read_us(); + float dt = (now - lastTime)/1000.0; + + if(dt>=SampleTime) { + /*Compute all the working error variables*/ + float input = Input; + float error = Setpoint - input; + ITerm += error * dt; + ITerm = constrain(ITerm, errorMin, errorMax); + float dInput = (input - lastInput)/dt; + + /*Compute PID Output*/ + output = kp * error + ki * ITerm - kd * dInput; + + output = constrain(output, outMin, outMax); +// *myOutput = output; + + /*Remember some variables for next time*/ + lastInput = input; + lastTime = now; + // return true; + } + return output; +} + + +/* SetTunings(...)************************************************************* + * This function allows the controller's dynamic performance to be adjusted. + * it's called automatically from the constructor, but tunings can also + * be adjusted on the fly during normal operation + ******************************************************************************/ +void PID::SetTunings(double Kp, double Ki, double Kd) +{ + if (Kp<0 || Ki<0 || Kd<0) return; + + + + double SampleTimeInSec = ((double)SampleTime)/1000; + kp = Kp; + ki = Ki; + kd = Kd; + + dispKp = Kp; + dispKi = Ki; + dispKd = Kd; + + if(controllerDirection ==REVERSE) { + kp = (0 - kp); + ki = (0 - ki); + kd = (0 - kd); + } +} + +/* SetSampleTime(...) ********************************************************* + * sets the period, in Milliseconds, at which the calculation is performed + ******************************************************************************/ +void PID::SetSampleTime(int NewSampleTime) +{ + if (NewSampleTime > 0) { + double ratio = (double)NewSampleTime + / (double)SampleTime; + ki *= ratio; + kd /= ratio; + SampleTime = (unsigned long)NewSampleTime; + } +} + +/* SetOutputLimits(...)**************************************************** + * This function will be used far more often than SetInputLimits. while + * the input to the controller will generally be in the 0-1023 range (which is + * the default already,) the output will be a little different. maybe they'll + * be doing a time window and will need 0-8000 or something. or maybe they'll + * want to clamp it from 0-125. who knows. at any rate, that can all be done + * here. + **************************************************************************/ +void PID::SetOutputLimits(double Min, double Max) +{ + if(Min >= Max) return; + outMin = Min; + outMax = Max; + + if(inAuto) + *myOutput = constrain(*myOutput, outMin, outMax); +} + +void PID::SetITermLimits(double Min, double Max) +{ + if(Min >= Max) return; + errorMin = Min; + errorMax = Max; + + if(inAuto) + ITerm = constrain(ITerm, errorMin, errorMax); +} + +/* SetMode(...)**************************************************************** + * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) + * when the transition from manual to auto occurs, the controller is + * automatically initialized + ******************************************************************************/ +void PID::SetMode(int Mode) +{ + bool newAuto = (Mode == AUTOMATIC); + if(newAuto == !inAuto) { + /*we just went from manual to auto*/ + PID::Initialize(); + } + inAuto = newAuto; +} + +/* Initialize()**************************************************************** + * does all the things that need to happen to ensure a bumpless transfer + * from manual to automatic mode. + ******************************************************************************/ +void PID::Initialize() +{ + ITerm = *myOutput; + lastInput = *myInput; + ITerm = constrain(ITerm, errorMin, errorMax); +} + +/* SetControllerDirection(...)************************************************* + * The PID will either be connected to a DIRECT acting process (+Output leads + * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to + * know which one, because otherwise we may increase the output when we should + * be decreasing. This is called from the constructor. + ******************************************************************************/ +void PID::SetControllerDirection(int Direction) +{ + if(inAuto && Direction !=controllerDirection) { + kp = (0 - kp); + ki = (0 - ki); + kd = (0 - kd); + } + controllerDirection = Direction; +} + +/* Status Funcions************************************************************* + * Just because you set the Kp=-1 doesn't mean it actually happened. these + * functions query the internal state of the PID. they're here for display + * purposes. this are the functions the PID Front-end uses for example + ******************************************************************************/ +double PID::GetKp() +{ + return dispKp; +} +double PID::GetKi() +{ + return dispKi; +} +double PID::GetKd() +{ + return dispKd; +} +int PID::GetMode() +{ + return inAuto ? AUTOMATIC : MANUAL; +} +int PID::GetDirection() +{ + return controllerDirection; +} + +