ported pid library from imu

Dependents:   wheelchaircontrol wheelchaircontrolRos

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?

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