Float PID

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; }

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;
 };

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 <PID_Timed.h>
+
+#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);
+}