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