Diff: main.cpp

Revision:

4:1125b24078a3

Parent:

1:571a7ee6023d

--- a/main.cpp	Fri May 27 16:26:14 2016 +0000
+++ b/main.cpp	Mon Jun 06 10:06:08 2016 +0000
@@ -56,10 +56,12 @@
 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 = 70;
 float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90
+int servo_work = 0;
 
-char Receive_Data[8] = {};
+
+//char Receive_Data[8] = {};
 
 int main() {
     init_BLUETOOTH();
@@ -70,6 +72,9 @@
     v1_ref = 0.0;
     v2_ref = 0.0;  
     
+    servo_duty = 0.079 + (0.084/180)*servo_angle; // 要修改
+    servo.write(servo_duty);
+    
     //bluetooth.baud(115200); //設定鮑率
     //pc.baud(57600);
     
@@ -77,9 +82,14 @@
     {
        if(bluetooth.readable())
         {
-            bluetooth.scanf("%f%f",&v1_ref,&v2_ref);
+            bluetooth.scanf("%f %f %d",&v1_ref,&v2_ref, &servo_work);
             v1_ref = v1_ref - 300.0f;
             v2_ref = v2_ref - 300.0f;
+            
+            if(servo_work == 0)
+            servo_angle = 70;
+            else
+            servo_angle = 20;
         }
        /*if(pc.readable())
         {
@@ -166,12 +176,10 @@
        }
     }
     
-    /*servo_duty = 0.079 + (0.084/180)*servo_angle; // 要修改
+    servo_duty = 0.079 + (0.084/180)*servo_angle; // 要修改
     servo.write(servo_duty);
-    servo = 1;
-    wait(0.1);       
-    servo = 0;   
-    */
+  
+    
 }
 
 void CN_interrupt(void)