Diff: PID.cpp

Revision:

3:316f974b7f98

Parent:

2:55bf0f813bb4

--- a/PID.cpp	Wed Jan 27 21:32:11 2016 +0000
+++ b/PID.cpp	Sun Feb 07 19:06:37 2016 +0000
@@ -1,10 +1,9 @@
 /**
- * @author Aaron Berk
+ * Arduino PID Library - Version 1.1.1
+ * @author Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
  *
  * @section LICENSE
  *
- * Copyright (c) 2010 ARM Limited
- *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
@@ -30,7 +29,7 @@
  *
  * This library is a port of Brett Beauregard's Arduino PID library:
  *
- *  http://www.arduino.cc/playground/Code/PIDLibrary
+ *  https://github.com/br3ttb/Arduino-PID-Library
  *
  * The wikipedia article on PID controllers is a good place to start on
  * understanding how they work:
@@ -48,290 +47,189 @@
  * Includes
  */
 #include "PID.h"
- 
-PID::PID(float Kc, float tauI, float tauD, float interval) {
- 
-    usingFeedForward = false;
-    inAuto           = false;
- 
-    //Default the limits to the full range of I/O: 3.3V
-    //Make sure to set these to more appropriate limits for
-    //your application.
-    setInputLimits(0.0, 3.3);
-    setOutputLimits(0.0, 3.3);
- 
-    tSample_ = interval;
- 
-    setTunings(Kc, tauI, tauD);
- 
-    setPoint_             = 0.0;
-    processVariable_      = 0.0;
-    prevProcessVariable_  = 0.0;
-    controllerOutput_     = 0.0;
-    prevControllerOutput_ = 0.0;
- 
-    accError_ = 0.0;
-    bias_     = 0.0;
-    
-    realOutput_ = 0.0;
- 
-}
- 
-void PID::setInputLimits(float inMin, float inMax) {
+#include "millis/millis.h"
  
-    //Make sure we haven't been given impossible values.
-    if (inMin >= inMax) {
-        return;
-    }
- 
-    //Rescale the working variables to reflect the changes.
-    prevProcessVariable_ *= (inMax - inMin) / inSpan_;
-    accError_            *= (inMax - inMin) / inSpan_;
- 
-    //Make sure the working variables are within the new limits.
-    if (prevProcessVariable_ > 1) {
-        prevProcessVariable_ = 1;
-    } else if (prevProcessVariable_ < 0) {
-        prevProcessVariable_ = 0;
-    }
- 
-    inMin_  = inMin;
-    inMax_  = inMax;
-    inSpan_ = inMax - inMin;
- 
-}
- 
-void PID::setOutputLimits(float outMin, float outMax) {
- 
-    //Make sure we haven't been given impossible values.
-    if (outMin >= outMax) {
-        return;
-    }
- 
-    //Rescale the working variables to reflect the changes.
-    prevControllerOutput_ *= (outMax - outMin) / outSpan_;
- 
-    //Make sure the working variables are within the new limits.
-    if (prevControllerOutput_ > 1) {
-        prevControllerOutput_ = 1;
-    } else if (prevControllerOutput_ < 0) {
-        prevControllerOutput_ = 0;
-    }
+extern Serial pc;
  
-    outMin_  = outMin;
-    outMax_  = outMax;
-    outSpan_ = outMax - outMin;
- 
-}
- 
-void PID::setTunings(float Kc, float tauI, float tauD) {
- 
-    //Verify that the tunings make sense.
-    if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) {
-        return;
-    }
- 
-    //Store raw values to hand back to user on request.
-    pParam_ = Kc;
-    iParam_ = tauI;
-    dParam_ = tauD;
- 
-    float tempTauR;
- 
-    if (tauI == 0.0) {
-        tempTauR = 0.0;
-    } else {
-        tempTauR = (1.0 / tauI) * tSample_;
-    }
- 
-    //For "bumpless transfer" we need to rescale the accumulated error.
-    if (inAuto) {
-        if (tempTauR == 0.0) {
-            accError_ = 0.0;
-        } else {
-            accError_ *= (Kc_ * tauR_) / (Kc * tempTauR);
-        }
-    }
- 
-    Kc_   = Kc;
-    tauR_ = tempTauR;
-    tauD_ = tauD / tSample_;
- 
-}
- 
-void PID::reset(void) {
- 
-    float scaledBias = 0.0;
- 
-    if (usingFeedForward) {
-        scaledBias = (bias_ - outMin_) / outSpan_;
-    } else {
-        scaledBias = (realOutput_ - outMin_) / outSpan_;
-    }
- 
-    prevControllerOutput_ = scaledBias;
-    prevProcessVariable_  = (processVariable_ - inMin_) / inSpan_;
- 
-    //Clear any error in the integral.
-    accError_ = 0;
- 
-}
- 
-void PID::setMode(int mode) {
- 
-    //We were in manual, and we just got set to auto.
-    //Reset the controller internals.
-    if (mode != 0 && !inAuto) {
-        reset();
-    }
- 
-    inAuto = (mode != 0);
- 
+
+/*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(float* Input, float* Output, float* Setpoint,
+        float Kp, float Ki, float Kd, int ControllerDirection)
+{
+    
+    myOutput = Output;
+    myInput = Input;
+    mySetpoint = Setpoint;
+    inAuto = false;
+    
+    PID::SetOutputLimits(0, 1000);             // default output limit corresponds to 
+                                                // the arduino pwm limits                                              
+    SampleTime = 100;                           // default Controller Sample Time is 0.1 seconds
+
+    PID::SetControllerDirection(ControllerDirection);
+    PID::SetTunings(Kp, Ki, Kd);
+
+    lastTime = millis()-SampleTime;          
 }
  
-void PID::setInterval(float interval) {
  
-    if (interval > 0) {
-        //Convert the time-based tunings to reflect this change.
-        tauR_     *= (interval / tSample_);
-        accError_ *= (tSample_ / interval);
-        tauD_     *= (interval / tSample_);
-        tSample_   = interval;
-    }
+/* 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*/
+      float input = *myInput;
+      float error = *mySetpoint - input;
+      ITerm+= (ki * error);
+      if(ITerm > outMax) ITerm= outMax;
+      else if(ITerm < outMin) ITerm= outMin;
+      float dInput = (input - lastInput);
+// pc.printf("Input = %f, Error = %f, Output = %f, SetPoint = %f\n\r",input,error,output,*mySetpoint);
  
-}
- 
-void PID::setSetPoint(float sp) {
+      /*Compute PID Output*/
+      float output = kp * error + ITerm- kd * dInput;
+// pc.printf("Input = %f, Error = %f, Output = %f, SetPoint = %f\n\r",input,error,output,*mySetpoint);
+      
+      if(output > outMax) output = outMax;
+      else if(output < outMin) output = outMin;
+      *myOutput = output;
  
-    setPoint_ = sp;
- 
+      /*Remember some variables for next time*/
+      lastInput = input;
+      lastTime = now;
+      return true;
+   }
+   else return false;
 }
- 
-void PID::setProcessValue(float pv) {
- 
-    processVariable_ = pv;
+
+
+/* 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(float Kp, float Ki, float Kd)
+{
+   if (Kp<0 || Ki<0 || Kd<0) return;
  
-}
- 
-void PID::setBias(float bias){
+   dispKp = Kp; dispKi = Ki; dispKd = Kd;
+   
+   float SampleTimeInSec = ((float)SampleTime)/1000;  
+   kp = Kp;
+   ki = Ki * SampleTimeInSec;
+   kd = Kd / SampleTimeInSec;
  
-    bias_ = bias;
-    usingFeedForward = 1;
- 
+  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)
+   {
+      float ratio  = (float)NewSampleTime
+                      / (float)SampleTime;
+      ki *= ratio;
+      kd /= ratio;
+      SampleTime = (unsigned long)NewSampleTime;
+   }
 }
  
-float PID::compute() {
- 
-    //Pull in the input and setpoint, and scale them into percent span.
-    float scaledPV = (processVariable_ - inMin_) / inSpan_;
- 
-    if (scaledPV > 1.0) {
-        scaledPV = 1.0;
-    } else if (scaledPV < 0.0) {
-        scaledPV = 0.0;
-    }
- 
-    float scaledSP = (setPoint_ - inMin_) / inSpan_;
-    if (scaledSP > 1.0) {
-        scaledSP = 1;
-    } else if (scaledSP < 0.0) {
-        scaledSP = 0;
-    }
- 
-    float error = scaledSP - scaledPV;
- 
-    //Check and see if the output is pegged at a limit and only
-    //integrate if it is not. This is to prevent reset-windup.
-    if (!(prevControllerOutput_ >= 1 && error > 0) && !(prevControllerOutput_ <= 0 && error < 0)) {
-        accError_ += error;
-    }
+/* 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(float Min, float Max)
+{
+   if(Min >= Max) return;
+   outMin = Min;
+   outMax = Max;
  
-    //Compute the current slope of the input signal.
-    float dMeas = (scaledPV - prevProcessVariable_) / tSample_;
- 
-    float scaledBias = 0.0;
- 
-    if (usingFeedForward) {
-        scaledBias = (bias_ - outMin_) / outSpan_;
+   if(inAuto)
+   {
+       if(*myOutput > outMax) *myOutput = outMax;
+       else if(*myOutput < outMin) *myOutput = outMin;
+     
+       if(ITerm > outMax) ITerm= outMax;
+       else if(ITerm < outMin) ITerm= outMin;
+   }
+}
+
+/* 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();
     }
- 
-    //Perform the PID calculation.
-    controllerOutput_ = scaledBias + Kc_ * (error + (tauR_ * accError_) - (tauD_ * dMeas));
- 
-    //Make sure the computed output is within output constraints.
-    if (controllerOutput_ < 0.0) {
-        controllerOutput_ = 0.0;
-    } else if (controllerOutput_ > 1.0) {
-        controllerOutput_ = 1.0;
-    }
- 
-    //Remember this output for the windup check next time.
-    prevControllerOutput_ = controllerOutput_;
-    //Remember the input for the derivative calculation next time.
-    prevProcessVariable_  = scaledPV;
- 
-    //Scale the output from percent span back out to a real world number.
-    return ((controllerOutput_ * outSpan_) + outMin_);
- 
+    inAuto = newAuto;
 }
  
-float PID::getInMin() {
- 
-    return inMin_;
- 
-}
- 
-float PID::getInMax() {
- 
-    return inMax_;
- 
-}
- 
-float PID::getOutMin() {
- 
-    return outMin_;
- 
-}
- 
-float PID::getOutMax() {
- 
-    return outMax_;
- 
-}
- 
-float PID::getInterval() {
- 
-    return tSample_;
- 
+/* 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;
+   if(ITerm > outMax) ITerm = outMax;
+   else if(ITerm < outMin) ITerm = outMin;
 }
- 
-float PID::getPParam() {
- 
-    return pParam_;
- 
-}
- 
-float PID::getIParam() {
- 
-    return iParam_;
- 
+
+/* 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;
 }
- 
-float PID::getDParam() {
- 
-    return dParam_;
- 
-}
- 
-            
-
 
-            
-
-
-    
+/* 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
+ ******************************************************************************/
+float PID::GetKp(){ return  dispKp; }
+float PID::GetKi(){ return  dispKi;}
+float PID::GetKd(){ return  dispKd;}
+int PID::GetMode(){ return  inAuto ? AUTOMATIC : MANUAL;}
+int PID::GetDirection(){ return controllerDirection;}
 
-
-
-
-