Diff: main.cpp

Revision:

10:4a6059c5c84c

Parent:

9:357fa70be9c7

Child:

11:187ff22b343f

--- a/main.cpp	Sat Jun 28 07:17:51 2014 +0000
+++ b/main.cpp	Thu Apr 09 11:18:07 2015 +0000
@@ -16,6 +16,7 @@
 #include "AigamozuControlPackets.h"
 #include "agzIDLIST.h"
 #include "aigamozuSetting.h"
+#include "agz_common.h"
 
 /////////////////////////////////////////
 //
@@ -32,7 +33,9 @@
 //Serial Connect Setting: XBEE <--> mbed
 XBee xbee(p13,p14);
 ZBRxResponse zbRx = ZBRxResponse();
-XBeeAddress64 remoteAddress = XBeeAddress64(PAN1B1_32H,PAN1B1_32L);
+XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L);
+
+AGZ_ROBOT robot[4];
 
 /////////////////////////////////////////
 //
@@ -61,6 +64,15 @@
     const int refresh_Time = 2000; //refresh time in ms
     myGPS.begin(GPS_BAUD_RATE); 
     
+    Timer collect_Timer;
+    const int collect_Time = 10000; //when we collect 4 GPS point, we use 
+    int collect_flag = 0;
+    char collect_state = 'a';
+    XBeeAddress64 collect_Address[4] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 
+                                        XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L)};
+    int id;
+  
+    
     //GPS Send Command
     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
@@ -72,6 +84,8 @@
     AigamozuControlPackets agz(agz_motorShield);
     refresh_Timer.start();
     
+    printf("test\n");
+    
     
     while (true) {
         
@@ -80,7 +94,8 @@
             if (xbee.getResponse().isAvailable()) {
                 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
                 xbee.getResponse().getZBRxResponse(zbRx);
-                uint8_t *buf = zbRx.getFrameData(); 
+                uint8_t *buf = zbRx.getFrameData();
+                uint8_t *buf1 = &buf[11]; 
                  
                  
                  //Check Command Type 
@@ -112,12 +127,27 @@
                             break;         
           
                         }
+                        
+                        case REQUEST_COMMAND:{
+                            
+                            id = buf1[6] - 'A';
+                            robot[id].set_state(buf1[9]);
+                            robot[id].set_LatitudeH(&buf1[13]);
+                            robot[id].set_LatitudeL(&buf1[17]);
+                            robot[id].set_LongitudeH(&buf1[21]);
+                            robot[id].set_LongitudeL(&buf1[25]); 
+                            
+                            printf("%ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(),  robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL());
+                            break;
+                        }
                     
                         default:
                         {
                         break;
                             }
                     }
+                    
+            
                 }
             }
             
@@ -139,8 +169,26 @@
             else agz.nowStatus = GPS_UNAVAIL;
         } 
         
-
         
+        if( collect_Timer.read_ms() >= collect_Time){
+            collect_Timer.reset();
+            
+            printf("Send Request\n");
             
-    }
+            //Create GPS Infomation Packet
+            agz.createReceiveStatusCommand('B', collect_state,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
+            //Select Destination
+            ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength());
+            //Send -> Base
+            xbee.send(tx64request);
+            
+            collect_flag++;
+            collect_state++;
+
+            if(collect_flag == 4){
+                collect_state = 'a';
+                collect_flag = 0;
+            }    
+         } 
+    } 
 }
\ No newline at end of file