Diff: PID_v2.cpp

Revision:

0:a4d8f5b3c546

Child:

2:99785a1007a4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID_v2.cpp	Sat Jun 04 03:16:52 2016 +0000
@@ -0,0 +1,220 @@
+/**********************************************************************************************
+ * Arduino PID Library - Version 1.1.1
+ * by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
+ *
+ * This Library is licensed under a GPLv3 License
+ **********************************************************************************************/
+#if 0
+#if ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+#endif
+#include "mbed.h"
+#include <PID_v2.h>
+
+extern Timer g_Timer;
+#define constrain(x,a,b) ((x) < (a) ? (a) : ((x) > (b) ? (b) : (x)))
+
+/*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 Kp, double Ki, double Kd, int ControllerDirection)
+{
+
+//   myOutput = Output;
+//   myInput = Input;
+//   mySetpoint = Setpoint;
+    inAuto = false;
+
+    PID::SetOutputLimits(0, 255);				//default output limit corresponds to
+    PID::SetITermLimits(0, 255);
+    //the arduino pwm limits
+
+    SampleTime = 1;							//default Controller Sample Time is 0.1 seconds
+
+    PID::SetControllerDirection(ControllerDirection);
+    PID::SetTunings(Kp, Ki, Kd);
+
+    //lastTime = micros()/1000 - SampleTime;
+    lastTime = g_Timer.read_us() / 1000 - SampleTime;
+}
+
+
+/* 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.
+ **********************************************************************************/
+float PID::Compute(float Input, float Setpoint)
+{
+    float output = 0;
+    if(!inAuto) return output;
+    //unsigned long now = micros();
+    unsigned long now = g_Timer.read_us();
+    float dt = (now - lastTime)/1000.0;
+
+    if(dt>=SampleTime) {
+        /*Compute all the working error variables*/
+        float input = Input;
+        float error = Setpoint - input;
+        ITerm += error * dt;
+        ITerm = constrain(ITerm, errorMin, errorMax);
+        float dInput = (input - lastInput)/dt;
+
+        /*Compute PID Output*/
+        output = kp * error + ki * ITerm - kd * dInput;
+
+        output = constrain(output, outMin, outMax);
+//	  *myOutput = output;
+
+        /*Remember some variables for next time*/
+        lastInput = input;
+        lastTime = now;
+        //  return true;
+    }
+    return output;
+}
+
+
+/* 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)
+{
+    if (Kp<0 || Ki<0 || Kd<0) return;
+
+
+
+    double SampleTimeInSec = ((double)SampleTime)/1000;
+    kp = Kp;
+    ki = Ki;
+    kd = Kd;
+
+    dispKp = Kp;
+    dispKi = Ki;
+    dispKd = Kd;
+
+    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) {
+        double ratio  = (double)NewSampleTime
+                        / (double)SampleTime;
+        ki *= ratio;
+        kd /= ratio;
+        SampleTime = (unsigned long)NewSampleTime;
+    }
+}
+
+/* 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(double Min, double Max)
+{
+    if(Min >= Max) return;
+    outMin = Min;
+    outMax = Max;
+
+    if(inAuto)
+        *myOutput = constrain(*myOutput, outMin, outMax);
+}
+
+void PID::SetITermLimits(double Min, double Max)
+{
+    if(Min >= Max) return;
+    errorMin = Min;
+    errorMax = Max;
+
+    if(inAuto)
+        ITerm = constrain(ITerm, errorMin, errorMax);
+}
+
+/* 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();
+    }
+    inAuto = newAuto;
+}
+
+/* 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;
+    ITerm = constrain(ITerm, errorMin, errorMax);
+}
+
+/* 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;
+}
+
+/* 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::GetKp()
+{
+    return  dispKp;
+}
+double PID::GetKi()
+{
+    return  dispKi;
+}
+double PID::GetKd()
+{
+    return  dispKd;
+}
+int PID::GetMode()
+{
+    return  inAuto ? AUTOMATIC : MANUAL;
+}
+int PID::GetDirection()
+{
+    return controllerDirection;
+}
+
+