![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
robot
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed agz_common
main.cpp@2:95955f38f47a, 2014-05-28 (annotated)
- Committer:
- m5171135
- Date:
- Wed May 28 11:54:22 2014 +0000
- Revision:
- 2:95955f38f47a
- Parent:
- 1:490b793b2e61
- Child:
- 3:1229ca3df855
???
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
m5171135 | 2:95955f38f47a | 1 | /**********************************************/ |
m5171135 | 2:95955f38f47a | 2 | // |
m5171135 | 2:95955f38f47a | 3 | // |
m5171135 | 2:95955f38f47a | 4 | // Program name: Aigamozu Robot Control |
m5171135 | 2:95955f38f47a | 5 | // author: Atsunori Maruyama |
m5171135 | 2:95955f38f47a | 6 | // |
m5171135 | 2:95955f38f47a | 7 | // |
m5171135 | 2:95955f38f47a | 8 | // ver -> 1.0 |
m5171135 | 2:95955f38f47a | 9 | // |
m5171135 | 2:95955f38f47a | 10 | // |
m5171135 | 2:95955f38f47a | 11 | /**********************************************/ |
m5171135 | 2:95955f38f47a | 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 | 1:490b793b2e61 | 16 | #include "VNH5019.h" |
m5171135 | 2:95955f38f47a | 17 | #include "agzIDLIST.h" |
m5171135 | 2:95955f38f47a | 18 | |
m5171135 | 2:95955f38f47a | 19 | |
m5171135 | 2:95955f38f47a | 20 | #define GPS_BAUD_RATE 9600 |
m5171135 | 2:95955f38f47a | 21 | #define XBEE_BAUD_RATE 57600 |
m5171135 | 2:95955f38f47a | 22 | #define PC_BAUD_RATE 57600 |
m5171135 | 2:95955f38f47a | 23 | #define IMU_BAUD_RATE 400000 |
m5171135 | 1:490b793b2e61 | 24 | |
m5171135 | 0:7d55d6ace996 | 25 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 26 | // |
m5171135 | 0:7d55d6ace996 | 27 | //Connection Setting |
m5171135 | 0:7d55d6ace996 | 28 | // |
m5171135 | 0:7d55d6ace996 | 29 | ///////////////////////////////////////// |
m5171135 | 2:95955f38f47a | 30 | |
m5171135 | 2:95955f38f47a | 31 | //Serial Connect Setting: PC <--> mbed |
m5171135 | 2:95955f38f47a | 32 | Serial pc(USBTX, USBRX); |
m5171135 | 2:95955f38f47a | 33 | |
m5171135 | 2:95955f38f47a | 34 | //Serial Connect Setting: GPS <--> mbed |
m5171135 | 0:7d55d6ace996 | 35 | Serial * gps_Serial; |
m5171135 | 2:95955f38f47a | 36 | |
m5171135 | 2:95955f38f47a | 37 | //Serial Connect Setting: XBEE <--> mbed |
m5171135 | 2:95955f38f47a | 38 | XBee xbee(p13,p14); |
m5171135 | 0:7d55d6ace996 | 39 | ZBRxResponse zbRx = ZBRxResponse(); |
m5171135 | 2:95955f38f47a | 40 | XBeeAddress64 remoteAddress = XBeeAddress64(PAN1B1_32H,PAN1B1_32L); |
m5171135 | 0:7d55d6ace996 | 41 | |
m5171135 | 0:7d55d6ace996 | 42 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 43 | // |
m5171135 | 0:7d55d6ace996 | 44 | //Pin Setting |
m5171135 | 0:7d55d6ace996 | 45 | // |
m5171135 | 0:7d55d6ace996 | 46 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 47 | |
m5171135 | 2:95955f38f47a | 48 | //Motor Contorol Pin Setting |
m5171135 | 1:490b793b2e61 | 49 | VNH5019 motorShield(p21,p22,p23,p24,p25,p26); |
m5171135 | 0:7d55d6ace996 | 50 | |
m5171135 | 2:95955f38f47a | 51 | //Interrupt Setting |
m5171135 | 0:7d55d6ace996 | 52 | Ticker axis; |
m5171135 | 0:7d55d6ace996 | 53 | Ticker auth_axis; |
m5171135 | 0:7d55d6ace996 | 54 | |
m5171135 | 0:7d55d6ace996 | 55 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 56 | // |
m5171135 | 0:7d55d6ace996 | 57 | //Each Value Setting |
m5171135 | 0:7d55d6ace996 | 58 | // |
m5171135 | 0:7d55d6ace996 | 59 | ///////////////////////////////////////// |
m5171135 | 2:95955f38f47a | 60 | int count = 0; |
m5171135 | 2:95955f38f47a | 61 | |
m5171135 | 0:7d55d6ace996 | 62 | //my status |
m5171135 | 0:7d55d6ace996 | 63 | //0: StndbyMode |
m5171135 | 0:7d55d6ace996 | 64 | //1: ManualMode |
m5171135 | 0:7d55d6ace996 | 65 | //2: AuthmaticMode(Random) |
m5171135 | 0:7d55d6ace996 | 66 | unsigned char my_status = 0; |
m5171135 | 0:7d55d6ace996 | 67 | |
m5171135 | 0:7d55d6ace996 | 68 | //0 bit: Motor Satatus |
m5171135 | 0:7d55d6ace996 | 69 | //1 bit: GPS Status |
m5171135 | 0:7d55d6ace996 | 70 | //2 bit: Sensor Status |
m5171135 | 0:7d55d6ace996 | 71 | //3 bit: Battery Status |
m5171135 | 0:7d55d6ace996 | 72 | unsigned char my_mode = 0; |
m5171135 | 0:7d55d6ace996 | 73 | |
m5171135 | 0:7d55d6ace996 | 74 | //ManualValue |
m5171135 | 0:7d55d6ace996 | 75 | int manual_count = 0; |
m5171135 | 0:7d55d6ace996 | 76 | int manual_flag = 0; |
m5171135 | 0:7d55d6ace996 | 77 | |
m5171135 | 0:7d55d6ace996 | 78 | //test value |
m5171135 | 0:7d55d6ace996 | 79 | long sub_latH = 12345; |
m5171135 | 0:7d55d6ace996 | 80 | long sub_latL = 67890; |
m5171135 | 0:7d55d6ace996 | 81 | long sub_lonH = 98765; |
m5171135 | 0:7d55d6ace996 | 82 | long sub_lonL = 43211; |
m5171135 | 0:7d55d6ace996 | 83 | |
m5171135 | 0:7d55d6ace996 | 84 | |
m5171135 | 0:7d55d6ace996 | 85 | union UNI_TEST_T |
m5171135 | 0:7d55d6ace996 | 86 | { |
m5171135 | 0:7d55d6ace996 | 87 | long a; |
m5171135 | 0:7d55d6ace996 | 88 | uint8_t b[4]; |
m5171135 | 0:7d55d6ace996 | 89 | }; |
m5171135 | 0:7d55d6ace996 | 90 | |
m5171135 | 0:7d55d6ace996 | 91 | UNI_TEST_T latH,latL,lonH,lonL; |
m5171135 | 0:7d55d6ace996 | 92 | |
m5171135 | 2:95955f38f47a | 93 | |
m5171135 | 0:7d55d6ace996 | 94 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 95 | // |
m5171135 | 0:7d55d6ace996 | 96 | //Interruption processing 1: time -> 0.1s |
m5171135 | 0:7d55d6ace996 | 97 | // |
m5171135 | 0:7d55d6ace996 | 98 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 99 | void axisRenovation(){ |
m5171135 | 0:7d55d6ace996 | 100 | |
m5171135 | 0:7d55d6ace996 | 101 | if(manual_count > 100){ |
m5171135 | 1:490b793b2e61 | 102 | motorShield.changeSpeed(0,0,0,0); |
m5171135 | 0:7d55d6ace996 | 103 | manual_count = 0; |
m5171135 | 0:7d55d6ace996 | 104 | } |
m5171135 | 2:95955f38f47a | 105 | if(my_mode == 1) manual_count++; |
m5171135 | 0:7d55d6ace996 | 106 | } |
m5171135 | 0:7d55d6ace996 | 107 | |
m5171135 | 0:7d55d6ace996 | 108 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 109 | // |
m5171135 | 0:7d55d6ace996 | 110 | //Interruption processing: time -> 1.0s |
m5171135 | 0:7d55d6ace996 | 111 | // |
m5171135 | 0:7d55d6ace996 | 112 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 113 | |
m5171135 | 0:7d55d6ace996 | 114 | void randomRenovation(){ |
m5171135 | 0:7d55d6ace996 | 115 | |
m5171135 | 2:95955f38f47a | 116 | if(count < 20){ |
m5171135 | 1:490b793b2e61 | 117 | motorShield.changeSpeed(1,127,1,127); |
m5171135 | 0:7d55d6ace996 | 118 | } |
m5171135 | 0:7d55d6ace996 | 119 | |
m5171135 | 0:7d55d6ace996 | 120 | else{ |
m5171135 | 2:95955f38f47a | 121 | motorShield.changeSpeed(1,64,2,64); |
m5171135 | 2:95955f38f47a | 122 | if(count > 21) { |
m5171135 | 2:95955f38f47a | 123 | |
m5171135 | 2:95955f38f47a | 124 | count = 0; |
m5171135 | 2:95955f38f47a | 125 | motorShield.changeSpeed(1,127,1,127); |
m5171135 | 2:95955f38f47a | 126 | |
m5171135 | 2:95955f38f47a | 127 | } |
m5171135 | 0:7d55d6ace996 | 128 | } |
m5171135 | 0:7d55d6ace996 | 129 | count++; |
m5171135 | 0:7d55d6ace996 | 130 | } |
m5171135 | 0:7d55d6ace996 | 131 | |
m5171135 | 0:7d55d6ace996 | 132 | |
m5171135 | 0:7d55d6ace996 | 133 | |
m5171135 | 0:7d55d6ace996 | 134 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 135 | // |
m5171135 | 0:7d55d6ace996 | 136 | //Main Processing |
m5171135 | 0:7d55d6ace996 | 137 | // |
m5171135 | 0:7d55d6ace996 | 138 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 139 | int main() { |
m5171135 | 0:7d55d6ace996 | 140 | //start up time |
m5171135 | 0:7d55d6ace996 | 141 | wait(3); |
m5171135 | 0:7d55d6ace996 | 142 | //set pc frequency to 57600bps |
m5171135 | 2:95955f38f47a | 143 | pc.baud(PC_BAUD_RATE); |
m5171135 | 0:7d55d6ace996 | 144 | //set xbee frequency to 57600bps |
m5171135 | 2:95955f38f47a | 145 | xbee.begin(XBEE_BAUD_RATE); |
m5171135 | 0:7d55d6ace996 | 146 | |
m5171135 | 0:7d55d6ace996 | 147 | //GPS setting |
m5171135 | 2:95955f38f47a | 148 | gps_Serial = new Serial(p28,p27); |
m5171135 | 2:95955f38f47a | 149 | Adafruit_GPS myGPS(gps_Serial); |
m5171135 | 2:95955f38f47a | 150 | Timer refresh_Timer; |
m5171135 | 0:7d55d6ace996 | 151 | const int refresh_Time = 2000; //refresh time in ms |
m5171135 | 2:95955f38f47a | 152 | myGPS.begin(GPS_BAUD_RATE); |
m5171135 | 0:7d55d6ace996 | 153 | |
m5171135 | 2:95955f38f47a | 154 | //GPS Send Command |
m5171135 | 2:95955f38f47a | 155 | myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); |
m5171135 | 0:7d55d6ace996 | 156 | myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); |
m5171135 | 0:7d55d6ace996 | 157 | myGPS.sendCommand(PGCMD_ANTENNA); |
m5171135 | 0:7d55d6ace996 | 158 | |
m5171135 | 0:7d55d6ace996 | 159 | //gyro_registor Setting |
m5171135 | 0:7d55d6ace996 | 160 | wait(2); |
m5171135 | 0:7d55d6ace996 | 161 | |
m5171135 | 0:7d55d6ace996 | 162 | //interrupt start |
m5171135 | 0:7d55d6ace996 | 163 | axis.attach(&axisRenovation, 0.1); |
m5171135 | 0:7d55d6ace996 | 164 | refresh_Timer.start(); |
m5171135 | 0:7d55d6ace996 | 165 | |
m5171135 | 2:95955f38f47a | 166 | /// @todo change to true |
m5171135 | 2:95955f38f47a | 167 | while (true) { |
m5171135 | 0:7d55d6ace996 | 168 | |
m5171135 | 0:7d55d6ace996 | 169 | //recive xbee module |
m5171135 | 0:7d55d6ace996 | 170 | xbee.readPacket(); |
m5171135 | 0:7d55d6ace996 | 171 | if (xbee.getResponse().isAvailable()) { |
m5171135 | 0:7d55d6ace996 | 172 | if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) { |
m5171135 | 0:7d55d6ace996 | 173 | xbee.getResponse().getZBRxResponse(zbRx); |
m5171135 | 0:7d55d6ace996 | 174 | unsigned char *buf = zbRx.getFrameData(); |
m5171135 | 0:7d55d6ace996 | 175 | |
m5171135 | 2:95955f38f47a | 176 | /// @todo use preporcessor (#define) or const. |
m5171135 | 2:95955f38f47a | 177 | /// @note It's magic number. |
m5171135 | 2:95955f38f47a | 178 | // COMMAND_TYPE commandType = buf[14]; |
m5171135 | 2:95955f38f47a | 179 | /// switch(commandType) |
m5171135 | 2:95955f38f47a | 180 | /// {case MANUAL:} |
m5171135 | 2:95955f38f47a | 181 | |
m5171135 | 2:95955f38f47a | 182 | |
m5171135 | 2:95955f38f47a | 183 | //switch(aigamoCTR.checkCommandType(buf)){ |
m5171135 | 0:7d55d6ace996 | 184 | switch((unsigned char)buf[14]){ |
m5171135 | 0:7d55d6ace996 | 185 | //ChanegeMode |
m5171135 | 0:7d55d6ace996 | 186 | case 'C':{ |
m5171135 | 0:7d55d6ace996 | 187 | my_mode = buf[19]; |
m5171135 | 1:490b793b2e61 | 188 | motorShield.changeSpeed(0,0,0,0); |
m5171135 | 0:7d55d6ace996 | 189 | if(my_mode == 2) auth_axis.attach(&randomRenovation, 1.0); |
m5171135 | 0:7d55d6ace996 | 190 | else auth_axis.detach(); |
m5171135 | 0:7d55d6ace996 | 191 | break; |
m5171135 | 0:7d55d6ace996 | 192 | } |
m5171135 | 0:7d55d6ace996 | 193 | |
m5171135 | 2:95955f38f47a | 194 | /* |
m5171135 | 2:95955f38f47a | 195 | case 'C': |
m5171135 | 2:95955f38f47a | 196 | agz.change_Mode(); |
m5171135 | 2:95955f38f47a | 197 | break; |
m5171135 | 2:95955f38f47a | 198 | */ |
m5171135 | 2:95955f38f47a | 199 | |
m5171135 | 0:7d55d6ace996 | 200 | //MunualCommand |
m5171135 | 0:7d55d6ace996 | 201 | case 'M':{ |
m5171135 | 2:95955f38f47a | 202 | /// @todo Magic. |
m5171135 | 0:7d55d6ace996 | 203 | if(my_mode == 1){ |
m5171135 | 0:7d55d6ace996 | 204 | manual_count = 0; |
m5171135 | 1:490b793b2e61 | 205 | motorShield.changeSpeed(buf[20],buf[21],buf[23],buf[24]); |
m5171135 | 0:7d55d6ace996 | 206 | } |
m5171135 | 0:7d55d6ace996 | 207 | break; |
m5171135 | 0:7d55d6ace996 | 208 | } |
m5171135 | 0:7d55d6ace996 | 209 | |
m5171135 | 2:95955f38f47a | 210 | /* |
m5171135 | 2:95955f38f47a | 211 | case 'M': |
m5171135 | 2:95955f38f47a | 212 | uint8_t sppeds_buf[4] = agz.parseMunualCommand(buf); |
m5171135 | 2:95955f38f47a | 213 | motorShield.changeSpeed(sppedbuf[0],sppedbuf[1],sppedbuf[2],sppedbuf[3]); |
m5171135 | 2:95955f38f47a | 214 | break; |
m5171135 | 2:95955f38f47a | 215 | */ |
m5171135 | 2:95955f38f47a | 216 | |
m5171135 | 0:7d55d6ace996 | 217 | //SendStatus |
m5171135 | 0:7d55d6ace996 | 218 | case 'S':{ |
m5171135 | 2:95955f38f47a | 219 | latH.a = myGPS.latitudeH;; |
m5171135 | 2:95955f38f47a | 220 | latL.a = myGPS.latitudeL; |
m5171135 | 2:95955f38f47a | 221 | lonH.a = myGPS.longitudeH; |
m5171135 | 2:95955f38f47a | 222 | lonL.a = myGPS.longitudeL; |
m5171135 | 0:7d55d6ace996 | 223 | //dummy data |
m5171135 | 2:95955f38f47a | 224 | //latH.a = sub_latH;latL.a = sub_latL;lonH.a = sub_lonH;lonL.a = sub_lonL; |
m5171135 | 2:95955f38f47a | 225 | |
m5171135 | 2:95955f38f47a | 226 | // if(validateCommand(commandType)); |
m5171135 | 2:95955f38f47a | 227 | // createReceiveStatusData(); |
m5171135 | 2:95955f38f47a | 228 | 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}; |
m5171135 | 0:7d55d6ace996 | 229 | ZBTxRequest tx64request(remoteAddress,data,sizeof(data)); |
m5171135 | 0:7d55d6ace996 | 230 | xbee.send(tx64request); |
m5171135 | 2:95955f38f47a | 231 | break; |
m5171135 | 2:95955f38f47a | 232 | /* |
m5171135 | 2:95955f38f47a | 233 | case 'S': |
m5171135 | 2:95955f38f47a | 234 | */ |
m5171135 | 0:7d55d6ace996 | 235 | } |
m5171135 | 0:7d55d6ace996 | 236 | |
m5171135 | 0:7d55d6ace996 | 237 | default: |
m5171135 | 0:7d55d6ace996 | 238 | { |
m5171135 | 0:7d55d6ace996 | 239 | break; |
m5171135 | 0:7d55d6ace996 | 240 | } |
m5171135 | 0:7d55d6ace996 | 241 | } |
m5171135 | 0:7d55d6ace996 | 242 | } |
m5171135 | 0:7d55d6ace996 | 243 | } |
m5171135 | 0:7d55d6ace996 | 244 | |
m5171135 | 0:7d55d6ace996 | 245 | myGPS.read(); |
m5171135 | 0:7d55d6ace996 | 246 | //recive gps module |
m5171135 | 0:7d55d6ace996 | 247 | //check if we recieved a new message from GPS, if so, attempt to parse it, |
m5171135 | 0:7d55d6ace996 | 248 | if ( myGPS.newNMEAreceived() ) { |
m5171135 | 0:7d55d6ace996 | 249 | if ( !myGPS.parse(myGPS.lastNMEA()) ) { |
m5171135 | 0:7d55d6ace996 | 250 | continue; |
m5171135 | 0:7d55d6ace996 | 251 | } |
m5171135 | 0:7d55d6ace996 | 252 | } |
m5171135 | 0:7d55d6ace996 | 253 | |
m5171135 | 0:7d55d6ace996 | 254 | if (refresh_Timer.read_ms() >= refresh_Time) { |
m5171135 | 0:7d55d6ace996 | 255 | refresh_Timer.reset(); |
m5171135 | 2:95955f38f47a | 256 | if (myGPS.fix) my_status = 0; |
m5171135 | 0:7d55d6ace996 | 257 | else my_status = 1; |
m5171135 | 0:7d55d6ace996 | 258 | } |
m5171135 | 0:7d55d6ace996 | 259 | } |
m5171135 | 0:7d55d6ace996 | 260 | } |