Diff: main.cpp

Revision:

16:4b502070bea8

Parent:

15:1c1ae4077900

Child:

17:3c82070177e5

--- a/main.cpp	Tue Sep 23 06:21:30 2014 +0000
+++ b/main.cpp	Tue Sep 23 12:08:15 2014 +0000
@@ -43,8 +43,8 @@
 
 Serial pc(USBTX,USBRX);
 
-MultiSerial xbee(p28,p27,read);
-MultiSerial armMbed(p9,p10,write);
+MultiSerial xbee(p28,p27);
+MultiSerial armMbed(p9,p10);
 
 /* data structure */
 typedef struct {
@@ -58,6 +58,8 @@
 
 uint8_t get_data[DATA_NUM]= {0};
 
+uint8_t check_data[1];
+
 /* Out put PC stdout function */
 
 void pc_print(){
@@ -72,6 +74,20 @@
     count++;
 }
 
+void connect_check(void){
+    xbee.start_read();
+    xbee.read_data(check_data,XBEE_KEY);
+    
+    while(check_data[0]!=0xCC);
+    while(check_data[0]==0xCC);
+    xbee.stop_read();
+    xbee.start_write();
+    check_data[0]=0xCC;
+    xbee.write_data(check_data,XBEE_KEY);
+    wait_ms(10);
+    xbee.stop_write();
+    }
+
 void stop_slowly(int count)
 {
 
@@ -103,8 +119,13 @@
 
     xbee_packet *pt_packet=&packet;
 
+    connect_check();
+    
+    xbee.start_read();
     xbee.read_data(get_data,XBEE_KEY);
 
+    connect_check();
+
     int counter=0;
 
     for(;;) {
@@ -163,8 +184,8 @@
         if(packet.leg&D) motors = 0xA5; //10100101 D
 
         /* Turn circling */
-        if(packet.leg&R_t) motors = 0x66; //01100110
-        if(packet.leg&L_t) motors = 0x99; //10011001
+        if(packet.leg&L_t) motors = 0x66; //01100110
+        if(packet.leg&R_t) motors = 0x99; //10011001
 
         pc_print();
     }