yokokawa
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of aigamozu_program_ver2 by
Diff: main.cpp
- Revision:
- 10:4a6059c5c84c
- Parent:
- 9:357fa70be9c7
- Child:
- 11:187ff22b343f
diff -r 357fa70be9c7 -r 4a6059c5c84c main.cpp --- a/main.cpp Sat Jun 28 07:17:51 2014 +0000 +++ b/main.cpp Thu Apr 09 11:18:07 2015 +0000 @@ -16,6 +16,7 @@ #include "AigamozuControlPackets.h" #include "agzIDLIST.h" #include "aigamozuSetting.h" +#include "agz_common.h" ///////////////////////////////////////// // @@ -32,7 +33,9 @@ //Serial Connect Setting: XBEE <--> mbed XBee xbee(p13,p14); ZBRxResponse zbRx = ZBRxResponse(); -XBeeAddress64 remoteAddress = XBeeAddress64(PAN1B1_32H,PAN1B1_32L); +XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L); + +AGZ_ROBOT robot[4]; ///////////////////////////////////////// // @@ -61,6 +64,15 @@ const int refresh_Time = 2000; //refresh time in ms myGPS.begin(GPS_BAUD_RATE); + Timer collect_Timer; + const int collect_Time = 10000; //when we collect 4 GPS point, we use + int collect_flag = 0; + char collect_state = 'a'; + XBeeAddress64 collect_Address[4] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), + XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L)}; + int id; + + //GPS Send Command myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); @@ -72,6 +84,8 @@ AigamozuControlPackets agz(agz_motorShield); refresh_Timer.start(); + printf("test\n"); + while (true) { @@ -80,7 +94,8 @@ if (xbee.getResponse().isAvailable()) { if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) { xbee.getResponse().getZBRxResponse(zbRx); - uint8_t *buf = zbRx.getFrameData(); + uint8_t *buf = zbRx.getFrameData(); + uint8_t *buf1 = &buf[11]; //Check Command Type @@ -112,12 +127,27 @@ break; } + + case REQUEST_COMMAND:{ + + id = buf1[6] - '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]); + + printf("%ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL()); + break; + } default: { break; } } + + } } @@ -139,8 +169,26 @@ else agz.nowStatus = GPS_UNAVAIL; } - + if( collect_Timer.read_ms() >= collect_Time){ + collect_Timer.reset(); + + printf("Send Request\n"); - } + //Create GPS Infomation Packet + agz.createReceiveStatusCommand('B', collect_state,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); + //Select Destination + ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength()); + //Send -> Base + xbee.send(tx64request); + + collect_flag++; + collect_state++; + + if(collect_flag == 4){ + collect_state = 'a'; + collect_flag = 0; + } + } + } } \ No newline at end of file