Diff: SpeedController.cpp

Revision:

5:4abba4f54406

Parent:

4:1b333860dd41

Child:

7:87c135463de7

--- a/SpeedController.cpp	Wed Nov 02 02:43:15 2016 +0000
+++ b/SpeedController.cpp	Sat Nov 12 06:50:56 2016 +0000
@@ -4,7 +4,7 @@
 /***EC classの方を先に読んでおくこと***/
 
 
-SpeedControl::SpeedControl(PinName signalA , PinName signalB , int s , double t , PinName pwm_F , PinName pwm_B) : Ec(signalA,signalB,s,t) , pwm_F_(pwm_F),pwm_B_(pwm_B) {
+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;
     out_duty=0;out=0;duty=0;
     pwm_F_.period_us(100);
@@ -35,23 +35,6 @@
     }
 }
 
-
-void SpeedControl::setPIDparam(double p,double i,double d){
-    Kv_p=p;
-    Kv_d=d;
-    Kv_i=i;
-}
-
-
-void SpeedControl::setDOconstant(double c){
-    C=c;
-}
-
-void SpeedControl::stop(){
-    pwm_F_=0;
-    pwm_B_=0;
-}
-
 void SpeedControl::turnF(double duty){
     if(duty>0.95) {
         pwm_F_=0.95;
@@ -70,4 +53,83 @@
         pwm_F_=0;
         pwm_B_=duty;
     }
-}
\ No newline at end of file
+}
+
+void SpeedControl::Accelarate(double target_duty){
+    double now_speed,old_speed;
+    double duty;
+    int start_point;
+    int max_point=int(target_duty/0.05);
+    int now_point=int((double)pwm_F_/0.05);
+    if(now_point<3) start_point=3;
+    else start_point=now_point;
+    
+    if(max_point>19) max_point=19;
+    else if(max_point<0) max_point=0;
+    if(max_point>now_point){
+        for(int i=start_point;i<=max_point;i++){
+            duty=(double)i*5.0/100.0;
+            printf("duty = %f\r\n",(double)i*5.0/100.0);
+            turnF(duty);
+            int count=0;
+            while(1){
+                old_speed=now_speed;
+                wait(0.01);
+                now_speed=getRPM();
+                if(now_speed>old_speed) {
+                    if(count>3) break;
+                    else count++;
+                }
+            }
+        }
+    }
+    else if(max_point<now_point){
+        for(int i=now_point;i>=max_point;i--){
+            duty=(double)i*5.0/100.0;
+            printf("duty = %f\r\n",(double)i*5.0/100.0);
+            turnF(duty);
+            int count=0;
+            while(1){
+                old_speed=now_speed;
+                wait(0.01);
+                now_speed=getRPM();
+                if(now_speed<old_speed) {
+                    if(count>3) break;
+                    else count++;
+                }
+            }
+        }
+    }
+}
+
+void SpeedControl::ScZ(double target_RPM){
+    double now_RPM=getRPM();
+    if(fabs(now_RPM-target_RPM)>200) 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);
+    diff_old=diff;
+    if(out_duty>0.01)out_duty=0.01;
+    if(out_duty<-0.01)out_duty=-0.01;
+    out+=out_duty;
+    duty=0.0001*out+target_RPM/C;
+    turnF(duty);
+}
+    
+
+void SpeedControl::setPIDparam(double p,double i,double d){
+    Kv_p=p;
+    Kv_d=d;
+    Kv_i=i;
+}
+
+
+void SpeedControl::setDOconstant(double c){
+    C=c;
+}
+
+void SpeedControl::stop(){
+    pwm_F_=0;
+    pwm_B_=0;
+}
+