2015/05/11

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

Fork of aigamozu_auto_for_robot_ver1 by aigamozu

Files at this revision

API Documentation at this revision

Comitter:
s1200058
Date:
Mon May 11 13:15:05 2015 +0000
Parent:
2:2afe6fd6e565
Commit message:
2015/05/11 yokokawa

Changed in this revision

AigamozuControlPackets.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 2afe6fd6e565 -r a66f4fa1f8a8 AigamozuControlPackets.lib
--- a/AigamozuControlPackets.lib	Mon Apr 27 13:25:52 2015 +0000
+++ b/AigamozuControlPackets.lib	Mon May 11 13:15:05 2015 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/teams/aigamozu/code/AigamozuControlPackets/#ac839aff80bc
+http://mbed.org/teams/aigamozu/code/AigamozuControlPackets/#0e815cca2cc7
diff -r 2afe6fd6e565 -r a66f4fa1f8a8 main.cpp
--- 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());