a quadcopter code
Dependencies: Pulse RangeFinder mbed
PID.cpp
- Committer:
- Gendy
- Date:
- 2015-11-24
- Revision:
- 0:4a55d0a21ea9
File content as of revision 0:4a55d0a21ea9:
#include <PID.h> #include <stdlib.h> #include <math.h> /*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(float* Input, float* Output, float* Setpoint, float Kp, float Ki, float Kd) { myOutput = Output; myInput = Input; mySetpoint = Setpoint; PID::SetOutputLimits(-1000,1000); //default output limit corresponds to //the arduino pwm limits PID::SetTunings(Kp, Ki, Kd); //PID::SetIntegratorLimits(399); } /* 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. **********************************************************************************/ void PID::Compute() { float input = *myInput; float error =(*mySetpoint)-input; ITerm+= (error+lasterror)/2; /* if(!ki==0){ if(ITerm < (-abs(max/ki))){ ITerm= (-abs(max/ki)); } else if(ITerm > abs(max/ki)){ ITerm= abs(max/ki); } } */ float dInput = (error - lasterror); /*Compute PID Output*/ float output = (kp * error )+(ki * (ITerm))+(kd * (dInput)); if(output > outMax) output = outMax; else if(output < outMin) output = outMin; *myOutput =output; /*Remember some variables for next time*/ lasterror=error; } /* 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(float Kp, float Ki, float Kd) { kp = Kp; ki = Ki; kd = Kd; //lasterror=0; //ITerm=0; } /* 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(float Min, float Max) { if(Min >= Max) return; outMin = Min; outMax = Max; } /****************SetIntegratorLimits**********/ void PID::SetIntegratorLimits(float Max) { max=Max; }