Diff: Communication/PID/PID.cpp

Revision:

5:3ae504b88679

diff -r e10d8736fd22 -r 3ae504b88679 Communication/PID/PID.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Communication/PID/PID.cpp	Mon Oct 01 09:07:27 2018 +0000
@@ -0,0 +1,72 @@
+#include "PID.h"
+#include <stdlib.h>
+
+namespace PID_SPACE
+{
+    
+    PID::PID(double deltaTime)
+    {
+        this->deltaTime = deltaTime;
+        this->dataRangeLower = 0;
+        this->dataRangeUpper = 100;
+        kp = 0;
+        ki = 0;
+        kd = 0;
+    }
+    
+    PID::PID(double deltaTime, double dataRangeLower, double dataRangeUpper)
+    {
+        this->deltaTime = deltaTime;
+        this->dataRangeLower = dataRangeLower;
+        this->dataRangeUpper = dataRangeUpper;
+        kp = 0;
+        ki = 0;
+        kd = 0;
+    }
+    
+    PID::PID(double deltaTime, double dataRangeLower, double dataRangeUpper, double KP, double KI, double KD)
+    {
+        this->deltaTime = deltaTime;
+        this->dataRangeLower = dataRangeLower;
+        this->dataRangeUpper = dataRangeUpper;
+        kp = KP;
+        ki = KI;
+        kd = KD;
+    }
+    
+    void PID::SetParam(double KP, double KI, double KD)
+    {
+        kp = KP;
+        ki = KI;
+        kd = KD;
+    }
+    
+    double PID::SetPV(double sensorData, double targetData)
+    {
+        double p, i, d;
+        
+        diff[0] = diff[1];
+        diff[1] = targetData - sensorData;
+        integral += ((diff[1] + diff[0]) / 2.0) * deltaTime;
+        
+        p = kp * diff[1];
+        i = ki * integral;
+        d = kd * ((diff[1] - diff[0]) / deltaTime);
+        mv = PID::limit(p + i + d, dataRangeLower, dataRangeUpper);
+        return mv;
+    }
+    
+    double PID::GetMV()
+    {
+        return mv;
+    }
+    
+    double PID::limit(double data,double lower,double upper)
+    {
+        if(data < lower)        return lower;
+        else if(data > upper)   return upper;
+        else                    return data;
+    }
+    
+}
+