aigamozu
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
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 |
--- 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
--- 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());