Diff: main.cpp

Revision:

3:a66f4fa1f8a8

Parent:

2:2afe6fd6e565

--- a/main.cpp	Mon Apr 27 13:25:52 2015 +0000
+++ b/main.cpp	Mon May 11 13:15:05 2015 +0000
@@ -10,6 +10,16 @@
 //
 /**********************************************/
 
+/*********************/
+// 2015/05/11 yokokawa mami
+// AGZ_ROBOTでbase[5], robot[2], managerを宣言しました。
+// XbeeAddressでbase_Address, robot_Address, manager_Addressを宣言しました。
+// case STATUS_REQUEST:での条件分岐
+// send_AddressのところにそのときのAddressが入るようにしました。
+// case RECEIVE_STATUS:での条件分岐
+// base, robot, managerに対応するように変更しました。
+/*********************/
+
 #include "mbed.h"
 #include "XBee.h"
 #include "MBed_Adafruit_GPS.h"
@@ -36,7 +46,7 @@
 ZBRxResponse zbRx = ZBRxResponse();
 XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L);
 
-AGZ_ROBOT robot[5];
+AGZ_ROBOT base[5], robot[2], manager;
 
 char MyID = 'Z';
 
@@ -134,10 +144,19 @@
     XBeeAddress64 collect_Address[5] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 
                                         XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L),
                                         XBeeAddress64(BASE5_32H,BASE5_32L)};
-    XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
-    int id,SenderIDc;
+    //XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
+    int id, SenderIDc;
     bool flag = true;
     
+    
+    XBeeAddress64 base_Address[5] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 
+                                    XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L),
+                                    XBeeAddress64(BASE5_32H,BASE5_32L)};
+    XBeeAddress64 robot_Address[2] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L)};
+    XBeeAddress64 manager_Address = XBeeAddress64(BASE1_32H,BASE1_32L);
+    
+    XBeeAddress64 send_Address;
+    
     //GPS Send Command
     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
@@ -161,7 +180,8 @@
                 xbee.getResponse().getZBRxResponse(zbRx);
                 uint8_t *buf = zbRx.getFrameData();
                 uint8_t *buf1 = &buf[11]; 
-                id = buf1[5] - 'A';
+                
+                //id = buf1[5] - 'A';
                 SenderIDc = buf1[5];
                  
                  
@@ -185,6 +205,7 @@
                     
                     //CommandType -> Send Status
                     case STATUS_REQUEST:{
+                            /*
                             if(SenderIDc == 'Z'){
                                 //send normal data
                                 //Create GPS Infomation Packet
@@ -201,15 +222,25 @@
                                 //Send -> Base
                                 xbee.send(tx64request1);
                                 
-                            }else{
+                            }else{*/
                                 
                                 printf("%d: catch request\n", id);
                                 
                                 //send normal data
                                 //Create GPS Infomation Packet
+                                if(SenderIDc == '0'){
+                                    send_Address = manager_Address;
+                                }
+                                if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
+                                    send_Address = robot_Address[SenderIDc - 'A'];
+                                }
+                                if(SenderIDc >= 'a' && SenderIDc <= 'z'){
+                                    send_Address = base_Address[SenderIDc - 'a'];
+                                }
+                                
                                 agz.createReceiveStatusCommand(MyID,SenderIDc,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
                                 //Select Destination
-                                ZBTxRequest tx64request2(collect_Address[id],agz.packetData,agz.getPacketLength());
+                                ZBTxRequest tx64request2(send_Address,agz.packetData,agz.getPacketLength());
                                 //Send -> Base
                                 xbee.send(tx64request2);
                             
@@ -219,7 +250,7 @@
                                 ZBTxRequest tx64request3(collect_Address[id],agz.packetData,agz.getPacketLength());
                                 //Send -> Base
                                 xbee.send(tx64request3);
-                            }
+                           // }
                             break;         
           
                         }
@@ -228,14 +259,33 @@
                             
                         //printf("Receive Status\n");      
     
-                        id = buf1[5] - '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]);
-                        
-                        agz.reNewBasePoint(id,robot[id].get_LatitudeH(),robot[id].get_LatitudeL(),robot[id].get_LongitudeH(),robot[id].get_LongitudeL());
+                        if(SenderIDc == '0'){
+                            id = 0;
+                            manager.set_state(buf1[9]);
+                            manager.set_LatitudeH(&buf1[13]);
+                            manager.set_LatitudeL(&buf1[17]);
+                            manager.set_LongitudeH(&buf1[21]);
+                            manager.set_LongitudeL(&buf1[25]);                        
+                            agz.reNewBasePoint(id,manager.get_LatitudeH(),manager.get_LatitudeL(),manager.get_LongitudeH(),manager.get_LongitudeL());
+                        }        
+                        if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
+                            id = SenderIDc - '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]);                        
+                            agz.reNewBasePoint(id,robot[id].get_LatitudeH(),robot[id].get_LatitudeL(),robot[id].get_LongitudeH(),robot[id].get_LongitudeL());
+                        }
+                        if(SenderIDc >= 'a' && SenderIDc <= 'z'){
+                            id = SenderIDc - 'a';
+                            base[id].set_state(buf1[9]);
+                            base[id].set_LatitudeH(&buf1[13]);
+                            base[id].set_LatitudeL(&buf1[17]);
+                            base[id].set_LongitudeH(&buf1[21]);
+                            base[id].set_LongitudeL(&buf1[25]);                        
+                            agz.reNewBasePoint(id,base[id].get_LatitudeH(),base[id].get_LatitudeL(),base[id].get_LongitudeH(),base[id].get_LongitudeL());
+                        }
                     /*   
                         printf("%d,", buf1[5]);                    
                         printf(" %ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL());