Diff: AI_Friday.cpp

Revision:

13:13d7f407d8b4

Parent:

12:c1a667ca6c53

--- a/AI_Friday.cpp	Wed May 25 10:39:33 2016 +0000
+++ b/AI_Friday.cpp	Wed May 25 10:45:29 2016 +0000
@@ -28,6 +28,7 @@
 Ticker timer1;
 void timer1_interrupt(void);
 void CN_interrupt(void);
+void _ISR_U2RXInterrupt(void);
 
 void init_TIMER(void);
 void init_PWM(void);
@@ -44,6 +45,16 @@
 float v2 = 0.0, v2_ref = -60.0;
 float v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 0.0;
 
+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;
+
+
+
 //****  receive and return by bluetooth ************** // bluetooth.getc()
 float xC, yC // car's position
 
@@ -75,6 +86,64 @@
         {
             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;
+        }
     }
 }
 
@@ -312,6 +381,43 @@
     //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)