20150511, Nakazawa

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

Fork of aigamozu_auto_ver3_3_2 by aigamozu

Revision:
4:480f4eb9bcbc
Parent:
1:d351cf39a6e3
Child:
7:209c07cc2b80
--- a/main.cpp	Sat Apr 25 04:40:37 2015 +0000
+++ b/main.cpp	Sun Apr 26 16:44:23 2015 +0000
@@ -36,7 +36,9 @@
 ZBRxResponse zbRx = ZBRxResponse();
 XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L);
 
-AGZ_ROBOT robot[4];
+AGZ_ROBOT robot[5];
+
+char Base_ID = 'A';
 
 /////////////////////////////////////////
 //
@@ -158,6 +160,7 @@
                 xbee.getResponse().getZBRxResponse(zbRx);
                 uint8_t *buf = zbRx.getFrameData();
                 uint8_t *buf1 = &buf[11]; 
+                id = buf1[5] - 'A';
                  
                  
                  //Check Command Type 
@@ -182,18 +185,33 @@
                     case STATUS_REQUEST:{
                             //send normal data
                             //Create GPS Infomation Packet
-                            agz.createReceiveStatusCommand('A','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
+                            agz.createReceiveStatusCommand(Base_ID,'a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
                             //Select Destination
                             ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength());
                             //Send -> Base
                             xbee.send(tx64request);
                             
                             //send Kalman data
-                            agz.createReceiveStatusCommandwithKalman('A','a',myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
+                            agz.createReceiveStatusCommandwithKalman(Base_ID,'a',myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
                             //Select Destination
                             ZBTxRequest tx64request1(robot_Address,agz.packetData,agz.getPacketLength());
                             //Send -> Base
                             xbee.send(tx64request);
+                            
+                            //send normal data
+                            //Create GPS Infomation Packet
+                            agz.createReceiveStatusCommand(Base_ID,'a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
+                            //Select Destination
+                            ZBTxRequest tx64request2(collect_Address[id],agz.packetData,agz.getPacketLength());
+                            //Send -> Base
+                            xbee.send(tx64request);
+                            
+                            //send Kalman data
+                            agz.createReceiveStatusCommandwithKalman(Base_ID,'a',myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
+                            //Select Destination
+                            ZBTxRequest tx64request3(collect_Address[id],agz.packetData,agz.getPacketLength());
+                            //Send -> Base
+                            xbee.send(tx64request);
                             break;         
           
                         }