ver2
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of aigamozu_auto_ver1 by
main.cpp@0:d8f3aa3b6876, 2015-04-11 (annotated)
- Committer:
- kityann
- Date:
- Sat Apr 11 11:57:26 2015 +0000
- Revision:
- 0:d8f3aa3b6876
- Child:
- 1:a5f98c7e1feb
auto program
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kityann | 0:d8f3aa3b6876 | 1 | /**********************************************/ |
kityann | 0:d8f3aa3b6876 | 2 | // |
kityann | 0:d8f3aa3b6876 | 3 | // |
kityann | 0:d8f3aa3b6876 | 4 | // |
kityann | 0:d8f3aa3b6876 | 5 | // Program name: Aigamozu Robot Control |
kityann | 0:d8f3aa3b6876 | 6 | // Author: Atsunori Maruyama |
kityann | 0:d8f3aa3b6876 | 7 | // Ver -> 1.3 |
kityann | 0:d8f3aa3b6876 | 8 | // Day -> 2014/06/09 |
kityann | 0:d8f3aa3b6876 | 9 | // |
kityann | 0:d8f3aa3b6876 | 10 | // |
kityann | 0:d8f3aa3b6876 | 11 | /**********************************************/ |
kityann | 0:d8f3aa3b6876 | 12 | |
kityann | 0:d8f3aa3b6876 | 13 | #include "mbed.h" |
kityann | 0:d8f3aa3b6876 | 14 | #include "XBee.h" |
kityann | 0:d8f3aa3b6876 | 15 | #include "MBed_Adafruit_GPS.h" |
kityann | 0:d8f3aa3b6876 | 16 | #include "AigamozuControlPackets.h" |
kityann | 0:d8f3aa3b6876 | 17 | #include "agzIDLIST.h" |
kityann | 0:d8f3aa3b6876 | 18 | #include "aigamozuSetting.h" |
kityann | 0:d8f3aa3b6876 | 19 | |
kityann | 0:d8f3aa3b6876 | 20 | ///////////////////////////////////////// |
kityann | 0:d8f3aa3b6876 | 21 | // |
kityann | 0:d8f3aa3b6876 | 22 | //Connection Setting |
kityann | 0:d8f3aa3b6876 | 23 | // |
kityann | 0:d8f3aa3b6876 | 24 | ///////////////////////////////////////// |
kityann | 0:d8f3aa3b6876 | 25 | |
kityann | 0:d8f3aa3b6876 | 26 | //Serial Connect Setting: PC <--> mbed |
kityann | 0:d8f3aa3b6876 | 27 | Serial pc(USBTX, USBRX); |
kityann | 0:d8f3aa3b6876 | 28 | |
kityann | 0:d8f3aa3b6876 | 29 | //Serial Connect Setting: GPS <--> mbed |
kityann | 0:d8f3aa3b6876 | 30 | Serial * gps_Serial; |
kityann | 0:d8f3aa3b6876 | 31 | |
kityann | 0:d8f3aa3b6876 | 32 | //Serial Connect Setting: XBEE <--> mbed |
kityann | 0:d8f3aa3b6876 | 33 | XBee xbee(p13,p14); |
kityann | 0:d8f3aa3b6876 | 34 | ZBRxResponse zbRx = ZBRxResponse(); |
kityann | 0:d8f3aa3b6876 | 35 | XBeeAddress64 remoteAddress = XBeeAddress64(PAN1B1_32H,PAN1B1_32L); |
kityann | 0:d8f3aa3b6876 | 36 | |
kityann | 0:d8f3aa3b6876 | 37 | //Timer |
kityann | 0:d8f3aa3b6876 | 38 | Timer t; |
kityann | 0:d8f3aa3b6876 | 39 | int flag=0; |
kityann | 0:d8f3aa3b6876 | 40 | |
kityann | 0:d8f3aa3b6876 | 41 | ///////////////////////////////////////// |
kityann | 0:d8f3aa3b6876 | 42 | // |
kityann | 0:d8f3aa3b6876 | 43 | //Pin Setting |
kityann | 0:d8f3aa3b6876 | 44 | // |
kityann | 0:d8f3aa3b6876 | 45 | ///////////////////////////////////////// |
kityann | 0:d8f3aa3b6876 | 46 | VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26); |
kityann | 0:d8f3aa3b6876 | 47 | |
kityann | 0:d8f3aa3b6876 | 48 | ///////////////////////////////////////// |
kityann | 0:d8f3aa3b6876 | 49 | // |
kityann | 0:d8f3aa3b6876 | 50 | //Main Processing |
kityann | 0:d8f3aa3b6876 | 51 | // |
kityann | 0:d8f3aa3b6876 | 52 | ///////////////////////////////////////// |
kityann | 0:d8f3aa3b6876 | 53 | int main() { |
kityann | 0:d8f3aa3b6876 | 54 | int count = 0; |
kityann | 0:d8f3aa3b6876 | 55 | //start up time |
kityann | 0:d8f3aa3b6876 | 56 | wait(3); |
kityann | 0:d8f3aa3b6876 | 57 | //set pc frequency to 57600bps |
kityann | 0:d8f3aa3b6876 | 58 | pc.baud(PC_BAUD_RATE); |
kityann | 0:d8f3aa3b6876 | 59 | //set xbee frequency to 57600bps |
kityann | 0:d8f3aa3b6876 | 60 | xbee.begin(XBEE_BAUD_RATE); |
kityann | 0:d8f3aa3b6876 | 61 | |
kityann | 0:d8f3aa3b6876 | 62 | //GPS setting |
kityann | 0:d8f3aa3b6876 | 63 | gps_Serial = new Serial(p28,p27); |
kityann | 0:d8f3aa3b6876 | 64 | Adafruit_GPS myGPS(gps_Serial); |
kityann | 0:d8f3aa3b6876 | 65 | Timer refresh_Timer; |
kityann | 0:d8f3aa3b6876 | 66 | const int refresh_Time = 2000; //refresh time in ms |
kityann | 0:d8f3aa3b6876 | 67 | myGPS.begin(GPS_BAUD_RATE); |
kityann | 0:d8f3aa3b6876 | 68 | |
kityann | 0:d8f3aa3b6876 | 69 | //GPS Send Command |
kityann | 0:d8f3aa3b6876 | 70 | myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); |
kityann | 0:d8f3aa3b6876 | 71 | myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); |
kityann | 0:d8f3aa3b6876 | 72 | myGPS.sendCommand(PGCMD_ANTENNA); |
kityann | 0:d8f3aa3b6876 | 73 | |
kityann | 0:d8f3aa3b6876 | 74 | wait(2); |
kityann | 0:d8f3aa3b6876 | 75 | |
kityann | 0:d8f3aa3b6876 | 76 | //interrupt start |
kityann | 0:d8f3aa3b6876 | 77 | AigamozuControlPackets agz(agz_motorShield); |
kityann | 0:d8f3aa3b6876 | 78 | refresh_Timer.start(); |
kityann | 0:d8f3aa3b6876 | 79 | |
kityann | 0:d8f3aa3b6876 | 80 | |
kityann | 0:d8f3aa3b6876 | 81 | while (true) { |
kityann | 0:d8f3aa3b6876 | 82 | |
kityann | 0:d8f3aa3b6876 | 83 | //Check Xbee Buffer Available |
kityann | 0:d8f3aa3b6876 | 84 | xbee.readPacket(); |
kityann | 0:d8f3aa3b6876 | 85 | if (xbee.getResponse().isAvailable()) { |
kityann | 0:d8f3aa3b6876 | 86 | if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) { |
kityann | 0:d8f3aa3b6876 | 87 | xbee.getResponse().getZBRxResponse(zbRx); |
kityann | 0:d8f3aa3b6876 | 88 | uint8_t *buf = zbRx.getFrameData(); |
kityann | 0:d8f3aa3b6876 | 89 | |
kityann | 0:d8f3aa3b6876 | 90 | |
kityann | 0:d8f3aa3b6876 | 91 | //Check Command Type |
kityann | 0:d8f3aa3b6876 | 92 | switch(agz.checkCommnadType(buf)){ |
kityann | 0:d8f3aa3b6876 | 93 | |
kityann | 0:d8f3aa3b6876 | 94 | //CommandType -> ChanegeMode |
kityann | 0:d8f3aa3b6876 | 95 | case CHANGE_MODE :{ |
kityann | 0:d8f3aa3b6876 | 96 | agz.changeMode(buf); |
kityann | 0:d8f3aa3b6876 | 97 | break; |
kityann | 0:d8f3aa3b6876 | 98 | } |
kityann | 0:d8f3aa3b6876 | 99 | |
kityann | 0:d8f3aa3b6876 | 100 | //CommandType -> Manual |
kityann | 0:d8f3aa3b6876 | 101 | case MANUAL:{ |
kityann | 0:d8f3aa3b6876 | 102 | //Check now Mode |
kityann | 0:d8f3aa3b6876 | 103 | if(agz.nowMode == MANUAL_MODE){ |
kityann | 0:d8f3aa3b6876 | 104 | agz.changeSpeed(buf); |
kityann | 0:d8f3aa3b6876 | 105 | } |
kityann | 0:d8f3aa3b6876 | 106 | break; |
kityann | 0:d8f3aa3b6876 | 107 | } |
kityann | 0:d8f3aa3b6876 | 108 | |
kityann | 0:d8f3aa3b6876 | 109 | //CommandType -> Send Status |
kityann | 0:d8f3aa3b6876 | 110 | case STATUS_REQUEST:{ |
kityann | 0:d8f3aa3b6876 | 111 | //Create GPS Infomation Packet |
kityann | 0:d8f3aa3b6876 | 112 | agz.createReceiveStatusCommand(1,6,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); |
kityann | 0:d8f3aa3b6876 | 113 | //Select Destination |
kityann | 0:d8f3aa3b6876 | 114 | ZBTxRequest tx64request(remoteAddress,agz.packetData,agz.getPacketLength()); |
kityann | 0:d8f3aa3b6876 | 115 | //Send -> Base |
kityann | 0:d8f3aa3b6876 | 116 | xbee.send(tx64request); |
kityann | 0:d8f3aa3b6876 | 117 | break; |
kityann | 0:d8f3aa3b6876 | 118 | |
kityann | 0:d8f3aa3b6876 | 119 | } |
kityann | 0:d8f3aa3b6876 | 120 | |
kityann | 0:d8f3aa3b6876 | 121 | default: |
kityann | 0:d8f3aa3b6876 | 122 | { |
kityann | 0:d8f3aa3b6876 | 123 | break; |
kityann | 0:d8f3aa3b6876 | 124 | } |
kityann | 0:d8f3aa3b6876 | 125 | } |
kityann | 0:d8f3aa3b6876 | 126 | } |
kityann | 0:d8f3aa3b6876 | 127 | } |
kityann | 0:d8f3aa3b6876 | 128 | |
kityann | 0:d8f3aa3b6876 | 129 | myGPS.read(); |
kityann | 0:d8f3aa3b6876 | 130 | //recive gps module |
kityann | 0:d8f3aa3b6876 | 131 | //check if we recieved a new message from GPS, if so, attempt to parse it, |
kityann | 0:d8f3aa3b6876 | 132 | if ( myGPS.newNMEAreceived() ) { |
kityann | 0:d8f3aa3b6876 | 133 | if ( !myGPS.parse(myGPS.lastNMEA()) ) { |
kityann | 0:d8f3aa3b6876 | 134 | continue; |
kityann | 0:d8f3aa3b6876 | 135 | } |
kityann | 0:d8f3aa3b6876 | 136 | } |
kityann | 0:d8f3aa3b6876 | 137 | |
kityann | 0:d8f3aa3b6876 | 138 | if (refresh_Timer.read_ms() >= refresh_Time) { |
kityann | 0:d8f3aa3b6876 | 139 | refresh_Timer.reset(); |
kityann | 0:d8f3aa3b6876 | 140 | if (myGPS.fix) { |
kityann | 0:d8f3aa3b6876 | 141 | agz.nowStatus = GPS_AVAIL; |
kityann | 0:d8f3aa3b6876 | 142 | agz.reNewPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); |
kityann | 0:d8f3aa3b6876 | 143 | } |
kityann | 0:d8f3aa3b6876 | 144 | else agz.nowStatus = GPS_UNAVAIL; |
kityann | 0:d8f3aa3b6876 | 145 | } |
kityann | 0:d8f3aa3b6876 | 146 | |
kityann | 0:d8f3aa3b6876 | 147 | |
kityann | 0:d8f3aa3b6876 | 148 | |
kityann | 0:d8f3aa3b6876 | 149 | |
kityann | 0:d8f3aa3b6876 | 150 | } |
kityann | 0:d8f3aa3b6876 | 151 | } |