diff --git a/PID_Timed.cpp b/PID_Timed.cpp index 8a408e0..151bb44 100644 --- a/PID_Timed.cpp +++ b/PID_Timed.cpp @@ -162,3 +162,162 @@ double PID::GetKi() { return dispKi; } double PID::GetKd() { return dispKd; } bool PID::isEnabled() { return inAuto; } int PID::GetDirection() { return controllerDirection; } + +PID_FLOAT::PID_FLOAT(float* Input, float* Output, float* Setpoint, + float Kp, float Ki, float Kd, float referenceSampleTime, + int POn, int ControllerDirection) +{ + myOutput = Output; + myInput = Input; + mySetpoint = Setpoint; + inAuto = false; + + // Default output limit corresponds to the arduino pwm limits + PID_FLOAT::SetOutputLimits(0, 255); + + ReferenceSampleTime = referenceSampleTime; + + PID_FLOAT::SetControllerDirection(ControllerDirection); + PID_FLOAT::SetTunings(Kp, Ki, Kd, POn); +} + +PID_FLOAT::PID_FLOAT(float* Input, float* Output, float* Setpoint, + float Kp, float Ki, float Kd, float referenceSampleTime, int ControllerDirection) + :PID_FLOAT::PID_FLOAT(Input, Output, Setpoint, Kp, Ki, Kd, referenceSampleTime, P_ON_E, ControllerDirection) +{ +} + +// 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_FLOAT::Compute(float SampleTime) +{ + if(!inAuto) return false; + + float ratio = SampleTime / ReferenceSampleTime; + ki = dispKi * ratio; + kd = dispKd / ratio; + + // Compute all the working error variables + float input = *myInput; + float error = *mySetpoint - input; + float 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 + float 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; + return true; +} + +void PID_FLOAT::clearErrorIntegral() { + outputSum = 0; + lastInput = *myInput; +} + +void PID_FLOAT::SetTunings(float Kp, float Ki, float Kd, int POn) +{ + if (Kp<0 || Ki<0 || Kd<0) return; + + pOn = POn; + pOnE = POn == P_ON_E; + + kp = Kp; + ki = Ki; + kd = Kd; + + if (controllerDirection == REVERSE) + { + kp = (0 - kp); + ki = (0 - ki); + kd = (0 - kd); + } + dispKp = kp; dispKi = ki; dispKd = kd; +} + +void PID_FLOAT::SetTunings(float Kp, float Ki, float Kd) { + SetTunings(Kp, Ki, Kd, pOn); +} + +void PID_FLOAT::SetReferenceSampleTime(float SampleTime) +{ + ReferenceSampleTime = SampleTime; +} + +void PID_FLOAT::SetOutputLimits(float Min, float 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; + } +} + +void PID_FLOAT::enable(bool en) +{ + if (en && !inAuto) + { + PID_FLOAT::Initialize(); // we just went from manual to auto + } + inAuto = en; +} + +// Does all the things that need to happen to ensure a bumpless transfer +// from manual to automatic mode. +void PID_FLOAT::Initialize() +{ + outputSum = *myOutput; + lastInput = *myInput; + if(outputSum > outMax) outputSum = outMax; + else if(outputSum < outMin) outputSum = outMin; +} + +// 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_FLOAT::SetControllerDirection(int Direction) +{ + if(inAuto && Direction !=controllerDirection) + { + kp = (0 - kp); + ki = (0 - ki); + kd = (0 - kd); + } + controllerDirection = Direction; +} + +float PID_FLOAT::GetKp() { return dispKp; } +float PID_FLOAT::GetKi() { return dispKi; } +float PID_FLOAT::GetKd() { return dispKd; } +bool PID_FLOAT::isEnabled() { return inAuto; } +int PID_FLOAT::GetDirection() { return controllerDirection; } diff --git a/PID_Timed.h b/PID_Timed.h index 1782a2b..a52d5b3 100644 --- a/PID_Timed.h +++ b/PID_Timed.h @@ -83,4 +83,88 @@ class PID double ReferenceSampleTime; double outMin, outMax; bool inAuto, pOnE; +}; + +class PID_FLOAT +{ +public: + + static const int8_t DIRECT = 0; + static const int8_t REVERSE = 1; + static const int8_t P_ON_M = 0; + static const int8_t P_ON_E = 1; + + // Constructor. Links the PID to the Input, Output, and + // Setpoint. Initial tuning parameters are also set here. + // (overload for specifying proportional mode) + PID_FLOAT(float*, float*, float*, float, float, float, float, int, int); + + // Constructor. Links the PID to the Input, Output, and + // Setpoint. Initial tuning parameters are also set here. + PID_FLOAT(float*, float*, float*, float, float, float, float, int); + + // true Auto, false Manual + void enable(bool en); + + // Perform the PID calculation. It should be called every time loop() cycles + bool Compute(float SampleTimeSec); + + // Clamp output to a specific range. 0-255 by default, but + // it's likely the user will want to change this depending on the application + void SetOutputLimits(float, float); + + // While most users will set the tunings once in the + // constructor, this function gives the user the option + // of changing tunings during runtime for Adaptive control + void SetTunings(float, float, float); + + // Overload for specifying proportional mode + void SetTunings(float, float, float, int); + + // Set the Direction, or "Action" of the controller. DIRECT + // means the output will increase when error is positive. REVERSE + // means the opposite. it's very unlikely that this will be needed + // once it is set in the constructor. + void SetControllerDirection(int); + + // Sets the frequency, in Milliseconds, with which + // the PID calculation is performed. Default is 100. + void SetReferenceSampleTime(float); + + // Zero out integral of error + void clearErrorIntegral(); + + float GetKp(); + float GetKi(); + float GetKd(); + bool isEnabled(); // Pause, unpause calculations + int GetDirection(); + +private: + void Initialize(); + + float dispKp; + float dispKi; + float dispKd; + + float kp; // (P)roportional Tuning Parameter + float ki; // (I)ntegral Tuning Parameter + float kd; // (D)erivative Tuning Parameter + + int controllerDirection; + int pOn; + + // Pointers to the Input, Output, and Setpoint variables + // This creates a hard link between the variables and the + // PID, freeing the user from having to constantly tell us + // what these values are. with pointers we'll just know. + float *myInput; + float *myOutput; + float *mySetpoint; + + float outputSum, lastInput; + + float ReferenceSampleTime; + float outMin, outMax; + bool inAuto, pOnE; }; \ No newline at end of file diff --git a/examples/PID_Basic_float/PID_Basic_float.ino b/examples/PID_Basic_float/PID_Basic_float.ino new file mode 100644 index 0000000..f880a2a --- /dev/null +++ b/examples/PID_Basic_float/PID_Basic_float.ino @@ -0,0 +1,49 @@ +/******************************************************** + * PID Basic Example + * Reading analog input 0 to control analog PWM output 3 + ********************************************************/ + +#include + +#define PIN_INPUT 0 +#define PIN_OUTPUT 3 + +//Define Variables we'll be connecting to +float Setpoint, Input, Output; + +//Specify the links and initial tuning parameters +float Kp=2, Ki=5, Kd=1; +PID_FLOAT myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, 0.03, PID::DIRECT); // 30 msec target average sampling rate + +void setup() +{ + //initialize the variables we're linked to + Input = analogRead(PIN_INPUT); + Setpoint = 100; + + // Set new Kp, Ki, Kd at any time if necessary + // myPID.SetTunings(Kp, Ki, Kd); + + // Set PID output limits if necessary + // myPID.SetOutputLimits(-1, 1); + + myPID.enable(true); +} + +void loop() +{ + delay(10); + Input = analogRead(PIN_INPUT); + myPID.Compute(0.01); // 10 msec elapsed since last sample + analogWrite(PIN_OUTPUT, Output); + + delay(50); + Input = analogRead(PIN_INPUT); + myPID.Compute(0.05); // 50 msec elapsed since last sample + analogWrite(PIN_OUTPUT, Output); + + delay(35); + Input = analogRead(PIN_INPUT); + myPID.Compute(0.035); // 35 msec elapsed since last sample + analogWrite(PIN_OUTPUT, Output); +}