Diff: main.cpp

Revision:

2:12c55b0c9cc2

Parent:

1:571a7ee6023d

Child:

3:f30bd4a0d761

--- a/main.cpp	Fri May 27 16:26:14 2016 +0000
+++ b/main.cpp	Sat May 28 15:54:48 2016 +0000
@@ -62,6 +62,7 @@
 char Receive_Data[8] = {};
 
 int main() {
+    
     init_BLUETOOTH();
     init_TIMER();
     init_PWM();
@@ -77,9 +78,10 @@
     {
        if(bluetooth.readable())
         {
-            bluetooth.scanf("%f%f",&v1_ref,&v2_ref);
+            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;
         }
        /*if(pc.readable())
         {
@@ -94,24 +96,6 @@
 
 void timer1_interrupt(void)
 {    
-
-    /*for(int i=0; i<8; i++)
-    {
-        Receive_Data[i] =  bluetooth.getc();  
-    }
-    //read data from matlab
-    //distance_target
-    v1_ref = (Receive_Data[1]-0x30)*100 + (Receive_Data[2]-0x30)*10 + (Receive_Data[3]-0x30);
-            
-    if(Receive_Data[0] == '-')v1_ref = -1* v1_ref;
-    else v1_ref = v1_ref;
-            
-    //ang_rel_target
-    v2_ref = (Receive_Data[5]-0x30)*100 + (Receive_Data[6]-0x30)*10 + (Receive_Data[7]-0x30);
-            
-    if(Receive_Data[4] == '-')v2_ref = -1* v2_ref;
-    else v2_ref = v2_ref;*/
-    
     //Motor 1
     v1 = (float)v1Count * 50.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
     v1_avg = v1_avg + ( v1 - v1_old[9])/10.0f;
@@ -166,12 +150,12 @@
        }
     }
     
-    /*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;   
-    */
+    //servo = 1;
+    //wait(0.1);       
+    //servo = 0;   
+    
 }
 
 void CN_interrupt(void)