Diff: PIDcontroller.cpp

Revision:

0:90457f6c41ea

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PIDcontroller.cpp	Fri Aug 10 07:22:48 2018 +0000
@@ -0,0 +1,24 @@
+#include "PIDcontroller.h"
+#include "mbed.h"
+ 
+ float PIDController::output(float sensorVal,float targetVal){
+    float p,i,d;
+    diff1 = diff2;
+    diff2 = sensorVal - targetVal;                        //偏差を取得
+       integral += (diff1 + diff2) / 2.0*delta_t;
+
+      p = KP * diff2;                       //P制御
+      i = KI * integral;                                      //I制御
+      d = KD * (diff2- diff1) / delta_t;       //D制御    
+      sum=  abs(p+i+d);
+     //   pc.printf("%3.5f\n",delta_t);
+      if(sum>1.0){
+        diff2=0;
+        integral=0;    
+    }
+      return sum;
+}
+
+void PIDController::print(){
+    printf("%3.5f\n",sum);
+}
\ No newline at end of file