Revision 8:079c3326816e, committed 2016-05-25

Comitter:

roger5641

Date:

Wed May 25 07:07:21 2016 +0000

Parent:

7:1232c7939984

Child:

10:06cc2b1d2aaf

Commit message:

add Bluetooth

Changed in this revision

--- a/ai.cpp	Tue May 24 08:12:52 2016 +0000
+++ b/ai.cpp	Wed May 25 07:07:21 2016 +0000
@@ -1,6 +1,6 @@
 
 
-****  receive and return by bluetooth **************
+****  receive and return by bluetooth ************** // bluetooth.getc()
 float xC, yC // car's position
 
 float angleC // car's angle 

--- a/main.cpp	Tue May 24 08:12:52 2016 +0000
+++ b/main.cpp	Wed May 25 07:07:21 2016 +0000
@@ -29,6 +29,7 @@
 Ticker timer1;
 void timer1_interrupt(void);
 void CN_interrupt(void);
+void _ISR_U2RXInterrupt(void);
 
 void init_TIMER(void);
 void init_PWM(void);
@@ -48,6 +49,14 @@
 
 float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90
 
+int Command_Flag = 0, Receive_Flag = 0, Receive_Counter = 0, 
+int Receive_Data[33];
+
+int Distance_Target = 0, Angle_Target = 0;
+int X_Position_1 = 0, Y_Position_1 = 0, Angle_1 = 0;
+int X_Position_2 = 0, Y_Position_2 = 0, Angle_2 = 0;
+int pwm_duty;
+
 int main() {
     
     init_TIMER();
@@ -58,6 +67,7 @@
     pc.baud(57600);
     
     
+    
     while(1) 
     {
         if(pc.readable())
@@ -68,6 +78,66 @@
         {
             pc.putc(bluetooth.getc());
         }
+        
+        if(Command_Flag == 1)
+        {
+            //read data from matlab
+            //distance_target
+            Distance_Target = (Receive_Data[2]-0x30)*100 + (Receive_Data[3]-0x30)*10 + (Receive_Data[4]-0x30);
+            
+            if(Receive_Data[1] == '-')Distance_Target = -1* Distance_Target;
+            else Distance_Target = Distance_Target;
+            
+            //ang_rel_target
+            Angle_Target = (Receive_Data[6]-0x30)*100 + (Receive_Data[7]-0x30)*10 + (Receive_Data[8]-0x30);
+            
+            if(Receive_Data[5] == '-')Distance_Target = -1* Distance_Target;
+            else Distance_Target = Distance_Target;
+            
+            //x_position_car_1
+            X_Position_1 = (Receive_Data[10]-0x30)*100 + (Receive_Data[11]-0x30)*10 + (Receive_Data[12]-0x30);
+            
+            if(Receive_Data[9] == '-')Distance_Target = -1* Distance_Target;
+            else Distance_Target = Distance_Target;
+            
+            //y_position_car_1
+            Y_Position_1 = (Receive_Data[14]-0x30)*100 + (Receive_Data[15]-0x30)*10 + (Receive_Data[16]-0x30);
+            
+            if(Receive_Data[13] == '-')Distance_Target = -1* Distance_Target;
+            else Distance_Target = Distance_Target;
+            
+            //angle_car_1
+            Angle_1 = (Receive_Data[18]-0x30)*100 + (Receive_Data[19]-0x30)*10 + (Receive_Data[20]-0x30);
+            
+            if(Receive_Data[17] == '-')Distance_Target = -1* Distance_Target;
+            else Distance_Target = Distance_Target;
+            
+            //x_position_car_2
+            X_Position_2 = (Receive_Data[22]-0x30)*100 + (Receive_Data[23]-0x30)*10 + (Receive_Data[24]-0x30);
+            
+            if(Receive_Data[21] == '-')Distance_Target = -1* Distance_Target;
+            else Distance_Target = Distance_Target;
+            
+            //y_position_car_2
+            Y_Position_2 = (Receive_Data[26]-0x30)*100 + (Receive_Data[27]-0x30)*10 + (Receive_Data[28]-0x30);
+            
+            if(Receive_Data[25] == '-')Distance_Target = -1* Distance_Target;
+            else Distance_Target = Distance_Target;
+            
+            //angle_car_1
+            Angle_2 = (Receive_Data[30]-0x30)*100 + (Receive_Data[31]-0x30)*10 + (Receive_Data[32]-0x30);
+            
+            if(Receive_Data[29] == '-')Distance_Target = -1* Distance_Target;
+            else Distance_Target = Distance_Target;
+            
+            // PWM
+            if(Receive_Data[33] == 'j')pwm_duty = 148;
+            else if(Receive_Data[33] == 'k')pwm_duty = 100;
+            
+            Command_Flag = 0;
+        }
+        
+        
     }
 }
 
@@ -122,7 +192,7 @@
     
     // Servo
     
-    servo_duty = 0.079 + (0.084/180)*angle;
+    //servo_duty = 0.079 + (0.084/180)*angle;
     servo.write(servo_duty);
     servo = 1;
     wait(0.1);       
@@ -243,6 +313,44 @@
     //v2Count -1
 }
 
+void _ISR_U2RXInterrupt(void)
+{
+/////////// Receive ////////////
+    static char Temp;
+    Temp = U2RXREG;
+
+    if(Receive_Flag == 1)
+    {
+        Receive_Counter++;
+        Receive_Data[Receive_Counter] = Temp;
+        
+        if(Receive_Counter == 33) // 8 data *4byte
+        {
+            //Send_Flag == 1
+            Command_Flag = 1;
+            Receive_Flag = 0;
+            Receive_Counter = 0;               
+        }        
+    }
+    
+    else
+    {
+        if(Temp == 36)  //'$'
+        {
+            Receive_Flag = 1;
+            Receive_Counter = 0;
+            Receive_Data[0] = Temp;
+        }
+        else
+        {
+            // Waiting   
+        }
+    }
+    
+    IFS1bits.U2RXIF = 0;
+}
+
+
 void init_TIMER(void)
 {
     timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)