336 lines
11 KiB
C++
336 lines
11 KiB
C++
/**********************************************************************************************
|
|
* Arduino PID Library - Version 1.2.1
|
|
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
|
|
*
|
|
* This Library is licensed under the MIT License
|
|
**********************************************************************************************/
|
|
|
|
#if ARDUINO >= 100
|
|
#include "Arduino.h"
|
|
#else
|
|
#include "WProgram.h"
|
|
#endif
|
|
|
|
#include "PID_v1.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(double* Input, double* Output, double* Setpoint,
|
|
double Kp, double Ki, double Kd, int POn, int ControllerDirection) {
|
|
myOutput = Output;
|
|
myInput = Input;
|
|
mySetpoint = Setpoint;
|
|
inAuto = false;
|
|
//Tuner = new PID_ATune(Input, Output);
|
|
PID::OutputLimits(0, 100);
|
|
PID::Direction(ControllerDirection);
|
|
PID::SetTunings(Kp, Ki, Kd, POn);
|
|
|
|
// Autotune defaults
|
|
noise_band = 0.5;
|
|
autotune_running = false;
|
|
oStep = 30;
|
|
LookbackSec(10);
|
|
|
|
SampleTime = 100; //default Controller Sample Time is 0.1 seconds
|
|
lastTime = millis()-SampleTime;
|
|
}
|
|
|
|
/*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) {
|
|
|
|
}
|
|
|
|
|
|
/* 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*/
|
|
double input = *myInput;
|
|
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(outputSum > outMax) outputSum= outMax;
|
|
else if(outputSum < outMin) outputSum= outMin;
|
|
|
|
/*Add Proportional on Error, if P_ON_E is specified*/
|
|
double output;
|
|
if(pOnE) output = kp * error;
|
|
else output = 0;
|
|
|
|
/*Compute Rest of PID Output*/
|
|
output += outputSum - kd * dInput;
|
|
|
|
if(output > outMax) output = outMax;
|
|
else if(output < outMin) output = outMin;
|
|
*myOutput = output;
|
|
|
|
/*Remember some variables for next time*/
|
|
lastInput = input;
|
|
lastTime = now;
|
|
return true;
|
|
}
|
|
else return false;
|
|
}
|
|
|
|
int PID::ComputeTune() {
|
|
//~ justevaled=false;
|
|
if(peakCount>9 && autotune_running) {
|
|
autotune_running = false;
|
|
FinishUp();
|
|
return 1;
|
|
}
|
|
unsigned long now = millis();
|
|
|
|
if((now-lastTime)<SampleTime) return false;
|
|
|
|
lastTime = now;
|
|
double refVal = *myInput;
|
|
//~ justevaled=true;
|
|
if(!autotune_running) {
|
|
//initialize working variables the first time around
|
|
peakType = 0;
|
|
peakCount=0;
|
|
atune_peak_change=false;
|
|
absMax=refVal;
|
|
absMin=refVal;
|
|
*mySetpoint = refVal;
|
|
autotune_running = true;
|
|
outputStart = *myOutput;
|
|
*myOutput = outputStart+oStep;
|
|
} else {
|
|
if(refVal>absMax)absMax=refVal;
|
|
if(refVal<absMin)absMin=refVal;
|
|
}
|
|
|
|
//oscillate the output base on the input's relation to the setpoint
|
|
|
|
if(refVal>*mySetpoint+noise_band) *myOutput = outputStart-oStep;
|
|
else if (refVal<*mySetpoint-noise_band) *myOutput = outputStart+oStep;
|
|
|
|
|
|
//bool isMax=true, isMin=true;
|
|
isMax=true;isMin=true;
|
|
//id peaks
|
|
for(int i=nLookBack-1;i>=0;i--) {
|
|
double val = lastInputs[i];
|
|
if(isMax) isMax = refVal>val;
|
|
if(isMin) isMin = refVal<val;
|
|
lastInputs[i+1] = lastInputs[i];
|
|
}
|
|
lastInputs[0] = refVal;
|
|
if(nLookBack<9){
|
|
//we don't want to trust the maxes or mins until the inputs array has been filled
|
|
return 0;
|
|
}
|
|
|
|
if(isMax) {
|
|
if(peakType==0)peakType=1;
|
|
if(peakType==-1) {
|
|
peakType = 1;
|
|
atune_peak_change=true;
|
|
peak2 = peak1;
|
|
}
|
|
peak1 = now;
|
|
peaks[peakCount] = refVal;
|
|
|
|
} else if(isMin) {
|
|
if(peakType==0)peakType=-1;
|
|
if(peakType==1){
|
|
peakType=-1;
|
|
peakCount++;
|
|
atune_peak_change=true;
|
|
}
|
|
|
|
if(peakCount<10)peaks[peakCount] = refVal;
|
|
}
|
|
|
|
if(atune_peak_change && peakCount>2) {
|
|
//we've transitioned. check if we can autotune based on the last peaks
|
|
double avgSeparation = (abs(peaks[peakCount-1]-peaks[peakCount-2])+abs(peaks[peakCount-2]-peaks[peakCount-3]))/2;
|
|
if( avgSeparation < 0.05*(absMax-absMin)){
|
|
FinishUp();
|
|
autotune_running = false;
|
|
return 1;
|
|
|
|
}
|
|
}
|
|
atune_peak_change=false;
|
|
return 0;
|
|
}
|
|
|
|
void PID::FinishUp(){
|
|
*myOutput = outputStart;
|
|
//we can generate tuning parameters!
|
|
Ku = 4*(2*oStep)/((absMax-absMin)*3.14159);
|
|
Pu = (double)(peak1-peak2) / 1000;
|
|
}
|
|
|
|
/* 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, int POn){
|
|
if (Kp<0 || Ki<0 || Kd<0) return;
|
|
|
|
pOn = POn;
|
|
pOnE = POn == P_ON_E;
|
|
|
|
double SampleTimeInSec = ((double)SampleTime)/1000;
|
|
kp = Kp;
|
|
ki = Ki * SampleTimeInSec;
|
|
kd = Kd / SampleTimeInSec;
|
|
|
|
if(controllerDirection ==REVERSE){
|
|
kp = (0 - kp);
|
|
ki = (0 - ki);
|
|
kd = (0 - kd);
|
|
}
|
|
}
|
|
|
|
/* SetTunings(...)*************************************************************
|
|
* Set Tunings using the last-rembered POn setting
|
|
******************************************************************************/
|
|
void PID::SetTunings(double Kp, double Ki, double Kd){
|
|
SetTunings(Kp, Ki, Kd, pOn);
|
|
}
|
|
|
|
/* 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;
|
|
}
|
|
}
|
|
|
|
/* OutputLimits(...)*******************************************************
|
|
* 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::OutputLimits(double Min, double Max){
|
|
if(Min >= Max) return;
|
|
outMin = Min;
|
|
outMax = Max;
|
|
|
|
if(inAuto){
|
|
if(*myOutput > outMax) *myOutput = outMax;
|
|
else if(*myOutput < outMin) *myOutput = outMin;
|
|
|
|
if(outputSum > outMax) outputSum= outMax;
|
|
else if(outputSum < outMin) outputSum= outMin;
|
|
}
|
|
}
|
|
|
|
/* Mode(...)*******************************************************************
|
|
* 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::Mode(int Mode){
|
|
bool newAuto = (Mode == 1);
|
|
if(newAuto && !inAuto){ /*we just went from manual to auto*/
|
|
PID::Initialize();
|
|
}
|
|
inAuto = newAuto;
|
|
OpMode = Mode;
|
|
}
|
|
|
|
int PID::CycleMode() {
|
|
if (OpMode + 1 == 3) {
|
|
PID::Mode(0);
|
|
} else {
|
|
PID::Mode(OpMode + 1);
|
|
}
|
|
|
|
return OpMode;
|
|
}
|
|
|
|
/* Initialize()****************************************************************
|
|
* does all the things that need to happen to ensure a bumpless transfer
|
|
* from manual to automatic mode.
|
|
******************************************************************************/
|
|
void PID::Initialize(){
|
|
outputSum = *myOutput;
|
|
lastInput = *myInput;
|
|
if(outputSum > outMax) outputSum = outMax;
|
|
else if(outputSum < outMin) outputSum = outMin;
|
|
}
|
|
|
|
/* Direction(...)**************************************************************
|
|
* 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::Direction(int Direction){
|
|
if(inAuto && Direction !=controllerDirection){
|
|
kp = (0 - kp);
|
|
ki = (0 - ki);
|
|
kd = (0 - kd);
|
|
}
|
|
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::Kp(){ return kp; }
|
|
double PID::Ki(){ return ki;}
|
|
double PID::Kd(){ return kd;}
|
|
modes PID::Mode(){ return OpMode;}
|
|
int PID::Direction(){ return controllerDirection;}
|
|
void PID::Cancel(){ autotune_running = false;}
|
|
double PID::TunedKp(){ return 0.6 * Ku;}
|
|
double PID::TunedKi(){ return 1.2*Ku / Pu ;} // Ki = Kc/Ti}
|
|
double PID::TunedKd(){ return 0.075 * Ku * Pu;} //Kd = Kc * Td}
|
|
void PID::OutputStep(double Step){ oStep = Step;}
|
|
double PID::OutputStep(){ return oStep;}
|
|
void PID::NoiseBand(double Band){ noise_band = Band;}
|
|
double PID::NoiseBand(){ return noise_band;}
|
|
|
|
void PID::LookbackSec(int value){
|
|
if (value<1) value = 1;
|
|
|
|
if(value<25) {
|
|
nLookBack = value * 4;
|
|
SampleTime = 250;
|
|
} else {
|
|
nLookBack = 100;
|
|
SampleTime = value*10;
|
|
}
|
|
}
|
|
|
|
int PID::LookbackSec(){ return nLookBack * SampleTime / 1000;}
|