![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
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
Revision 3:a66f4fa1f8a8, committed 2015-05-11
- 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());