ported pid library from imu
Dependents: wheelchaircontrol wheelchaircontrolRos
Revision 1:45383b49536c, committed 2018-08-17
- Comitter:
- ryanlin97
- Date:
- Fri Aug 17 20:46:39 2018 +0000
- Parent:
- 0:539e3b3b91e1
- Commit message:
- adjust for jesus;
Changed in this revision
PID_v1.cpp | Show annotated file Show diff for this revision Revisions of this file |
PID_v1.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/PID_v1.cpp Thu Aug 09 16:36:32 2018 +0000 +++ b/PID_v1.cpp Fri Aug 17 20:46:39 2018 +0000 @@ -19,38 +19,24 @@ * 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* Input, double* Output, double* Setpoint, - double Kp, double Ki, double Kd, int POn, int ControllerDirection) -{ +PID::PID(double* Input, double* Output, double* Setpoint, double Kp, double Ki, double Kd, int POn, int ControllerDirection){ + PIDtimer.start(); + PIDtimer.reset(); myOutput = Output; myInput = Input; mySetpoint = Setpoint; - inAuto = false; - PIDtimer.start(); - PID::SetOutputLimits(0, 1); //default output limit corresponds to - //the arduino pwm limits - - SampleTime = 100; //default Controller Sample Time is 0.1 seconds + inAuto = true; + PID::SetOutputLimits(0, 1); //default output limit corresponds to + //the arduino pwm limits + SampleTime = .1; //default Controller Sample Time is 0.1 seconds PID::SetControllerDirection(ControllerDirection); PID::SetTunings(Kp, Ki, Kd, POn); lastTime = PIDtimer.read()-SampleTime; - printf("input %f output %f pOn %i Setpoint %f\n", *myInput, *myOutput, pOn, *mySetpoint); -} - -/*Constructor (...)********************************************************* - * To allow backwards compatability for v1.1, or for people that just want - * to use Proportional on Error without explicitly saying so - ***************************************************************************/ - -PID::PID(double* Input, double* Output, double* Setpoint, - double Kp, double Ki, double Kd, int ControllerDirection) -{ -PID::PID(Input, Output, Setpoint, Kp, Ki, Kd, P_ON_E, ControllerDirection); -} - - + // printf("the values are %f, %f, %f, %f, %f, %f \r\n", *myOutput, *myInput, *mySetpoint, kp, ki, kd); + } + /* 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 @@ -59,10 +45,9 @@ **********************************************************************************/ bool PID::Compute() { - printf("input %f output %f pOn %i Setpoint %f outmin %f outmax %f \n", *myInput, *myOutput, pOn, *mySetpoint, outMin, outMax); if(!inAuto) return false; - unsigned long now = PIDtimer.read(); - unsigned long timeChange = (now - lastTime); + double now = PIDtimer.read(); + double timeChange = (now - lastTime); if(timeChange>=SampleTime) { /*Compute all the working error variables*/ @@ -70,31 +55,33 @@ double error = *mySetpoint - input; double dInput = (input - lastInput); outputSum+= (ki * error); - + /*Add Proportional on Measurement, if P_ON_M is specified*/ - if(!pOnE) outputSum-= kp * dInput; - + if(!pOnE) outputSum -= kp * dInput; + printf("outputSum = %f \r\n", outputSum); if(outputSum > outMax) outputSum= outMax; else if(outputSum < outMin) outputSum= outMin; /*Add Proportional on Error, if P_ON_E is specified*/ - double output; + double output; if(pOnE) output = kp * error; else output = 0; - /*Compute Rest of PID Output*/ output += outputSum - kd * dInput; - if(output > outMax) output = outMax; + if(output > outMax) output = outMax; else if(output < outMin) output = outMin; - *myOutput = output; + *myOutput = output; /*Remember some variables for next time*/ lastInput = input; lastTime = now; - return true; + return true; } - else return false; + else + { + return false; + } } /* SetTunings(...)************************************************************* @@ -162,11 +149,11 @@ if(inAuto) { - if(*myOutput > outMax) *myOutput = outMax; - else if(*myOutput < outMin) *myOutput = outMin; + if(*myOutput > outMax) *myOutput = outMax; + else if(*myOutput < outMin) *myOutput = outMin; - if(outputSum > outMax) outputSum= outMax; - else if(outputSum < outMin) outputSum= outMin; + if(outputSum > outMax) outputSum= outMax; + else if(outputSum < outMin) outputSum= outMin; } } @@ -186,7 +173,7 @@ } /* Initialize()**************************************************************** - * does all the things that need to happen to ensure a bumpless transfer + * does all the things that need to happen to ensure a bumpless transfer * from manual to automatic mode. ******************************************************************************/ void PID::Initialize() @@ -207,7 +194,7 @@ { if(inAuto && Direction !=controllerDirection) { - kp = (0 - kp); + kp = (0 - kp); ki = (0 - ki); kd = (0 - kd); } @@ -223,10 +210,4 @@ double PID::GetKi(){ return dispKi;} double PID::GetKd(){ return dispKd;} int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} -int PID::GetDirection(){ return controllerDirection;} - -/*void printEverything() { - printf("input %f output %f pOn %f Setpoint %f outmin %f outmax %f \n", *myInput, *myOutput, pOn, *mySetpoint, outMin, outMax); - printf("KP %f KI %f Kd %f \n", PID::GetKp(), PID::GetKi(), PID::GetKd()); - } -*/ +int PID::GetDirection(){ return controllerDirection;} \ No newline at end of file
--- a/PID_v1.h Thu Aug 09 16:36:32 2018 +0000 +++ b/PID_v1.h Fri Aug 17 20:46:39 2018 +0000 @@ -22,9 +22,6 @@ PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and double, double, double, int, int);// Setpoint. Initial tuning parameters are also set here. // (overload for specifying proportional mode) - - PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and - double, double, double, int); // Setpoint. Initial tuning parameters are also set here void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0) @@ -53,10 +50,7 @@ void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which // the PID calculation is performed. default is 100 - - double *myInput; // * Pointers to the Input, Output, and Setpoint variables - double *myOutput; // This creates a hard link between the variables and the - double *mySetpoint; + //Display functions **************************************************************** double GetKp(); // These functions query the pid for interal values. @@ -79,9 +73,9 @@ int controllerDirection; int pOn; - // double *myInput; // * Pointers to the Input, Output, and Setpoint variables - // double *myOutput; // This creates a hard link between the variables and the - // double *mySetpoint; // PID, freeing the user from having to constantly tell us + double *myInput; // * Pointers to the Input, Output, and Setpoint variables + double *myOutput; // This creates a hard link between the variables and the + double *mySetpoint; // PID, freeing the user from having to constantly tell us // what these values are. with pointers we'll just know. unsigned long lastTime;