Diff: SpeedController.cpp

Revision:

9:a919aa92e65e

Parent:

7:87c135463de7

Child:

10:216d5a573dc7

diff -r 833757a1df66 -r a919aa92e65e SpeedController.cpp
--- a/SpeedController.cpp	Mon Nov 21 06:31:24 2016 +0000
+++ b/SpeedController.cpp	Mon Nov 28 13:01:04 2016 +0000
@@ -69,7 +69,7 @@
     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);
+            printf("duty = %f RPM = %f\r\n",(double)i*5.0/100.0,getRPM());
             turnF(duty);
             int count=0;
             while(1){
@@ -86,7 +86,7 @@
     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);
+            printf("duty = %f RPM = %f\r\n",(double)i*5.0/100.0,getRPM());
             turnF(duty);
             int count=0;
             while(1){
@@ -103,8 +103,8 @@
 }
 
 void SpeedControl::ScZ(double target_RPM){
-    double now_RPM=getRPM();
-    if(fabs(now_RPM-target_RPM)>200) Accelarate(target_RPM/C);
+    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);
@@ -115,6 +115,19 @@
     duty=0.0001*out+target_RPM/C;
     turnF(duty);
 }
+
+void SpeedControl::ScZ2(double target_RPM){
+    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);
+    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.0001*out+target_RPM/C;
+    turnF(duty);
+}
     
 
 void SpeedControl::setPIDparam(double p,double i,double d){