2015/05/30

Dependencies:   ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed

Fork of Aigamozu_Robot_ver4_2 by aigamozu

Revision:
14:5deb7a4f1cd4
Parent:
12:48ef69b4f0e4
Child:
15:f3d01f37f00d
--- a/main.cpp	Thu May 14 12:09:22 2015 +0000
+++ b/main.cpp	Thu May 14 12:26:14 2015 +0000
@@ -385,6 +385,11 @@
     int count = 0;
     myGPS.begin(GPS_BAUD_RATE); 
     
+    Timer collect_Timer;
+    const int collect_Time = 2000; //when we collect 4 GPS point, we use 
+    int collect_flag = 0;
+    char collect_state = 'a';
+    
     char SenderIDc;
     //GPS Send Command
     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
@@ -463,8 +468,29 @@
             Get_GPS(&myGPS);
 
         }
+        
+                //get base GPS
+        if( collect_Timer.read_ms() >= collect_Time){
+            collect_Timer.reset();
+            
+            //printf("Send Request:%d,%d\n",collect_flag,collect_state);
+                      
+            agz.createRequestCommand(MyID, collect_state);
+            //Select Destination
+            ZBTxRequest tx64request(base_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;
+            }    
+         } 
                 
-        if(agz.nowMode == AUTO_MODE && auto_Timer.read_ms() >= auto_Time){
+        if(agz.nowMode == AUTO_GPS_MODE && auto_Timer.read_ms() >= auto_Time){
             auto_Timer.reset();
             auto_Move(); 
         }