/********************************************************************************************** * Arduino PID Library - Version 1.0.2 * by Brett Beauregard brettbeauregard.com * * Modified by Jason Melvin to include feedforward and adjustable windup * * This Code is licensed under a Creative Commons Attribution-ShareAlike 3.0 Unported License. **********************************************************************************************/ #if ARDUINO >= 100 #include "Arduino.h" #else #include #endif #include /*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* Input, double* Output, double* Setpoint, double Kp, double Ki, double Kd, double Kf, int ControllerDirection) { PID::SetOutputLimits(0, 255); //default output limit corresponds to //the arduino pwm limits windupI = outMax; // default windup limit is outMax sampleTime = 100; // default Controller Sample Time is 0.1 seconds ff_zero = 0; // default feedforward zero point is 0.0 (for temp control, set to ambient) PID::SetControllerDirection(ControllerDirection); PID::SetTunings(Kp, Ki, Kd, Kf); lastTime = millis()-sampleTime; inAuto = false; myOutput = Output; myInput = Input; mySetpoint = Setpoint; } /* 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 **********************************************************************************/ void PID::Compute() { if(!inAuto) return; unsigned long now = millis(); unsigned long timeChange = (now - lastTime); if(timeChange>=sampleTime) { /*Compute all the working error variables*/ double input = *myInput; // read current input condition double error = *mySetpoint - input; // error is difference between setpoint and input iTerm+= (ki * error); // add additional error to the integral term iTerm = constrain( iTerm , -windupI , windupI ); // limit the integral term to +/- windup parameter double dInput = (input - lastInput); // derivative is based on change in the input /*Compute PID Output*/ double output = kp * error + iTerm - kd * dInput + kf * (*mySetpoint - ff_zero); output = constrain( output , outMin , outMax ); // limit output to within min/max *myOutput = output; // write the current output /*Remember some variables for next time*/ lastInput = input; // last input is for computing the derivative term lastTime = now; // last time is for determining when to recompute } } /* 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, double Kf) { if (Kp<0 || Ki<0 || Kd<0 || Kf<0) return; dispKp = Kp; dispKi = Ki; dispKd = Kd; dispKf = Kf; double sampleTimeInSec = ((double)sampleTime)/1000; kp = Kp; ki = Ki * sampleTimeInSec; kd = Kd / sampleTimeInSec; kf = Kf; if(controllerDirection ==REVERSE) { kp = (0 - kp); ki = (0 - ki); kd = (0 - kd); kf = (0 - kf); } } /* 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; } } /* SetWindupI(...)******************** * Sets the windup limit for the integral term, * which is otherwise limited to outMax ***************************************/ void PID::SetWindupI(double limit) { if (limit > 0) windupI = limit; } /* SetFF_zero(...)******************** * Sets the zero point for the feedforward term, * which is otherwise 0.0 ***************************************/ void PID::SetFFzero(double zeropoint) { ff_zero = zeropoint; } /* 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; if (outMax) windupI *= Max / outMax; outMin = Min; outMax = Max; if(inAuto) { if(*myOutput > outMax) *myOutput = outMax; else if(*myOutput < outMin) *myOutput = outMin; if(iTerm > outMax) iTerm= outMax; else if(iTerm < outMin) iTerm= outMin; } } /* 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; if(iTerm > windupI) iTerm = windupI; else if(iTerm < outMin) iTerm = outMin; } /* 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); kf = (0 - kf); } 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;} double PID::GetKf(){ return dispKf;} double PID::GetWi(){ return windupI;} double PID::GetFFzero(){ return ff_zero;} int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} int PID::GetDirection(){ return controllerDirection;}