Diff: AI_Friday.cpp

Revision:

26:dbdbfdb4dd41

Parent:

25:7053736ea6cc

Child:

27:1696ef46b538

diff -r 7053736ea6cc -r dbdbfdb4dd41 AI_Friday.cpp
--- a/AI_Friday.cpp	Thu May 26 05:08:24 2016 +0000
+++ b/AI_Friday.cpp	Thu May 26 10:28:19 2016 +0000
@@ -30,7 +30,6 @@
 Ticker timer1;
 void timer1_interrupt(void);
 void CN_interrupt(void);
-//void _ISR_U2RXInterrupt(void);
 
 void init_TIMER(void);
 void init_PWM(void);
@@ -54,13 +53,10 @@
 int angle = 90;
 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] = {0};
-
 double 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;
 
 float k_forward = 20, k_turn = 10;
 
@@ -106,69 +102,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;
-        }
     }
 }
 
 void timer1_interrupt(void)
 {
+    for(int i=0; i<32; i++)
+    {
+        Receive_Data[i] =  bluetooth.getc();  
+    }
+    //read data from matlab
+    //distance_target
+    Distance_Target = (Receive_Data[1]-0x30)*100 + (Receive_Data[2]-0x30)*10 + (Receive_Data[3]-0x30);
+            
+    if(Receive_Data[0] == '-')Distance_Target = -1* Distance_Target;
+    else Distance_Target = Distance_Target;
+            
+    //ang_rel_target
+    Angle_Target = (Receive_Data[5]-0x30)*100 + (Receive_Data[6]-0x30)*10 + (Receive_Data[7]-0x30);
+            
+    if(Receive_Data[4] == '-')Distance_Target = -1* Distance_Target;
+    else Distance_Target = Distance_Target;
+            
+    //x_position_car_1
+    X_Position_1 = (Receive_Data[9]-0x30)*100 + (Receive_Data[10]-0x30)*10 + (Receive_Data[11]-0x30);
+            
+    if(Receive_Data[8] == '-')Distance_Target = -1* Distance_Target;
+    else Distance_Target = Distance_Target;
+            
+    //y_position_car_1
+    Y_Position_1 = (Receive_Data[13]-0x30)*100 + (Receive_Data[14]-0x30)*10 + (Receive_Data[15]-0x30);
+            
+    if(Receive_Data[12] == '-')Distance_Target = -1* Distance_Target;
+    else Distance_Target = Distance_Target;
+            
+    //angle_car_1
+    Angle_1 = (Receive_Data[17]-0x30)*100 + (Receive_Data[18]-0x30)*10 + (Receive_Data[19]-0x30);
+            
+    if(Receive_Data[16] == '-')Distance_Target = -1* Distance_Target;
+    else Distance_Target = Distance_Target;
+            
+    //x_position_car_2
+    X_Position_2 = (Receive_Data[21]-0x30)*100 + (Receive_Data[22]-0x30)*10 + (Receive_Data[23]-0x30);
+            
+    if(Receive_Data[20] == '-')Distance_Target = -1* Distance_Target;
+    else Distance_Target = Distance_Target;
+            
+    //y_position_car_2
+    Y_Position_2 = (Receive_Data[25]-0x30)*100 + (Receive_Data[26]-0x30)*10 + (Receive_Data[27]-0x30);
+            
+    if(Receive_Data[24] == '-')Distance_Target = -1* Distance_Target;
+    else Distance_Target = Distance_Target;
+            
+    //angle_car_1
+    Angle_2 = (Receive_Data[29]-0x30)*100 + (Receive_Data[30]-0x30)*10 + (Receive_Data[31]-0x30);
+            
+    if(Receive_Data[28] == '-')Distance_Target = -1* Distance_Target;
+    else Distance_Target = Distance_Target;
+ 
+    
+    
     //Motor 1
     v1 = (float)v1Count * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
     v1Count = 0;
@@ -495,11 +488,9 @@
               }
          
           break;
-            
-        
+    
     }
-    
-    
+       
 }
 
 void CN_interrupt(void)
@@ -609,44 +600,6 @@
     //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)