Diff: pid/pid.cpp

Revision:

0:813f5cd20cf1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pid/pid.cpp	Wed Sep 05 05:40:11 2018 +0000
@@ -0,0 +1,73 @@
+#include "mbed.h"
+#include "pid.h"
+
+
+PID::PID(void){
+    initialize();
+}
+
+PID::PID(double Pgain, double Igain, double Dgain){
+    initialize();
+    setPIDgain(Pgain,Igain,Dgain);
+}
+
+PID::PID(double Pgain, double Igain, double Dgain, double Max, double Min){
+    initialize();
+    setPIDgain(Pgain,Igain,Dgain);
+    setMaxMin(Max,Min);
+}
+
+PID::~PID(){
+
+}
+
+void PID::initialize(void){
+    kp = 0.0;   ki = 0.0;   kd = 0.0;
+    max = 0.0; min = 0.0;
+    dt = 0.0;
+    integral = 0.0;
+    for(uint8_t i=0; i<2; i++){
+        oldval[i] = 0.0;
+        diff[i] = 0.0;
+    }
+    maxcheck = false;
+    mincheck = false;
+}
+
+void PID::setPIDgain(double Pgain, double Igain, double Dgain){
+    kp = Pgain;
+    ki = Igain;
+    kd = Dgain;
+}
+
+void PID::setMaxMin(double Max, double Min){
+    if(Max < Min) return;   //最大値<最小値であれば設定せずに終了
+    max = Max;
+    min = Min;
+    maxcheck = true; mincheck = true;
+}
+
+void PID::switchMaxMin(bool Maxcheck, bool Mincheck){
+    maxcheck = Maxcheck;
+    mincheck = Mincheck;
+}
+
+double PID::calcPID(double nowval, double targetval, double dt){
+    double p,i,d,pid;
+
+    diff[1] = diff[0];
+    diff[0] = nowval - targetval;
+    if(diff[1] == 0.0) return 0.0;   //前回の値がない場合，0.0を返す
+
+    integral += (diff[0] + diff[1]) / 2.0 * dt;
+
+    p = kp * diff[0];
+    i = ki * integral;
+    d = kd * (diff[0] - diff[1]) / dt;
+    pid = p + i + d;
+    if(maxcheck && pid>max) pid = max;
+    if(mincheck && pid<min) pid = min;
+
+    return pid;
+
+}