yokokawa
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of aigamozu_program_ver2 by
main.cpp
- Committer:
- m5171135
- Date:
- 2014-05-28
- Revision:
- 2:95955f38f47a
- Parent:
- 1:490b793b2e61
- Child:
- 3:1229ca3df855
File content as of revision 2:95955f38f47a:
/**********************************************/ // // // Program name: Aigamozu Robot Control // author: Atsunori Maruyama // // // ver -> 1.0 // // /**********************************************/ #include "mbed.h" #include "XBee.h" #include "MBed_Adafruit_GPS.h" #include "VNH5019.h" #include "agzIDLIST.h" #define GPS_BAUD_RATE 9600 #define XBEE_BAUD_RATE 57600 #define PC_BAUD_RATE 57600 #define IMU_BAUD_RATE 400000 ///////////////////////////////////////// // //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); //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; //test value long sub_latH = 12345; long sub_latL = 67890; long sub_lonH = 98765; long sub_lonL = 43211; union UNI_TEST_T { long a; uint8_t b[4]; }; UNI_TEST_T latH,latL,lonH,lonL; ///////////////////////////////////////// // //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(); /// @todo change to true 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(); /// @todo use preporcessor (#define) or const. /// @note It's magic number. // COMMAND_TYPE commandType = buf[14]; /// switch(commandType) /// {case MANUAL:} //switch(aigamoCTR.checkCommandType(buf)){ switch((unsigned char)buf[14]){ //ChanegeMode case 'C':{ 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; } /* case 'C': agz.change_Mode(); break; */ //MunualCommand case 'M':{ /// @todo Magic. if(my_mode == 1){ manual_count = 0; motorShield.changeSpeed(buf[20],buf[21],buf[23],buf[24]); } break; } /* case 'M': uint8_t sppeds_buf[4] = agz.parseMunualCommand(buf); motorShield.changeSpeed(sppedbuf[0],sppedbuf[1],sppedbuf[2],sppedbuf[3]); break; */ //SendStatus case 'S':{ latH.a = myGPS.latitudeH;; latL.a = myGPS.latitudeL; lonH.a = myGPS.longitudeH; lonL.a = myGPS.longitudeL; //dummy data //latH.a = sub_latH;latL.a = sub_latL;lonH.a = sub_lonH;lonL.a = sub_lonL; // if(validateCommand(commandType)); // createReceiveStatusData(); uint8_t data[] = {65,71,83,82,70,'2',84,'A',83,my_status,71,80,83,latH.b[0],latH.b[1],latH.b[2],latH.b[3],latL.b[0],latL.b[1],latL.b[2],latL.b[3],lonH.b[0],lonH.b[1],lonH.b[2],lonH.b[3],lonL.b[0],lonL.b[1],lonL.b[2],lonL.b[3],65,71,69}; ZBTxRequest tx64request(remoteAddress,data,sizeof(data)); xbee.send(tx64request); break; /* case 'S': */ } 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; } } }