ver2
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of aigamozu_auto_ver1 by
Diff: main.cpp
- Revision:
- 1:a5f98c7e1feb
- Parent:
- 0:d8f3aa3b6876
- Child:
- 2:e9a8664b52ff
diff -r d8f3aa3b6876 -r a5f98c7e1feb main.cpp --- a/main.cpp Sat Apr 11 11:57:26 2015 +0000 +++ b/main.cpp Sat Apr 11 12:25:34 2015 +0000 @@ -16,6 +16,7 @@ #include "AigamozuControlPackets.h" #include "agzIDLIST.h" #include "aigamozuSetting.h" +#include "agz_common.h" ///////////////////////////////////////// // @@ -32,11 +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); -//Timer -Timer t; -int flag=0; +AGZ_ROBOT robot[4]; ///////////////////////////////////////// // @@ -51,7 +50,6 @@ // ///////////////////////////////////////// int main() { - int count = 0; //start up time wait(3); //set pc frequency to 57600bps @@ -66,6 +64,16 @@ const int refresh_Time = 2000; //refresh time in ms myGPS.begin(GPS_BAUD_RATE); + Timer collect_Timer; + const int collect_Time = 200; //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)}; + XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L); + int id; + + //GPS Send Command myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); @@ -77,16 +85,19 @@ AigamozuControlPackets agz(agz_motorShield); refresh_Timer.start(); + printf("test\n"); + while (true) { - + //Check Xbee Buffer Available xbee.readPacket(); 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 switch(agz.checkCommnadType(buf)){ @@ -109,22 +120,40 @@ //CommandType -> Send Status case STATUS_REQUEST:{ //Create GPS Infomation Packet - agz.createReceiveStatusCommand(1,6,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); + agz.createReceiveStatusCommand('B','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); //Select Destination - ZBTxRequest tx64request(remoteAddress,agz.packetData,agz.getPacketLength()); + ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength()); //Send -> Base xbee.send(tx64request); break; } + + case RECEIVE_STATUS:{ + + printf("Receive Status\n"); + + 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("%d\n", id); + printf("%ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL()); + break; + } - default: - { + default:{ break; - } } } + + + } } + myGPS.read(); //recive gps module @@ -134,18 +163,37 @@ continue; } } - if (refresh_Timer.read_ms() >= refresh_Time) { refresh_Timer.reset(); if (myGPS.fix) { agz.nowStatus = GPS_AVAIL; agz.reNewPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); + + printf("my %ld, %ld, %ld, %ld\n", myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); } else agz.nowStatus = GPS_UNAVAIL; + } - - + //get base GPS + if( collect_Timer.read_ms() >= collect_Time){ + collect_Timer.reset(); - } + printf("Send Request\n"); + + agz.createRequestCommand('A', collect_state); + //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