riko-ten / EC2

ec

Dependents:   F3RC

Fork of EC by ROBOSTEP_LIBRARY

Diff: SpeedController.cpp

Revision:
13:caa6db604077
Parent:
12:530f6184830a
Child:
14:28b6628fe90f
Child:
15:aef7cc059e50
--- a/SpeedController.cpp	Fri Dec 16 07:07:27 2016 +0000
+++ b/SpeedController.cpp	Sat Jan 21 07:11:08 2017 +0000
@@ -123,7 +123,7 @@
     now_RPM=getRPM();
     diff=target_RPM-now_RPM;
     integral+=diff;
-    out_duty=(now_time_-old_time_)*(Kv_p*diff+Kv_d*(diff-diff_old)+Kv_i*integral);
+    out_duty=(now_time_-old_time_)*Kv_p*diff+Kv_d*(diff-diff_old)/(now_time_-old_time_)+Kv_i*integral;
     old_time_=now_time_;
     diff_old=diff;
     if(out_duty>0.001)out_duty=0.001;