Diff: main.cpp

Revision:

3:f30bd4a0d761

Parent:

2:12c55b0c9cc2

Child:

5:63c9ab496df9

--- a/main.cpp	Sat May 28 15:54:48 2016 +0000
+++ b/main.cpp	Sat Jun 04 03:31:27 2016 +0000
@@ -56,7 +56,7 @@
 float v2_err = 0.0, v2_err_old = 0.0, PIout_2 = 0.0;
 float v2_old[10] = {}, v2_avg = 0.0;
 
-int servo_angle = 87;
+int servo_angle = 50;
 float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90
 
 char Receive_Data[8] = {};
@@ -78,10 +78,27 @@
     {
        if(bluetooth.readable())
         {
-            bluetooth.scanf("%f%f%d",&v1_ref,&v2_ref,&servo_angle);
-            v1_ref = v1_ref - 300.0f;
-            v2_ref = v2_ref - 300.0f;
-            servo_angle = servo_angle - 20;
+            bluetooth.scanf("%f%f",&v1_ref,&v2_ref);
+            
+            if((0<=v1_ref<=600)&&(0<=v2_ref<=600))
+            {
+                v1_ref = v1_ref - 300.0f;
+                v2_ref = v2_ref - 300.0f;       
+            }
+            else if((v1_ref>900)&&(0<=v2_ref<=600))
+            {
+                v1_ref = v1_ref - 600.0f;
+                v2_ref = v2_ref - 300.0f;
+                servo_angle = 50;       
+            }
+            else if((0<=v1_ref<=600)&&(v2_ref>900))
+            {
+                v1_ref = v1_ref - 300.0f;
+                v2_ref = v2_ref - 600.0f;
+                servo_angle = 15;       
+            }
+            
+            
         }
        /*if(pc.readable())
         {
@@ -152,9 +169,9 @@
     
     servo_duty = 0.079 + (0.084/180)*servo_angle; // 要修改
     servo.write(servo_duty);
-    //servo = 1;
-    //wait(0.1);       
-    //servo = 0;   
+    servo = 1;
+    wait(0.1);       
+    servo = 0;   
     
 }