Diff: SpeedController.cpp

Revision:

26:45a53e3c81b1

Parent:

24:8ac46a6d2ac4

Child:

27:72711b6cbe2a

diff -r d73c40fd4b55 -r 45a53e3c81b1 SpeedController.cpp
--- a/SpeedController.cpp	Fri Jul 07 09:05:42 2017 +0000
+++ b/SpeedController.cpp	Wed Aug 02 06:55:54 2017 +0000
@@ -5,7 +5,9 @@
 
 
 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;now_time_=0;old_time_=0;
+    
+    Kv_p=0;Kv_d=0;diff=0;diff_old=0;now_time_=0;old_time_=0;
+    now_omega=0;now_RPM=0;
     out_duty=0;out=0;duty=0;
     pwm_F_.period_us(100);
     pwm_B_.period_us(100);
@@ -16,12 +18,11 @@
     now_omega=omega;
     now_time_=timer.read();
     diff= target_omega-now_omega;
-    integral+=diff;
-    out_duty=Kv_p*diff+Kv_d*(diff-diff_old)/(now_time_-old_time_)+Kv_i*integral*(now_time_-old_time_);
+    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)&&(duty<0.95)) out+=out_duty;
+    if((duty>=-0.95)&&(duty<=0.95)) out+=out_duty;
     duty=0.0001*out+target_omega/C;
     if(duty<-0.95)duty=-0.95;
     else if(duty>0.95)duty=0.95;
@@ -41,6 +42,9 @@
     if(duty>0.95) {
         pwm_F_=0.95;
         pwm_B_=0;
+    } else if(duty<0) {
+        pwm_F_=0;
+        pwm_B_=0;
     } else {
         pwm_F_=duty;
         pwm_B_=0;
@@ -51,13 +55,17 @@
     if(duty>0.95) {
         pwm_F_=0;
         pwm_B_=0.95;
+    } else if(duty<0) {
+        pwm_F_=0;
+        pwm_B_=0;
     } else {
         pwm_F_=0;
         pwm_B_=duty;
     }
 }
 
-void SpeedControl::Accelarate(double target_duty){
+//RC2017で一時期使っていたがあまり出来が良くないので消し飛ばしました。ただ完全に消去するのもあれなので一応コメントアウトという形で残したよ
+/*void SpeedControl::Accelarate(double target_duty){
     double now_speed,old_speed;
     double duty;
     int start_point;
@@ -102,32 +110,31 @@
             }
         }
     }
-}
+}*/
 
-void SpeedControl::ScZ(double target_RPM){
+/*void SpeedControl::ScZ(double target_RPM){
     now_RPM=getRPM();
     //if(fabs(now_RPM-target_RPM)>300) Accelarate(target_RPM/C);
     diff=target_RPM-now_RPM;
     integral+=diff;
-    out_duty=0.01*(Kv_p*diff+Kv_d*(diff-diff_old)+Kv_i*integral);
+    out_duty=0.01*(Kv_p*diff+Kv_d*(diff-diff_old));
     diff_old=diff;
     if(out_duty>0.04)out_duty=0.04;
     if(out_duty<-0.04)out_duty=-0.04;
     if((duty>=0)&&(duty<0.95)) out+=out_duty;
-    duty=0.0001*out/*+target_RPM/C*/;
+    duty=0.0001*out+target_RPM/C;
     if(duty>=0.95) {
         duty=0.94;
         out=0.94/0.0001;
     }
     turnF(duty);
-}
+}*/
 
 void SpeedControl::ScZ2(double target_RPM){
     now_time_=timer.read();
     now_RPM=getRPM();
     diff=target_RPM-now_RPM;
-    integral+=diff;
-    out_duty=(now_time_-old_time_)*Kv_p*diff+Kv_d*(diff-diff_old)/(now_time_-old_time_)+Kv_i*integral;
+    out_duty=(now_time_-old_time_)*Kv_p*diff+Kv_d*(diff-diff_old)/(now_time_-old_time_);
     old_time_=now_time_;
     diff_old=diff;
     if(out_duty>0.001)out_duty=0.001;
@@ -138,10 +145,9 @@
 }
     
 
-void SpeedControl::setPIDparam(double p,double i,double d){
+void SpeedControl::setPDparam(double p,double d){
     Kv_p=p;
     Kv_d=d;
-    Kv_i=i;
 }
 
 
@@ -152,7 +158,7 @@
 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;
+    diff=0;diff_old=0;now_time_=0;old_time_=0;
     out=0;out_duty=0;
 }
 
@@ -160,4 +166,3 @@
     pwm_F_=0;
     pwm_B_=0;
 }
-