Diff: CommonLibraries/PID/PID.cpp

Revision:

4:ba9df71868df

diff -r e10d8736fd22 -r ba9df71868df CommonLibraries/PID/PID.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CommonLibraries/PID/PID.cpp	Mon Oct 01 13:47:19 2018 +0000
@@ -0,0 +1,84 @@
+/*
+* PID.cpp
+*
+* Created: 2016/07/01 17:47:16
+*  Author: yuuki
+*/
+
+#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] = sensorData - targetData;
+        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;
+    }
+
+    //速度系PID関数
+    double PID::SetSpeed(double sensorData,double targetData)
+    {
+        //TODO:速度単位系の変換
+    }
+    
+}
\ No newline at end of file