Diff: SpeedController.cpp

Revision:

12:530f6184830a

Parent:

10:216d5a573dc7

Child:

13:caa6db604077

--- a/SpeedController.cpp	Tue Dec 13 07:38:51 2016 +0000
+++ b/SpeedController.cpp	Fri Dec 16 07:07:27 2016 +0000
@@ -119,15 +119,17 @@
 }
 
 void SpeedControl::ScZ2(double target_RPM){
+    now_time_=timer.read();
     now_RPM=getRPM();
     diff=target_RPM-now_RPM;
     integral+=diff;
-    out_duty=0.01*(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)+Kv_i*integral);
+    old_time_=now_time_;
     diff_old=diff;
     if(out_duty>0.001)out_duty=0.001;
     if(out_duty<-0.001)out_duty=-0.001;
     out+=out_duty;
-    duty=0.001*out/*+target_RPM/C*/;
+    duty=0.001*out;
     turnF(duty);
 }
     
@@ -143,6 +145,13 @@
     C=c;
 }
 
+void SpeedControl::reset(){
+    S=0;stateA=0;stateB=0;count=0;pre_count=0.0,omega=0;
+    rev=0;now_time=0;old_time=0;RPM=0;RPM_old=0;
+    diff=0;diff_old=0;integral=0;now_time_=0;old_time_=0;
+    out=0;out_duty=0;
+}
+
 void SpeedControl::stop(){
     pwm_F_=0;
     pwm_B_=0;