This is a copy of the Reference Standard PID controller ala controlguru.com
Fork of PID by
Diff: PID.cpp
- Revision:
- 3:316f974b7f98
- Parent:
- 2:55bf0f813bb4
diff -r 55bf0f813bb4 -r 316f974b7f98 PID.cpp --- a/PID.cpp Wed Jan 27 21:32:11 2016 +0000 +++ b/PID.cpp Sun Feb 07 19:06:37 2016 +0000 @@ -1,10 +1,9 @@ /** - * @author Aaron Berk + * Arduino PID Library - Version 1.1.1 + * @author Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com * * @section LICENSE * - * Copyright (c) 2010 ARM Limited - * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights @@ -30,7 +29,7 @@ * * This library is a port of Brett Beauregard's Arduino PID library: * - * http://www.arduino.cc/playground/Code/PIDLibrary + * https://github.com/br3ttb/Arduino-PID-Library * * The wikipedia article on PID controllers is a good place to start on * understanding how they work: @@ -48,290 +47,189 @@ * Includes */ #include "PID.h" - -PID::PID(float Kc, float tauI, float tauD, float interval) { - - usingFeedForward = false; - inAuto = false; - - //Default the limits to the full range of I/O: 3.3V - //Make sure to set these to more appropriate limits for - //your application. - setInputLimits(0.0, 3.3); - setOutputLimits(0.0, 3.3); - - tSample_ = interval; - - setTunings(Kc, tauI, tauD); - - setPoint_ = 0.0; - processVariable_ = 0.0; - prevProcessVariable_ = 0.0; - controllerOutput_ = 0.0; - prevControllerOutput_ = 0.0; - - accError_ = 0.0; - bias_ = 0.0; - - realOutput_ = 0.0; - -} - -void PID::setInputLimits(float inMin, float inMax) { +#include "millis/millis.h" - //Make sure we haven't been given impossible values. - if (inMin >= inMax) { - return; - } - - //Rescale the working variables to reflect the changes. - prevProcessVariable_ *= (inMax - inMin) / inSpan_; - accError_ *= (inMax - inMin) / inSpan_; - - //Make sure the working variables are within the new limits. - if (prevProcessVariable_ > 1) { - prevProcessVariable_ = 1; - } else if (prevProcessVariable_ < 0) { - prevProcessVariable_ = 0; - } - - inMin_ = inMin; - inMax_ = inMax; - inSpan_ = inMax - inMin; - -} - -void PID::setOutputLimits(float outMin, float outMax) { - - //Make sure we haven't been given impossible values. - if (outMin >= outMax) { - return; - } - - //Rescale the working variables to reflect the changes. - prevControllerOutput_ *= (outMax - outMin) / outSpan_; - - //Make sure the working variables are within the new limits. - if (prevControllerOutput_ > 1) { - prevControllerOutput_ = 1; - } else if (prevControllerOutput_ < 0) { - prevControllerOutput_ = 0; - } +extern Serial pc; - outMin_ = outMin; - outMax_ = outMax; - outSpan_ = outMax - outMin; - -} - -void PID::setTunings(float Kc, float tauI, float tauD) { - - //Verify that the tunings make sense. - if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) { - return; - } - - //Store raw values to hand back to user on request. - pParam_ = Kc; - iParam_ = tauI; - dParam_ = tauD; - - float tempTauR; - - if (tauI == 0.0) { - tempTauR = 0.0; - } else { - tempTauR = (1.0 / tauI) * tSample_; - } - - //For "bumpless transfer" we need to rescale the accumulated error. - if (inAuto) { - if (tempTauR == 0.0) { - accError_ = 0.0; - } else { - accError_ *= (Kc_ * tauR_) / (Kc * tempTauR); - } - } - - Kc_ = Kc; - tauR_ = tempTauR; - tauD_ = tauD / tSample_; - -} - -void PID::reset(void) { - - float scaledBias = 0.0; - - if (usingFeedForward) { - scaledBias = (bias_ - outMin_) / outSpan_; - } else { - scaledBias = (realOutput_ - outMin_) / outSpan_; - } - - prevControllerOutput_ = scaledBias; - prevProcessVariable_ = (processVariable_ - inMin_) / inSpan_; - - //Clear any error in the integral. - accError_ = 0; - -} - -void PID::setMode(int mode) { - - //We were in manual, and we just got set to auto. - //Reset the controller internals. - if (mode != 0 && !inAuto) { - reset(); - } - - inAuto = (mode != 0); - + +/*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, int ControllerDirection) +{ + + myOutput = Output; + myInput = Input; + mySetpoint = Setpoint; + inAuto = false; + + PID::SetOutputLimits(0, 1000); // default output limit corresponds to + // the arduino pwm limits + SampleTime = 100; // default Controller Sample Time is 0.1 seconds + + PID::SetControllerDirection(ControllerDirection); + PID::SetTunings(Kp, Ki, Kd); + + lastTime = millis()-SampleTime; } -void PID::setInterval(float interval) { - if (interval > 0) { - //Convert the time-based tunings to reflect this change. - tauR_ *= (interval / tSample_); - accError_ *= (tSample_ / interval); - tauD_ *= (interval / tSample_); - tSample_ = interval; - } +/* 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. + **********************************************************************************/ +bool PID::Compute() +{ + if(!inAuto) return false; + unsigned long now = millis(); + unsigned long timeChange = (now - lastTime); + if(timeChange>=SampleTime) + { + /*Compute all the working error variables*/ + float input = *myInput; + float error = *mySetpoint - input; + ITerm+= (ki * error); + if(ITerm > outMax) ITerm= outMax; + else if(ITerm < outMin) ITerm= outMin; + float dInput = (input - lastInput); +// pc.printf("Input = %f, Error = %f, Output = %f, SetPoint = %f\n\r",input,error,output,*mySetpoint); -} - -void PID::setSetPoint(float sp) { + /*Compute PID Output*/ + float output = kp * error + ITerm- kd * dInput; +// pc.printf("Input = %f, Error = %f, Output = %f, SetPoint = %f\n\r",input,error,output,*mySetpoint); + + if(output > outMax) output = outMax; + else if(output < outMin) output = outMin; + *myOutput = output; - setPoint_ = sp; - + /*Remember some variables for next time*/ + lastInput = input; + lastTime = now; + return true; + } + else return false; } - -void PID::setProcessValue(float pv) { - - processVariable_ = pv; + + +/* 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) +{ + if (Kp<0 || Ki<0 || Kd<0) return; -} - -void PID::setBias(float bias){ + dispKp = Kp; dispKi = Ki; dispKd = Kd; + + float SampleTimeInSec = ((float)SampleTime)/1000; + kp = Kp; + ki = Ki * SampleTimeInSec; + kd = Kd / SampleTimeInSec; - bias_ = bias; - usingFeedForward = 1; - + 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) + { + float ratio = (float)NewSampleTime + / (float)SampleTime; + ki *= ratio; + kd /= ratio; + SampleTime = (unsigned long)NewSampleTime; + } } -float PID::compute() { - - //Pull in the input and setpoint, and scale them into percent span. - float scaledPV = (processVariable_ - inMin_) / inSpan_; - - if (scaledPV > 1.0) { - scaledPV = 1.0; - } else if (scaledPV < 0.0) { - scaledPV = 0.0; - } - - float scaledSP = (setPoint_ - inMin_) / inSpan_; - if (scaledSP > 1.0) { - scaledSP = 1; - } else if (scaledSP < 0.0) { - scaledSP = 0; - } - - float error = scaledSP - scaledPV; - - //Check and see if the output is pegged at a limit and only - //integrate if it is not. This is to prevent reset-windup. - if (!(prevControllerOutput_ >= 1 && error > 0) && !(prevControllerOutput_ <= 0 && error < 0)) { - accError_ += error; - } +/* 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; - //Compute the current slope of the input signal. - float dMeas = (scaledPV - prevProcessVariable_) / tSample_; - - float scaledBias = 0.0; - - if (usingFeedForward) { - scaledBias = (bias_ - outMin_) / outSpan_; + 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(); } - - //Perform the PID calculation. - controllerOutput_ = scaledBias + Kc_ * (error + (tauR_ * accError_) - (tauD_ * dMeas)); - - //Make sure the computed output is within output constraints. - if (controllerOutput_ < 0.0) { - controllerOutput_ = 0.0; - } else if (controllerOutput_ > 1.0) { - controllerOutput_ = 1.0; - } - - //Remember this output for the windup check next time. - prevControllerOutput_ = controllerOutput_; - //Remember the input for the derivative calculation next time. - prevProcessVariable_ = scaledPV; - - //Scale the output from percent span back out to a real world number. - return ((controllerOutput_ * outSpan_) + outMin_); - + inAuto = newAuto; } -float PID::getInMin() { - - return inMin_; - -} - -float PID::getInMax() { - - return inMax_; - -} - -float PID::getOutMin() { - - return outMin_; - -} - -float PID::getOutMax() { - - return outMax_; - -} - -float PID::getInterval() { - - return tSample_; - +/* 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 > outMax) ITerm = outMax; + else if(ITerm < outMin) ITerm = outMin; } - -float PID::getPParam() { - - return pParam_; - -} - -float PID::getIParam() { - - return iParam_; - + +/* 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; } - -float PID::getDParam() { - - return dParam_; - -} - - - - - - - +/* 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 + ******************************************************************************/ +float PID::GetKp(){ return dispKp; } +float PID::GetKi(){ return dispKi;} +float PID::GetKd(){ return dispKd;} +int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} +int PID::GetDirection(){ return controllerDirection;} - - - -