Revision 1:28542afc96d0, committed 2017-02-15

Comitter:

d15321854

Date:

Wed Feb 15 12:33:10 2017 +0000

Parent:

0:6e12a3e5af19

Commit message:

123

Changed in this revision

--- a/PID.cpp	Thu Sep 02 16:48:10 2010 +0000
+++ b/PID.cpp	Wed Feb 15 12:33:10 2017 +0000
@@ -48,277 +48,136 @@
  * 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;
+#include "mbed.h"
 
-    accError_ = 0.0;
-    bias_     = 0.0;
-    
-    realOutput_ = 0.0;
-
-}
-
-void PID::setInputLimits(float inMin, float inMax) {
-
-    //Make sure we haven't been given impossible values.
-    if (inMin >= inMax) {
-        return;
-    }
+PID::PID(double* Input, double* Output, double* Setpoint,
+        double Kp, double Ki, double Kd, int ControllerDirection, float* NOWTIME, float* LASTTIME)
+{
+  
+    myOutput = Output;
+    myInput = Input;
+    mySetpoint = Setpoint;
+  inAuto = false;
+  
+  PID::SetOutputLimits(0, 387);       //default output limit corresponds to 
+                        //the arduino pwm limits
 
-    //Rescale the working variables to reflect the changes.
-    prevProcessVariable_ *= (inMax - inMin) / inSpan_;
-    accError_            *= (inMax - inMin) / inSpan_;
+    SampleTime =0.01;             //default Controller Sample Time is 0.1 seconds
 
-    //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;
-
+    PID::SetControllerDirection(ControllerDirection);
+    PID::SetTunings(Kp, Ki, Kd);
+    //float TIME=* NOWTIME
+    lastTime =* NOWTIME-SampleTime;                                                 ///*******************
+    //lastTime =0;                                                 ///*******************
 }
 
-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;
-    }
-
-    outMin_  = outMin;
-    outMax_  = outMax;
-    outSpan_ = outMax - outMin;
-
+void PID::SetSampleTime(int NewSampleTime)
+{
+   if (NewSampleTime > 0)
+   {
+      double ratio  = (double)NewSampleTime
+                      / (double)SampleTime;
+      ki *= ratio;
+      kd /= ratio;
+      SampleTime = (unsigned long)NewSampleTime;
+   }
 }
 
-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::SetOutputLimits(double Min, double Max)
+{
+   if(Min >= Max) return;
+   outMin = Min;
+   outMax = Max;
+ 
+   if(inAuto)
+   {
+     if( *myOutput > outMax ) *myOutput = outMax;
+     else if( *myOutput < outMin ) *myOutput = outMin;
+   
+     if(ITerm > outMax) ITerm= outMax;
+     else if(ITerm < outMin) ITerm= outMin;
+   }
 }
 
-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);
-
+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(controllerDirection ==REVERSE)
+   {
+      kp = (0 - kp);
+      ki = (0 - ki);
+      kd = (0 - kd);
+   }
 }
 
-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;
-    }
-
+void PID::Initialize()
+{
+   ITerm = *myOutput;
+   lastInput = *myInput;
+   if(ITerm > outMax) ITerm = outMax;
+   else if(ITerm < outMin) ITerm = outMin;
 }
 
-void PID::setSetPoint(float sp) {
-
-    setPoint_ = sp;
-
-}
-
-void PID::setProcessValue(float pv) {
-
-    processVariable_ = pv;
-
-}
-
-void PID::setBias(float bias){
-
-    bias_ = bias;
-    usingFeedForward = 1;
-
+void PID::SetMode(int Mode)
+{
+    bool newAuto = (Mode == AUTOMATIC);
+    if(newAuto == !inAuto)
+    {  /*we just went from manual to auto*/
+        PID::Initialize();
+    }
+    inAuto = newAuto;
 }
 
-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;
-    }
-
-    //Compute the current slope of the input signal.
-    float dMeas = (scaledPV - prevProcessVariable_) / tSample_;
-
-    float scaledBias = 0.0;
-
-    if (usingFeedForward) {
-        scaledBias = (bias_ - outMin_) / outSpan_;
-    }
-
-    //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_);
-
+void PID::SetControllerDirection(int Direction)
+{
+   if(inAuto && Direction !=controllerDirection)
+   {
+    kp = (0 - kp);
+      ki = (0 - ki);
+      kd = (0 - kd);
+   }   
+   controllerDirection = Direction;
 }
 
-float PID::getInMin() {
-
-    return inMin_;
-
-}
-
-float PID::getInMax() {
-
-    return inMax_;
-
-}
-
-float PID::getOutMin() {
-
-    return outMin_;
-
-}
-
-float PID::getOutMax() {
-
-    return outMax_;
-
-}
+double PID::Compute(float* now)
+{
 
-float PID::getInterval() {
-
-    return tSample_;
-
-}
-
-float PID::getPParam() {
-
-    return pParam_;
-
-}
-
-float PID::getIParam() {
-
-    return iParam_;
-
-}
-
-float PID::getDParam() {
-
-    return dParam_;
-
-}
+   float timeChange = (* now - lastTime);
+   //Serial.print("  timeChange: ");
+   //Serial.print(timeChange);
+   if(timeChange>=SampleTime)
+   {
+      /*Compute all the working error variables*/
+      double input = *myInput;
+      double error = *mySetpoint - input;
+      ITerm+= (dispKi * error);
+      if(abs(ITerm/error) > outMax) ITerm= error*outMax;
+      else if(abs(ITerm/error) < outMin) ITerm= error*outMin;
+      double dInput = (input - lastInput);
+ 
+      /*Compute PID Output*/
+      //double output = dispKp * error + ITerm- dispKd * dInput;
+      double output =(1+dispKd*dInput)*(dispKp*error + dispKi*ITerm);
+      //if(abs(output/(*mySetpoint)) > outMax) output= (*mySetpoint)*outMax;
+      //else if(abs(output/(*mySetpoint)) < outMin) output= (*mySetpoint)*outMin;
+      *myOutput = output + input;
+      //printf("**********************mySetpoint:%.3f\n",*mySetpoint);
+      //printf("**********************input:%.3f\n",input);
+      //printf("**********************myOutput:%.3f\n",*myOutput);
+      if( *myOutput > outMax ) *myOutput = outMax;
+        else if( *myOutput < outMin ) *myOutput = outMin;
+      //printf("**********************myOutput:%.3f\n",*myOutput);
+      //printf("********PID do**************\n");
+      /*Remember some variables for next time*/
+      lastInput = input;
+      lastTime = * now;
+    }
+}
\ No newline at end of file

--- a/PID.h	Thu Sep 02 16:48:10 2010 +0000
+++ b/PID.h	Wed Feb 15 12:33:10 2017 +0000
@@ -44,170 +44,83 @@
  *  http://www.controlguru.com/
  */
 
-#ifndef PID_H
-#define PID_H
-
 /**
  * Includes
  */
 #include "mbed.h"
 
-/**
- * Defines
- */
-#define MANUAL_MODE 0
-#define AUTO_MODE   1
-
-/**
- * Proportional-integral-derivative controller.
- */
-class PID {
-
-public:
+class PID
+{
 
-    /**
-     * Constructor.
-     *
-     * Sets default limits [0-3.3V], calculates tuning parameters, and sets
-     * manual mode with no bias.
-     *
-     * @param Kc - Tuning parameter
-     * @param tauI - Tuning parameter
-     * @param tauD - Tuning parameter
-     * @param interval PID calculation performed every interval seconds.
-     */
-    PID(float Kc, float tauI, float tauD, float interval);
 
-    /**
-     * Scale from inputs to 0-100%.
-     *
-     * @param InMin The real world value corresponding to 0%.
-     * @param InMax The real world value corresponding to 100%.
-     */
-    void setInputLimits(float inMin , float inMax);
+  public:
 
-    /**
-     * Scale from outputs to 0-100%.
-     *
-     * @param outMin The real world value corresponding to 0%.
-     * @param outMax The real world value corresponding to 100%.
-     */
-    void setOutputLimits(float outMin, float outMax);
-
-    /**
-     * Calculate PID constants.
-     *
-     * Allows parameters to be changed on the fly without ruining calculations.
-     *
-     * @param Kc - Tuning parameter
-     * @param tauI - Tuning parameter
-     * @param tauD - Tuning parameter
-     */
-    void setTunings(float Kc, float tauI, float tauD);
+  //Constants used in some of the functions below
+  #define AUTOMATIC 1
+  #define MANUAL  0
+  #define DIRECT  0
+  #define REVERSE  1
 
-    /**
-     * Reinitializes controller internals. Automatically
-     * called on a manual to auto transition.
-     */
-    void reset(void);
-    
-    /**
-     * Set PID to manual or auto mode.
-     *
-     * @param mode        0 -> Manual
-     *             Non-zero -> Auto
-     */
-    void setMode(int mode);
-    
-    /**
-     * Set how fast the PID loop is run.
-     *
-     * @param interval PID calculation peformed every interval seconds.
-     */
-    void setInterval(float interval);
     
-    /**
-     * Set the set point.
-     *
-     * @param sp The set point as a real world value.
-     */
-    void setSetPoint(float sp);
-    
-    /**
-     * Set the process value.
-     *
-     * @param pv The process value as a real world value.
-     */
-    void setProcessValue(float pv);
-    
-    /**
-     * Set the bias.
-     *
-     * @param bias The bias for the controller output.
-     */
-    void setBias(float bias);
+  //commonly used functions **************************************************************************
+    PID(double*, double*, double*,        // * constructor.  links the PID to the Input, Output, and 
+        double, double, double, int, float*, float*);     //   Setpoint.  Initial tuning parameters are also set here
+  
+    void SetMode(int Mode);               // * sets PID to either Manual (0) or Auto (non-0)
 
-    /**
-     * PID calculation.
-     *
-     * @return The controller output as a float between outMin and outMax.
-     */
-    float compute(void);
+    double Compute(float*);                       // * performs the PID calculation.  it should be
+                                          //   called every time loop() cycles. ON/OFF and
+                                          //   calculation frequency can be set using SetMode
+                                          //   SetSampleTime respectively
 
-    //Getters.
-    float getInMin();
-    float getInMax();
-    float getOutMin();
-    float getOutMax();
-    float getInterval();
-    float getPParam();
-    float getIParam();
-    float getDParam();
+    void SetOutputLimits(double, double); //clamps the output to a specific range. 0-255 by default, but
+                      //it's likely the user will want to change this depending on
+                      //the application
+  
 
-private:
-
-    bool usingFeedForward;
-    bool inAuto;
 
-    //Actual tuning parameters used in PID calculation.
-    float Kc_;
-    float tauR_;
-    float tauD_;
-    
-    //Raw tuning parameters.
-    float pParam_;
-    float iParam_;
-    float dParam_;
-    
-    //The point we want to reach.
-    float setPoint_;         
-    //The thing we measure.
-    float processVariable_;  
-    float prevProcessVariable_;
-    //The output that affects the process variable.
-    float controllerOutput_; 
-    float prevControllerOutput_;
+  //available but not commonly used functions ********************************************************
+    void SetTunings(double, double,       // * While most users will set the tunings once in the 
+                    double);            //   constructor, this function gives the user the option
+                                          //   of changing tunings during runtime for Adaptive control
+  void SetControllerDirection(int);   // * Sets 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 SetSampleTime(int);              // * sets the frequency, in Milliseconds, with which 
+                                          //   the PID calculation is performed.  default is 100
+                      
+                      
+                      
+  //Display functions ****************************************************************
+  double GetKp();             // These functions query the pid for interal values.
+  double GetKi();             //  they were created mainly for the pid front-end,
+  double GetKd();             // where it's important to know what is actually 
+  int GetMode();              //  inside the PID.
+  int GetDirection();           //
 
-    //We work in % for calculations so these will scale from
-    //real world values to 0-100% and back again.
-    float inMin_;
-    float inMax_;
-    float inSpan_;
-    float outMin_;
-    float outMax_;
-    float outSpan_;
+  private:
+  void Initialize();
+  
+  double dispKp;        // * we'll hold on to the tuning parameters in user-entered 
+  double dispKi;        //   format for display purposes
+  double dispKd;        //
+    
+  double kp;                  // * (P)roportional Tuning Parameter
+    double ki;                  // * (I)ntegral Tuning Parameter
+    double kd;                  // * (D)erivative Tuning Parameter
+
+  int controllerDirection;
 
-    //The accumulated error, i.e. integral.
-    float accError_;
-    //The controller output bias.
-    float bias_;
+    double *myInput;              // * Pointers to the Input, Output, and Setpoint variables
+    double *myOutput;             //   This creates a hard link between the variables and the 
+    double *mySetpoint;           //   PID, freeing the user from having to constantly tell us
+                                  //   what these values are.  with pointers we'll just know.
+        
+  float lastTime;
+  double ITerm, lastInput;
 
-    //The interval between samples.
-    float tSample_;          
-
-    //Controller output as a real world value.
-    volatile float realOutput_;
-
-};
-
-#endif /* PID_H */
+  float SampleTime;
+  double outMin, outMax;
+  bool inAuto;
+};
\ No newline at end of file