Diff: PID.cpp

Revision:

1:32c9f196607d

Parent:

0:6e12a3e5af19

diff -r 6e12a3e5af19 -r 32c9f196607d PID.cpp
--- a/PID.cpp	Thu Sep 02 16:48:10 2010 +0000
+++ b/PID.cpp	Wed Aug 30 02:08:29 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