Revision 1:50963ee12158, committed 2018-03-25

Comitter:

billwu1149

Date:

Sun Mar 25 06:35:59 2018 +0000

Parent:

0:810f12542b58

Commit message:

aecl

Changed in this revision

--- a/main.cpp	Tue Jul 11 14:02:39 2017 +0000
+++ b/main.cpp	Sun Mar 25 06:35:59 2018 +0000
@@ -152,41 +152,43 @@
         Gyro_y   = Gyro - Q_bias;
         //===========Alpha dot=============
         //alpha_dot = meas2*PI/180;//因為此處是設定以y軸當作轉軸來看(轉成徑度)
-        kp=0.35;
-        kd=0.03;
+        kp=0.5;
+        kd=0.2;
         angle1=Angle*PI/180;
         angle_dot=meas2*PI/180;
         u=kp*abs(angle1)+kd*abs(angle_dot);
-        pwm1=0.5+u;
-        pwm2=0.5-u;
+        //pwm1=0.5+u;
+        //pwm2=0.5-u;
         //u2=u*0.07*sin(angle1);
         
-        
-        if (0<Angle<3 || 0>Angle>-3)
+        if (Angle>4)
         {
-         servo1.write(0.5);
-         servo2.write(0.5);   
+         pwm1=0.5+u;
+         pwm2=0.5-u;    
         }
         
-        if (Angle>3)
+        else if (Angle<-4)
         {
-         servo1.write( pwm1);
-         servo2.write( pwm2);    
+         pwm1=0.5-u;
+         pwm2=0.5+u;   
         }
+        else if (0<Angle<4 || 0>Angle>-4)
+        {
+         pwm1=0.5;
+         pwm2=0.5;   
+        }
+     
         
-        if (Angle<-3)
-        {
-         servo1.write( pwm2);
-         servo2.write( pwm1);   
-        }
+        servo1.write( pwm1);
+        servo2.write( pwm2);
         
         while((clk.read() - Tin)<0.01){} //設定取樣時間
         Tin = clk.read();   //Reset the loop timer
         
         
         
-        printf("angle_dot=%f  ,Angle=%f\n",meas2,Angle);
-        printf("u=%f\n",u);
+        printf("angle_dot=%.3f  ,Angle=%.3f\n",meas2,Angle);
+        printf("u=%.3f\n",u);
         
           }
 }