yokokawa
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of aigamozu_program_ver2 by
main.cpp
- Committer:
- s1200058
- Date:
- 2015-04-10
- Revision:
- 11:187ff22b343f
- Parent:
- 10:4a6059c5c84c
- Child:
- 12:3e7ad5c07976
File content as of revision 11:187ff22b343f:
/**********************************************/ // // // // Program name: Aigamozu Robot Control // Author: Atsunori Maruyama // Ver -> 1.3 // Day -> 2014/06/09 // // /**********************************************/ #include "mbed.h" #include "XBee.h" #include "MBed_Adafruit_GPS.h" #include "AigamozuControlPackets.h" #include "agzIDLIST.h" #include "aigamozuSetting.h" #include "agz_common.h" ///////////////////////////////////////// // //Connection Setting // ///////////////////////////////////////// //Serial Connect Setting: PC <--> mbed Serial pc(USBTX, USBRX); //Serial Connect Setting: GPS <--> mbed Serial * gps_Serial; //Serial Connect Setting: XBEE <--> mbed XBee xbee(p13,p14); ZBRxResponse zbRx = ZBRxResponse(); XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L); AGZ_ROBOT robot[4]; ///////////////////////////////////////// // //Pin Setting // ///////////////////////////////////////// VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26); ///////////////////////////////////////// // //Main Processing // ///////////////////////////////////////// int main() { //start up time wait(3); //set pc frequency to 57600bps pc.baud(PC_BAUD_RATE); //set xbee frequency to 57600bps xbee.begin(XBEE_BAUD_RATE); //GPS setting gps_Serial = new Serial(p28,p27); Adafruit_GPS myGPS(gps_Serial); Timer refresh_Timer; 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)}; 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); myGPS.sendCommand(PGCMD_ANTENNA); wait(2); //interrupt start 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 *buf1 = &buf[11]; //Check Command Type switch(agz.checkCommnadType(buf)){ printf("%c\n", buf[14]); //CommandType -> ChanegeMode case CHANGE_MODE :{ agz.changeMode(buf); break; } //CommandType -> Manual case MANUAL:{ //Check now Mode if(agz.nowMode == MANUAL_MODE){ agz.changeSpeed(buf); } break; } //CommandType -> Send Status case STATUS_REQUEST:{ //Create GPS Infomation Packet agz.createReceiveStatusCommand('B','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); //Select Destination ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength()); //Send -> Base xbee.send(tx64request); break; } case RECEIVE_STATUS:{ printf("ok\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: { break; } } } } myGPS.read(); //recive gps module //check if we recieved a new message from GPS, if so, attempt to parse it, if ( myGPS.newNMEAreceived() ) { if ( !myGPS.parse(myGPS.lastNMEA()) ) { 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); } else agz.nowStatus = GPS_UNAVAIL; } 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){ printf("%d %c\n", collect_flag, collect_state); collect_state = 'a'; collect_flag = 0; } } } }