Diff: SpeedController.cpp

Revision:

10:216d5a573dc7

Parent:

9:a919aa92e65e

Child:

12:530f6184830a

--- a/SpeedController.cpp	Mon Nov 28 13:01:04 2016 +0000
+++ b/SpeedController.cpp	Tue Nov 29 12:45:45 2016 +0000
@@ -5,7 +5,7 @@
 
 
 SpeedControl::SpeedControl(PinName signalA , PinName signalB , PinName signalZ , int s , double t , PinName pwm_F , PinName pwm_B) : Ec(signalA,signalB,signalZ,s,t) , pwm_F_(pwm_F),pwm_B_(pwm_B) {
-    Kv_p=0;Kv_d=0;Kv_i=0;diff=0;diff_old=0;integral=0;
+    Kv_p=0;Kv_d=0;Kv_i=0;diff=0;diff_old=0;integral=0;now_time_=0;old_time_=0;
     out_duty=0;out=0;duty=0;
     pwm_F_.period_us(100);
     pwm_B_.period_us(100);
@@ -13,10 +13,11 @@
 }
 
 void SpeedControl::Sc(double target_omega){       //スカンジウムじゃないよ
-    double now_omega=omega;
+    now_omega=omega;
+    now_time_=timer.read();
     diff= target_omega-now_omega;
     integral+=diff;
-    out_duty=Kv_p*diff+Kv_d*(diff-diff_old)+Kv_i*integral;
+    out_duty=Kv_p*diff+Kv_d*(diff-diff_old)/(now_time_-old_time_)+Kv_i*integral*(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;
@@ -24,6 +25,7 @@
     duty=0.0001*out+target_omega/C;
     if(duty<-0.95)duty=-0.95;
     else if(duty>0.95)duty=0.95;
+    old_time_=now_time_;
     
     if(duty>=0){
         pwm_F_=duty;
@@ -125,7 +127,7 @@
     if(out_duty>0.001)out_duty=0.001;
     if(out_duty<-0.001)out_duty=-0.001;
     out+=out_duty;
-    duty=0.0001*out+target_RPM/C;
+    duty=0.001*out/*+target_RPM/C*/;
     turnF(duty);
 }