robot
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed agz_common
main.cpp
- Committer:
- m5171135
- Date:
- 2014-05-30
- Revision:
- 3:1229ca3df855
- Parent:
- 2:95955f38f47a
- Child:
- 4:39d2aadf3068
File content as of revision 3:1229ca3df855:
/**********************************************/ // // // // Program name: Aigamozu Robot Control // author: Atsunori Maruyama // ver -> 1.0 // day -> 2014/05/21 // // // /**********************************************/ #include "mbed.h" #include "XBee.h" #include "MBed_Adafruit_GPS.h" #include "VNH5019.h" #include "agzIDLIST.h" #include "aigamozuSetting.h" #include "AigamozuControlPackets.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(PAN1B1_32H,PAN1B1_32L); ///////////////////////////////////////// // //Pin Setting // ///////////////////////////////////////// //Motor Contorol Pin Setting VNH5019 motorShield(p21,p22,p23,p24,p25,p26); AigamozuControlPackets agz; //Interrupt Setting Ticker axis; Ticker auth_axis; ///////////////////////////////////////// // //Each Value Setting // ///////////////////////////////////////// int count = 0; //my status //0: StndbyMode //1: ManualMode //2: AuthmaticMode(Random) unsigned char my_status = 0; //0 bit: Motor Satatus //1 bit: GPS Status //2 bit: Sensor Status //3 bit: Battery Status unsigned char my_mode = 0; //ManualValue int manual_count = 0; int manual_flag = 0; ///////////////////////////////////////// // //Interruption processing 1: time -> 0.1s // ///////////////////////////////////////// void axisRenovation(){ if(manual_count > 100){ motorShield.changeSpeed(0,0,0,0); manual_count = 0; } if(my_mode == 1) manual_count++; } ///////////////////////////////////////// // //Interruption processing: time -> 1.0s // ///////////////////////////////////////// void randomRenovation(){ if(count < 20){ motorShield.changeSpeed(1,127,1,127); } else{ motorShield.changeSpeed(1,64,2,64); if(count > 21) { count = 0; motorShield.changeSpeed(1,127,1,127); } } count++; } ///////////////////////////////////////// // //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); //GPS Send Command myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); myGPS.sendCommand(PGCMD_ANTENNA); //gyro_registor Setting wait(2); //interrupt start axis.attach(&axisRenovation, 0.1); refresh_Timer.start(); while (true) { //recive xbee module xbee.readPacket(); if (xbee.getResponse().isAvailable()) { if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) { xbee.getResponse().getZBRxResponse(zbRx); unsigned char *buf = zbRx.getFrameData(); //Analyze the command type switch(agz.checkCommnadType(buf)){ //CommnadType -> ChangeModeCommand;;; case CHANGE_MODE:{ my_mode = buf[19]; motorShield.changeSpeed(0,0,0,0); if(my_mode == 2) auth_axis.attach(&randomRenovation, 1.0); else auth_axis.detach(); break; } //CommnadType -> ManualCommand;;; case MANUAL:{ /// @todo Magic. if(my_mode == 1){ manual_count = 0; motorShield.changeSpeed(buf[20],buf[21],buf[23],buf[24]); } break; } //SendStatus case STATUS_REQUEST:{ //Create packet uint8_t* data = agz.createStatusPacket(myGPS.latitudeH, myGPS.latitudeL, myGPS.longitudeH, myGPS.longitudeL,1,2); ZBTxRequest tx64request(remoteAddress,data,sizeof(data)); //Send GPS ingomation xbee.send(tx64request); break; } //Default 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) my_status = 0; else my_status = 1; } } }