Diff: PID.h

Revision:

4:208f5279143a

Child:

5:f1613df66ceb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.h	Sat Mar 23 19:46:09 2019 +0000
@@ -0,0 +1,96 @@
+class PID
+{
+    private:
+    float Kp_, Ki_, Kd_, Ts, PrevErr, PrevPrevErr, prevControlAction, setPoint;
+    
+    float inMin_;
+    float inMax_;
+    
+    float outMin_;
+    float outMax_;
+    
+    public:
+    PID (float Kp, float Ki, float Kd, float sampleTime)
+    {
+        PrevErr = 0;
+        PrevPrevErr = 0;
+        prevControlAction = 0;
+        setPoint = 0;
+        
+        inMin_ = -3.3f;
+        inMax_ = 3.3f;
+        outMin_ = -1.0f;
+        outMax_ = 1.0f;
+        
+        Ts = sampleTime;
+        Kp_ = Kp;
+        Ki_ = Ki;
+        Kd_ = Kd;
+    }
+    
+    float compute (float currVal)
+    {
+    if (currVal > 
+    float currErr = setPoint - (currVal);
+    float controlAction;
+    
+    if (currErr > inMax_) {currErr = inMax_;}
+    if (currErr < inMin_) {currErr = inMin_;}
+    
+    controlAction = prevControlAction - (PrevErr*Kp_) + (Kp_*currErr)+ (Ki_*Ts*currErr) + ((Kd_/Ts)*(currErr - PrevErr - PrevErr + PrevPrevErr));
+    
+    if (controlAction > inMax_) {controlAction = inMax_;}
+    if (controlAction < inMin_) {controlAction = inMin_;} 
+   
+    prevControlAction = controlAction;
+    PrevPrevErr = PrevErr;
+    PrevErr = currErr;
+    //scale the control Action to the correct output limits and output
+    return ((((controlAction)- inMin_)/(inMax_ - inMin_)) * (outMax_ - outMin_)) + outMin_;
+    }
+    
+    void setSetPoint(float _sP)
+    {
+        setPoint = _sP;
+    }
+    
+    void setOutputLimits(float outMin,float outMax)
+    {
+        if (outMin > outMax) {return;}
+        
+        outMin_ = outMin;
+        outMax_ = outMax;
+    }
+    
+    void setInputLimits(float inMin,float inMax)
+    {
+        if (inMin > inMax) {return;}
+        //scales the previous errors and control action to correct input limits
+        PrevPrevErr = scaler(inMin_,inMax_,inMin,inMax,PrevPrevErr);
+        PrevErr = scaler(inMin_,inMax_,inMin,inMax,PrevErr);
+        prevControlAction = scaler(inMin_,inMax_,inMin,inMax,prevControlAction);
+        
+        inMin_ = inMin;
+        inMax_ = inMax;
+    }
+    
+    float scaler(float prevMin, float prevMax, float newMin, float newMax, float var)
+    {
+        if (var > prevMax) {var = prevMax;}
+        if (var < prevMin) {var = prevMin;}
+        return (((var-prevMin)/(prevMax - prevMin))*(newMax-newMin))+newMin;
+    }
+    
+    float returnPrevCA()
+    {
+        return   prevControlAction;
+    }
+    
+    float returnInMax()
+    {
+        return inMax_;   
+    }
+    
+};
+
+//(((temp-inMin)/(inMax - inMin))*(outMax-outMin))+outMin
\ No newline at end of file