robot
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed agz_common
main.cpp@11:187ff22b343f, 2015-04-10 (annotated)
- Committer:
- s1200058
- Date:
- Fri Apr 10 12:59:22 2015 +0000
- Revision:
- 11:187ff22b343f
- Parent:
- 10:4a6059c5c84c
yokokawa;
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" |
s1200058 | 10:4a6059c5c84c | 19 | #include "agz_common.h" |
m5171135 | 1:490b793b2e61 | 20 | |
m5171135 | 0:7d55d6ace996 | 21 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 22 | // |
m5171135 | 0:7d55d6ace996 | 23 | //Connection Setting |
m5171135 | 0:7d55d6ace996 | 24 | // |
m5171135 | 0:7d55d6ace996 | 25 | ///////////////////////////////////////// |
m5171135 | 4:39d2aadf3068 | 26 | |
m5171135 | 2:95955f38f47a | 27 | //Serial Connect Setting: PC <--> mbed |
m5171135 | 2:95955f38f47a | 28 | Serial pc(USBTX, USBRX); |
m5171135 | 2:95955f38f47a | 29 | |
m5171135 | 2:95955f38f47a | 30 | //Serial Connect Setting: GPS <--> mbed |
m5171135 | 0:7d55d6ace996 | 31 | Serial * gps_Serial; |
m5171135 | 2:95955f38f47a | 32 | |
m5171135 | 2:95955f38f47a | 33 | //Serial Connect Setting: XBEE <--> mbed |
m5171135 | 2:95955f38f47a | 34 | XBee xbee(p13,p14); |
m5171135 | 0:7d55d6ace996 | 35 | ZBRxResponse zbRx = ZBRxResponse(); |
s1200058 | 10:4a6059c5c84c | 36 | XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L); |
s1200058 | 10:4a6059c5c84c | 37 | |
s1200058 | 10:4a6059c5c84c | 38 | AGZ_ROBOT robot[4]; |
m5171135 | 0:7d55d6ace996 | 39 | |
m5171135 | 0:7d55d6ace996 | 40 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 41 | // |
m5171135 | 0:7d55d6ace996 | 42 | //Pin Setting |
m5171135 | 0:7d55d6ace996 | 43 | // |
m5171135 | 0:7d55d6ace996 | 44 | ///////////////////////////////////////// |
m5171135 | 6:21e2792e66c7 | 45 | VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26); |
m5171135 | 0:7d55d6ace996 | 46 | |
m5171135 | 0:7d55d6ace996 | 47 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 48 | // |
m5171135 | 0:7d55d6ace996 | 49 | //Main Processing |
m5171135 | 0:7d55d6ace996 | 50 | // |
m5171135 | 0:7d55d6ace996 | 51 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 52 | int main() { |
m5171135 | 0:7d55d6ace996 | 53 | //start up time |
m5171135 | 0:7d55d6ace996 | 54 | wait(3); |
m5171135 | 0:7d55d6ace996 | 55 | //set pc frequency to 57600bps |
m5171135 | 2:95955f38f47a | 56 | pc.baud(PC_BAUD_RATE); |
m5171135 | 0:7d55d6ace996 | 57 | //set xbee frequency to 57600bps |
m5171135 | 2:95955f38f47a | 58 | xbee.begin(XBEE_BAUD_RATE); |
m5171135 | 0:7d55d6ace996 | 59 | |
m5171135 | 0:7d55d6ace996 | 60 | //GPS setting |
m5171135 | 2:95955f38f47a | 61 | gps_Serial = new Serial(p28,p27); |
m5171135 | 2:95955f38f47a | 62 | Adafruit_GPS myGPS(gps_Serial); |
m5171135 | 2:95955f38f47a | 63 | Timer refresh_Timer; |
m5171135 | 0:7d55d6ace996 | 64 | const int refresh_Time = 2000; //refresh time in ms |
m5171135 | 2:95955f38f47a | 65 | myGPS.begin(GPS_BAUD_RATE); |
m5171135 | 0:7d55d6ace996 | 66 | |
s1200058 | 10:4a6059c5c84c | 67 | Timer collect_Timer; |
s1200058 | 10:4a6059c5c84c | 68 | const int collect_Time = 10000; //when we collect 4 GPS point, we use |
s1200058 | 10:4a6059c5c84c | 69 | int collect_flag = 0; |
s1200058 | 10:4a6059c5c84c | 70 | char collect_state = 'a'; |
s1200058 | 10:4a6059c5c84c | 71 | XBeeAddress64 collect_Address[4] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), |
s1200058 | 10:4a6059c5c84c | 72 | XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L)}; |
s1200058 | 11:187ff22b343f | 73 | XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L); |
s1200058 | 10:4a6059c5c84c | 74 | int id; |
s1200058 | 10:4a6059c5c84c | 75 | |
s1200058 | 10:4a6059c5c84c | 76 | |
m5171135 | 2:95955f38f47a | 77 | //GPS Send Command |
m5171135 | 2:95955f38f47a | 78 | myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); |
m5171135 | 0:7d55d6ace996 | 79 | myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); |
m5171135 | 0:7d55d6ace996 | 80 | myGPS.sendCommand(PGCMD_ANTENNA); |
m5171135 | 0:7d55d6ace996 | 81 | |
m5171135 | 0:7d55d6ace996 | 82 | wait(2); |
m5171135 | 0:7d55d6ace996 | 83 | |
m5171135 | 0:7d55d6ace996 | 84 | //interrupt start |
m5171135 | 6:21e2792e66c7 | 85 | AigamozuControlPackets agz(agz_motorShield); |
m5171135 | 0:7d55d6ace996 | 86 | refresh_Timer.start(); |
m5171135 | 4:39d2aadf3068 | 87 | |
s1200058 | 10:4a6059c5c84c | 88 | printf("test\n"); |
s1200058 | 10:4a6059c5c84c | 89 | |
m5171135 | 4:39d2aadf3068 | 90 | |
m5171135 | 2:95955f38f47a | 91 | while (true) { |
m5171135 | 0:7d55d6ace996 | 92 | |
m5171135 | 4:39d2aadf3068 | 93 | //Check Xbee Buffer Available |
m5171135 | 0:7d55d6ace996 | 94 | xbee.readPacket(); |
m5171135 | 0:7d55d6ace996 | 95 | if (xbee.getResponse().isAvailable()) { |
m5171135 | 0:7d55d6ace996 | 96 | if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) { |
m5171135 | 0:7d55d6ace996 | 97 | xbee.getResponse().getZBRxResponse(zbRx); |
s1200058 | 10:4a6059c5c84c | 98 | uint8_t *buf = zbRx.getFrameData(); |
s1200058 | 10:4a6059c5c84c | 99 | uint8_t *buf1 = &buf[11]; |
m5171135 | 0:7d55d6ace996 | 100 | |
m5171135 | 2:95955f38f47a | 101 | |
m5171135 | 4:39d2aadf3068 | 102 | //Check Command Type |
m5171135 | 3:1229ca3df855 | 103 | switch(agz.checkCommnadType(buf)){ |
m5171135 | 7:bfc65eac624e | 104 | |
s1200058 | 11:187ff22b343f | 105 | printf("%c\n", buf[14]); |
s1200058 | 11:187ff22b343f | 106 | |
m5171135 | 4:39d2aadf3068 | 107 | //CommandType -> ChanegeMode |
m5171135 | 4:39d2aadf3068 | 108 | case CHANGE_MODE :{ |
m5171135 | 6:21e2792e66c7 | 109 | agz.changeMode(buf); |
m5171135 | 0:7d55d6ace996 | 110 | break; |
m5171135 | 0:7d55d6ace996 | 111 | } |
m5171135 | 4:39d2aadf3068 | 112 | |
m5171135 | 4:39d2aadf3068 | 113 | //CommandType -> Manual |
m5171135 | 3:1229ca3df855 | 114 | case MANUAL:{ |
m5171135 | 4:39d2aadf3068 | 115 | //Check now Mode |
m5171135 | 4:39d2aadf3068 | 116 | if(agz.nowMode == MANUAL_MODE){ |
m5171135 | 6:21e2792e66c7 | 117 | agz.changeSpeed(buf); |
m5171135 | 0:7d55d6ace996 | 118 | } |
m5171135 | 0:7d55d6ace996 | 119 | break; |
m5171135 | 0:7d55d6ace996 | 120 | } |
m5171135 | 0:7d55d6ace996 | 121 | |
m5171135 | 4:39d2aadf3068 | 122 | //CommandType -> Send Status |
m5171135 | 3:1229ca3df855 | 123 | case STATUS_REQUEST:{ |
m5171135 | 4:39d2aadf3068 | 124 | //Create GPS Infomation Packet |
m5171135 | 9:357fa70be9c7 | 125 | agz.createReceiveStatusCommand('B','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); |
m5171135 | 4:39d2aadf3068 | 126 | //Select Destination |
s1200058 | 11:187ff22b343f | 127 | ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength()); |
m5171135 | 4:39d2aadf3068 | 128 | //Send -> Base |
m5171135 | 0:7d55d6ace996 | 129 | xbee.send(tx64request); |
m5171135 | 4:39d2aadf3068 | 130 | break; |
m5171135 | 4:39d2aadf3068 | 131 | |
m5171135 | 0:7d55d6ace996 | 132 | } |
s1200058 | 10:4a6059c5c84c | 133 | |
s1200058 | 11:187ff22b343f | 134 | case RECEIVE_STATUS:{ |
s1200058 | 10:4a6059c5c84c | 135 | |
s1200058 | 11:187ff22b343f | 136 | printf("ok\n"); |
s1200058 | 11:187ff22b343f | 137 | |
s1200058 | 10:4a6059c5c84c | 138 | id = buf1[6] - 'A'; |
s1200058 | 10:4a6059c5c84c | 139 | robot[id].set_state(buf1[9]); |
s1200058 | 10:4a6059c5c84c | 140 | robot[id].set_LatitudeH(&buf1[13]); |
s1200058 | 10:4a6059c5c84c | 141 | robot[id].set_LatitudeL(&buf1[17]); |
s1200058 | 10:4a6059c5c84c | 142 | robot[id].set_LongitudeH(&buf1[21]); |
s1200058 | 10:4a6059c5c84c | 143 | robot[id].set_LongitudeL(&buf1[25]); |
s1200058 | 10:4a6059c5c84c | 144 | |
s1200058 | 11:187ff22b343f | 145 | printf("%d\n", id); |
s1200058 | 11:187ff22b343f | 146 | printf("%ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL()); |
s1200058 | 10:4a6059c5c84c | 147 | break; |
s1200058 | 10:4a6059c5c84c | 148 | } |
m5171135 | 4:39d2aadf3068 | 149 | |
m5171135 | 0:7d55d6ace996 | 150 | default: |
m5171135 | 0:7d55d6ace996 | 151 | { |
m5171135 | 0:7d55d6ace996 | 152 | break; |
m5171135 | 0:7d55d6ace996 | 153 | } |
m5171135 | 0:7d55d6ace996 | 154 | } |
s1200058 | 10:4a6059c5c84c | 155 | |
s1200058 | 10:4a6059c5c84c | 156 | |
m5171135 | 0:7d55d6ace996 | 157 | } |
m5171135 | 0:7d55d6ace996 | 158 | } |
m5171135 | 0:7d55d6ace996 | 159 | |
m5171135 | 0:7d55d6ace996 | 160 | myGPS.read(); |
m5171135 | 0:7d55d6ace996 | 161 | //recive gps module |
m5171135 | 0:7d55d6ace996 | 162 | //check if we recieved a new message from GPS, if so, attempt to parse it, |
m5171135 | 0:7d55d6ace996 | 163 | if ( myGPS.newNMEAreceived() ) { |
m5171135 | 0:7d55d6ace996 | 164 | if ( !myGPS.parse(myGPS.lastNMEA()) ) { |
m5171135 | 0:7d55d6ace996 | 165 | continue; |
m5171135 | 0:7d55d6ace996 | 166 | } |
m5171135 | 0:7d55d6ace996 | 167 | } |
m5171135 | 0:7d55d6ace996 | 168 | |
m5171135 | 0:7d55d6ace996 | 169 | if (refresh_Timer.read_ms() >= refresh_Time) { |
m5171135 | 0:7d55d6ace996 | 170 | refresh_Timer.reset(); |
m5171135 | 8:760cb743e4fc | 171 | if (myGPS.fix) { |
m5171135 | 8:760cb743e4fc | 172 | agz.nowStatus = GPS_AVAIL; |
m5171135 | 8:760cb743e4fc | 173 | agz.reNewPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); |
m5171135 | 8:760cb743e4fc | 174 | } |
m5171135 | 4:39d2aadf3068 | 175 | else agz.nowStatus = GPS_UNAVAIL; |
m5171135 | 9:357fa70be9c7 | 176 | } |
m5171135 | 9:357fa70be9c7 | 177 | |
m5171135 | 9:357fa70be9c7 | 178 | |
s1200058 | 10:4a6059c5c84c | 179 | if( collect_Timer.read_ms() >= collect_Time){ |
s1200058 | 10:4a6059c5c84c | 180 | collect_Timer.reset(); |
s1200058 | 10:4a6059c5c84c | 181 | |
s1200058 | 10:4a6059c5c84c | 182 | printf("Send Request\n"); |
s1200058 | 11:187ff22b343f | 183 | |
s1200058 | 11:187ff22b343f | 184 | agz.createRequestCommand('A', collect_state); |
s1200058 | 10:4a6059c5c84c | 185 | //Select Destination |
s1200058 | 10:4a6059c5c84c | 186 | ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength()); |
s1200058 | 10:4a6059c5c84c | 187 | //Send -> Base |
s1200058 | 10:4a6059c5c84c | 188 | xbee.send(tx64request); |
s1200058 | 10:4a6059c5c84c | 189 | |
s1200058 | 10:4a6059c5c84c | 190 | collect_flag++; |
s1200058 | 10:4a6059c5c84c | 191 | collect_state++; |
s1200058 | 10:4a6059c5c84c | 192 | |
s1200058 | 10:4a6059c5c84c | 193 | if(collect_flag == 4){ |
s1200058 | 11:187ff22b343f | 194 | printf("%d %c\n", collect_flag, collect_state); |
s1200058 | 10:4a6059c5c84c | 195 | collect_state = 'a'; |
s1200058 | 10:4a6059c5c84c | 196 | collect_flag = 0; |
s1200058 | 10:4a6059c5c84c | 197 | } |
s1200058 | 10:4a6059c5c84c | 198 | } |
s1200058 | 10:4a6059c5c84c | 199 | } |
m5171135 | 0:7d55d6ace996 | 200 | } |