riko-ten / EC2

ec

Dependents:   F3RC

Fork of EC by ROBOSTEP_LIBRARY

Diff: SpeedController.cpp

Revision:
35:b47330f0dec2
Parent:
32:297384f9d261
--- a/SpeedController.cpp	Thu Jul 05 01:23:58 2018 +0000
+++ b/SpeedController.cpp	Sat Nov 03 23:09:50 2018 +0000
@@ -18,13 +18,13 @@
 void SpeedControl::Sc(double target_omega){       //スカンジウムじゃないよ
     now_omega=omega;
     now_time_=timer.read();
-    diff= target_omega-now_omega;
+     diff= (target_omega-now_omega)/C;
     out_duty=Kv_p*diff+Kv_d*(diff-diff_old)/(now_time_-old_time_);
     diff_old=diff;
     if(out_duty>0.1)out_duty=0.1;
     if(out_duty<-0.1)out_duty=-0.1;
-    if((duty>=-0.95)&&(duty<=0.95)) out+=out_duty;
-    duty=0.00002*out+target_omega/C;
+    if((duty>=-0.95)&&(duty<=0.95)) duty=duty+out_duty;
+    //duty=target_omega/C;
     if(duty<-0.95)duty=-0.95;
     else if(duty>0.95)duty=0.95;
     old_time_=now_time_;