diff --git a/PID_v1/PID_v1.cpp b/PID_v1/PID_v1.cpp index 6c95895..6c63016 100644 --- a/PID_v1/PID_v1.cpp +++ b/PID_v1/PID_v1.cpp @@ -32,9 +32,8 @@ PID::PID(double* Input, double* Output, double* Setpoint, SampleTime = 100; //default Controller Sample Time is 0.1 seconds PID::SetControllerDirection(ControllerDirection); + PID::SetResolution(MILLIS); // Use a resolution of milliseconds by default PID::SetTunings(Kp, Ki, Kd); - - lastTime = millis()-SampleTime; } @@ -47,17 +46,25 @@ PID::PID(double* Input, double* Output, double* Setpoint, bool PID::Compute() { if(!inAuto) return false; - unsigned long now = millis(); - unsigned long timeChange = (now - lastTime); - if(timeChange>=SampleTime) + unsigned long now = PID::GetTime(); + timeChange = (now - lastTime); + if(SampleTime == 0 || timeChange>=SampleTime) { /*Compute all the working error variables*/ double input = *myInput; double error = *mySetpoint - input; - ITerm+= (ki * error); + + double dInput; + if (SampleTime > 0) { + ITerm += (ki * error); + dInput = (input - lastInput); + } else { + ITerm += (ki * error)*(((double)timeChange)/secondsDivider); + dInput = (input - lastInput)/(((double)timeChange)/secondsDivider); + } + if(ITerm > outMax) ITerm= outMax; else if(ITerm < outMin) ITerm= outMin; - double dInput = (input - lastInput); /*Compute PID Output*/ double output = kp * error + ITerm- kd * dInput; @@ -85,11 +92,17 @@ void PID::SetTunings(double Kp, double Ki, double Kd) if (Kp<0 || Ki<0 || Kd<0) return; dispKp = Kp; dispKi = Ki; dispKd = Kd; - - double SampleTimeInSec = ((double)SampleTime)/1000; - kp = Kp; - ki = Ki * SampleTimeInSec; - kd = Kd / SampleTimeInSec; + + if (SampleTime > 0) { + double SampleTimeInSec = ((double)SampleTime)/secondsDivider; + kp = Kp; + ki = Ki * SampleTimeInSec; + kd = Kd / SampleTimeInSec; + } else { + kp = Kp; + ki = Ki; + kd = Kd; + } if(controllerDirection ==REVERSE) { @@ -100,18 +113,25 @@ void PID::SetTunings(double Kp, double Ki, double Kd) } /* SetSampleTime(...) ********************************************************* - * sets the period, in Milliseconds, at which the calculation is performed + * sets the period, in Milliseconds, at which the calculation is performed + * If it's set to 0 or a negative value it will computer every time the + * function is called ******************************************************************************/ void PID::SetSampleTime(int NewSampleTime) { if (NewSampleTime > 0) { - double ratio = (double)NewSampleTime - / (double)SampleTime; + double ratio; + if (SampleTime > 0) + ratio = (double)NewSampleTime/(double)SampleTime; + else + ratio = (double)NewSampleTime/(double)timeChange; // We will assume the user is calling Compute at a regular interval + ki *= ratio; kd /= ratio; SampleTime = (unsigned long)NewSampleTime; - } + } else + SampleTime = 0; // We will compute every time the function is called } /* SetOutputLimits(...)**************************************************** @@ -182,6 +202,29 @@ void PID::SetControllerDirection(int Direction) controllerDirection = Direction; } +/* GetTime()******************************************************************* + * Will get the current time either by using millis() or micros() + ******************************************************************************/ +unsigned long PID::GetTime() +{ + if (secondsDivider == 1000.0) return millis(); + return micros(); +} + +/* SetResolution(...)********************************************************** + * Will set the resolution of GetTime(). + * MILLIS will set the resolution to milliseconds while + * MICROS will set the resolution to microseconds. + ******************************************************************************/ +void PID::SetResolution(int resolution) +{ + if (resolution == MILLIS) + secondsDivider = 1000.0; + else + secondsDivider = 1000000.0; + lastTime = PID::GetTime()-SampleTime; // Update last time variable +} + /* 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 diff --git a/PID_v1/PID_v1.h b/PID_v1/PID_v1.h index 6f86697..a9a8005 100644 --- a/PID_v1/PID_v1.h +++ b/PID_v1/PID_v1.h @@ -13,6 +13,8 @@ class PID #define MANUAL 0 #define DIRECT 0 #define REVERSE 1 + #define MILLIS 0 + #define MICROS 1 //commonly used functions ************************************************************************** PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and @@ -41,6 +43,9 @@ class PID // once it is set in the constructor. void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which // the PID calculation is performed. default is 100 + void SetResolution(int); // * Set the resolution of the GetTime() function. + // MILLIS sets the resolution to milliseconds. + // MICROS sets the resolution to microseconds. @@ -53,6 +58,8 @@ class PID private: void Initialize(); + unsigned long GetTime(); // * This will call either millis() or micros() + // depending on the used resolution. double dispKp; // * we'll hold on to the tuning parameters in user-entered double dispKi; // format for display purposes @@ -71,8 +78,10 @@ class PID unsigned long lastTime; double ITerm, lastInput; + unsigned long timeChange; unsigned long SampleTime; + double secondsDivider; double outMin, outMax; bool inAuto; }; diff --git a/PID_v1/keywords.txt b/PID_v1/keywords.txt index 55969c1..3cb776b 100644 --- a/PID_v1/keywords.txt +++ b/PID_v1/keywords.txt @@ -18,6 +18,7 @@ SetOutputLimits KEYWORD2 SetTunings KEYWORD2 SetControllerDirection KEYWORD2 SetSampleTime KEYWORD2 +SetResolution KEYWORD2 GetKp KEYWORD2 GetKi KEYWORD2 GetKd KEYWORD2 @@ -31,4 +32,6 @@ GetDirection KEYWORD2 AUTOMATIC LITERAL1 MANUAL LITERAL1 DIRECT LITERAL1 -REVERSE LITERAL1 \ No newline at end of file +REVERSE LITERAL1 +MILLIS LITERAL1 +MICROS LITERAL1 \ No newline at end of file