robot
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed agz_common
main.cpp@3:1229ca3df855, 2014-05-30 (annotated)
- Committer:
- m5171135
- Date:
- Fri May 30 15:36:33 2014 +0000
- Revision:
- 3:1229ca3df855
- Parent:
- 2:95955f38f47a
- Child:
- 4:39d2aadf3068
2014/5/30 Atsunori Maruyama; add library "AigamozuPacketControl"
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 | 2:95955f38f47a | 6 | // author: Atsunori Maruyama |
m5171135 | 3:1229ca3df855 | 7 | // ver -> 1.0 |
m5171135 | 3:1229ca3df855 | 8 | // day -> 2014/05/21 |
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 | 3:1229ca3df855 | 18 | #include "aigamozuSetting.h" |
m5171135 | 3:1229ca3df855 | 19 | #include "AigamozuControlPackets.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 | 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 | 2:95955f38f47a | 42 | //Motor Contorol Pin Setting |
m5171135 | 1:490b793b2e61 | 43 | VNH5019 motorShield(p21,p22,p23,p24,p25,p26); |
m5171135 | 3:1229ca3df855 | 44 | AigamozuControlPackets agz; |
m5171135 | 3:1229ca3df855 | 45 | |
m5171135 | 0:7d55d6ace996 | 46 | |
m5171135 | 2:95955f38f47a | 47 | //Interrupt Setting |
m5171135 | 0:7d55d6ace996 | 48 | Ticker axis; |
m5171135 | 0:7d55d6ace996 | 49 | Ticker auth_axis; |
m5171135 | 0:7d55d6ace996 | 50 | |
m5171135 | 0:7d55d6ace996 | 51 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 52 | // |
m5171135 | 0:7d55d6ace996 | 53 | //Each Value Setting |
m5171135 | 0:7d55d6ace996 | 54 | // |
m5171135 | 0:7d55d6ace996 | 55 | ///////////////////////////////////////// |
m5171135 | 2:95955f38f47a | 56 | int count = 0; |
m5171135 | 2:95955f38f47a | 57 | |
m5171135 | 0:7d55d6ace996 | 58 | //my status |
m5171135 | 0:7d55d6ace996 | 59 | //0: StndbyMode |
m5171135 | 0:7d55d6ace996 | 60 | //1: ManualMode |
m5171135 | 0:7d55d6ace996 | 61 | //2: AuthmaticMode(Random) |
m5171135 | 0:7d55d6ace996 | 62 | unsigned char my_status = 0; |
m5171135 | 0:7d55d6ace996 | 63 | |
m5171135 | 0:7d55d6ace996 | 64 | //0 bit: Motor Satatus |
m5171135 | 0:7d55d6ace996 | 65 | //1 bit: GPS Status |
m5171135 | 0:7d55d6ace996 | 66 | //2 bit: Sensor Status |
m5171135 | 0:7d55d6ace996 | 67 | //3 bit: Battery Status |
m5171135 | 0:7d55d6ace996 | 68 | unsigned char my_mode = 0; |
m5171135 | 0:7d55d6ace996 | 69 | |
m5171135 | 0:7d55d6ace996 | 70 | //ManualValue |
m5171135 | 0:7d55d6ace996 | 71 | int manual_count = 0; |
m5171135 | 0:7d55d6ace996 | 72 | int manual_flag = 0; |
m5171135 | 0:7d55d6ace996 | 73 | |
m5171135 | 0:7d55d6ace996 | 74 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 75 | // |
m5171135 | 0:7d55d6ace996 | 76 | //Interruption processing 1: time -> 0.1s |
m5171135 | 0:7d55d6ace996 | 77 | // |
m5171135 | 0:7d55d6ace996 | 78 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 79 | void axisRenovation(){ |
m5171135 | 0:7d55d6ace996 | 80 | |
m5171135 | 0:7d55d6ace996 | 81 | if(manual_count > 100){ |
m5171135 | 1:490b793b2e61 | 82 | motorShield.changeSpeed(0,0,0,0); |
m5171135 | 0:7d55d6ace996 | 83 | manual_count = 0; |
m5171135 | 0:7d55d6ace996 | 84 | } |
m5171135 | 2:95955f38f47a | 85 | if(my_mode == 1) manual_count++; |
m5171135 | 0:7d55d6ace996 | 86 | } |
m5171135 | 0:7d55d6ace996 | 87 | |
m5171135 | 0:7d55d6ace996 | 88 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 89 | // |
m5171135 | 0:7d55d6ace996 | 90 | //Interruption processing: time -> 1.0s |
m5171135 | 0:7d55d6ace996 | 91 | // |
m5171135 | 0:7d55d6ace996 | 92 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 93 | |
m5171135 | 0:7d55d6ace996 | 94 | void randomRenovation(){ |
m5171135 | 0:7d55d6ace996 | 95 | |
m5171135 | 2:95955f38f47a | 96 | if(count < 20){ |
m5171135 | 1:490b793b2e61 | 97 | motorShield.changeSpeed(1,127,1,127); |
m5171135 | 0:7d55d6ace996 | 98 | } |
m5171135 | 0:7d55d6ace996 | 99 | |
m5171135 | 0:7d55d6ace996 | 100 | else{ |
m5171135 | 2:95955f38f47a | 101 | motorShield.changeSpeed(1,64,2,64); |
m5171135 | 2:95955f38f47a | 102 | if(count > 21) { |
m5171135 | 2:95955f38f47a | 103 | |
m5171135 | 2:95955f38f47a | 104 | count = 0; |
m5171135 | 2:95955f38f47a | 105 | motorShield.changeSpeed(1,127,1,127); |
m5171135 | 2:95955f38f47a | 106 | |
m5171135 | 2:95955f38f47a | 107 | } |
m5171135 | 0:7d55d6ace996 | 108 | } |
m5171135 | 0:7d55d6ace996 | 109 | count++; |
m5171135 | 0:7d55d6ace996 | 110 | } |
m5171135 | 0:7d55d6ace996 | 111 | |
m5171135 | 0:7d55d6ace996 | 112 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 113 | // |
m5171135 | 0:7d55d6ace996 | 114 | //Main Processing |
m5171135 | 0:7d55d6ace996 | 115 | // |
m5171135 | 0:7d55d6ace996 | 116 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 117 | int main() { |
m5171135 | 0:7d55d6ace996 | 118 | //start up time |
m5171135 | 0:7d55d6ace996 | 119 | wait(3); |
m5171135 | 0:7d55d6ace996 | 120 | //set pc frequency to 57600bps |
m5171135 | 2:95955f38f47a | 121 | pc.baud(PC_BAUD_RATE); |
m5171135 | 0:7d55d6ace996 | 122 | //set xbee frequency to 57600bps |
m5171135 | 2:95955f38f47a | 123 | xbee.begin(XBEE_BAUD_RATE); |
m5171135 | 0:7d55d6ace996 | 124 | |
m5171135 | 0:7d55d6ace996 | 125 | //GPS setting |
m5171135 | 2:95955f38f47a | 126 | gps_Serial = new Serial(p28,p27); |
m5171135 | 2:95955f38f47a | 127 | Adafruit_GPS myGPS(gps_Serial); |
m5171135 | 2:95955f38f47a | 128 | Timer refresh_Timer; |
m5171135 | 0:7d55d6ace996 | 129 | const int refresh_Time = 2000; //refresh time in ms |
m5171135 | 2:95955f38f47a | 130 | myGPS.begin(GPS_BAUD_RATE); |
m5171135 | 0:7d55d6ace996 | 131 | |
m5171135 | 2:95955f38f47a | 132 | //GPS Send Command |
m5171135 | 2:95955f38f47a | 133 | myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); |
m5171135 | 0:7d55d6ace996 | 134 | myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); |
m5171135 | 0:7d55d6ace996 | 135 | myGPS.sendCommand(PGCMD_ANTENNA); |
m5171135 | 0:7d55d6ace996 | 136 | |
m5171135 | 0:7d55d6ace996 | 137 | //gyro_registor Setting |
m5171135 | 0:7d55d6ace996 | 138 | wait(2); |
m5171135 | 0:7d55d6ace996 | 139 | |
m5171135 | 0:7d55d6ace996 | 140 | //interrupt start |
m5171135 | 0:7d55d6ace996 | 141 | axis.attach(&axisRenovation, 0.1); |
m5171135 | 0:7d55d6ace996 | 142 | refresh_Timer.start(); |
m5171135 | 3:1229ca3df855 | 143 | |
m5171135 | 2:95955f38f47a | 144 | while (true) { |
m5171135 | 0:7d55d6ace996 | 145 | |
m5171135 | 0:7d55d6ace996 | 146 | //recive xbee module |
m5171135 | 0:7d55d6ace996 | 147 | xbee.readPacket(); |
m5171135 | 0:7d55d6ace996 | 148 | if (xbee.getResponse().isAvailable()) { |
m5171135 | 0:7d55d6ace996 | 149 | if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) { |
m5171135 | 0:7d55d6ace996 | 150 | xbee.getResponse().getZBRxResponse(zbRx); |
m5171135 | 0:7d55d6ace996 | 151 | unsigned char *buf = zbRx.getFrameData(); |
m5171135 | 0:7d55d6ace996 | 152 | |
m5171135 | 2:95955f38f47a | 153 | |
m5171135 | 3:1229ca3df855 | 154 | //Analyze the command type |
m5171135 | 3:1229ca3df855 | 155 | switch(agz.checkCommnadType(buf)){ |
m5171135 | 3:1229ca3df855 | 156 | |
m5171135 | 3:1229ca3df855 | 157 | //CommnadType -> ChangeModeCommand;;; |
m5171135 | 3:1229ca3df855 | 158 | case CHANGE_MODE:{ |
m5171135 | 0:7d55d6ace996 | 159 | my_mode = buf[19]; |
m5171135 | 1:490b793b2e61 | 160 | motorShield.changeSpeed(0,0,0,0); |
m5171135 | 0:7d55d6ace996 | 161 | if(my_mode == 2) auth_axis.attach(&randomRenovation, 1.0); |
m5171135 | 0:7d55d6ace996 | 162 | else auth_axis.detach(); |
m5171135 | 0:7d55d6ace996 | 163 | break; |
m5171135 | 0:7d55d6ace996 | 164 | } |
m5171135 | 0:7d55d6ace996 | 165 | |
m5171135 | 3:1229ca3df855 | 166 | //CommnadType -> ManualCommand;;; |
m5171135 | 3:1229ca3df855 | 167 | case MANUAL:{ |
m5171135 | 2:95955f38f47a | 168 | /// @todo Magic. |
m5171135 | 0:7d55d6ace996 | 169 | if(my_mode == 1){ |
m5171135 | 0:7d55d6ace996 | 170 | manual_count = 0; |
m5171135 | 1:490b793b2e61 | 171 | motorShield.changeSpeed(buf[20],buf[21],buf[23],buf[24]); |
m5171135 | 0:7d55d6ace996 | 172 | } |
m5171135 | 0:7d55d6ace996 | 173 | break; |
m5171135 | 0:7d55d6ace996 | 174 | } |
m5171135 | 0:7d55d6ace996 | 175 | |
m5171135 | 0:7d55d6ace996 | 176 | //SendStatus |
m5171135 | 3:1229ca3df855 | 177 | case STATUS_REQUEST:{ |
m5171135 | 3:1229ca3df855 | 178 | //Create packet |
m5171135 | 3:1229ca3df855 | 179 | uint8_t* data = agz.createStatusPacket(myGPS.latitudeH, myGPS.latitudeL, myGPS.longitudeH, myGPS.longitudeL,1,2); |
m5171135 | 0:7d55d6ace996 | 180 | ZBTxRequest tx64request(remoteAddress,data,sizeof(data)); |
m5171135 | 3:1229ca3df855 | 181 | //Send GPS ingomation |
m5171135 | 0:7d55d6ace996 | 182 | xbee.send(tx64request); |
m5171135 | 3:1229ca3df855 | 183 | break; |
m5171135 | 0:7d55d6ace996 | 184 | } |
m5171135 | 3:1229ca3df855 | 185 | |
m5171135 | 3:1229ca3df855 | 186 | //Default |
m5171135 | 0:7d55d6ace996 | 187 | default: |
m5171135 | 0:7d55d6ace996 | 188 | { |
m5171135 | 0:7d55d6ace996 | 189 | break; |
m5171135 | 0:7d55d6ace996 | 190 | } |
m5171135 | 0:7d55d6ace996 | 191 | } |
m5171135 | 0:7d55d6ace996 | 192 | } |
m5171135 | 0:7d55d6ace996 | 193 | } |
m5171135 | 3:1229ca3df855 | 194 | |
m5171135 | 0:7d55d6ace996 | 195 | |
m5171135 | 0:7d55d6ace996 | 196 | myGPS.read(); |
m5171135 | 0:7d55d6ace996 | 197 | //recive gps module |
m5171135 | 0:7d55d6ace996 | 198 | //check if we recieved a new message from GPS, if so, attempt to parse it, |
m5171135 | 0:7d55d6ace996 | 199 | if ( myGPS.newNMEAreceived() ) { |
m5171135 | 0:7d55d6ace996 | 200 | if ( !myGPS.parse(myGPS.lastNMEA()) ) { |
m5171135 | 0:7d55d6ace996 | 201 | continue; |
m5171135 | 0:7d55d6ace996 | 202 | } |
m5171135 | 0:7d55d6ace996 | 203 | } |
m5171135 | 0:7d55d6ace996 | 204 | |
m5171135 | 0:7d55d6ace996 | 205 | if (refresh_Timer.read_ms() >= refresh_Time) { |
m5171135 | 0:7d55d6ace996 | 206 | refresh_Timer.reset(); |
m5171135 | 2:95955f38f47a | 207 | if (myGPS.fix) my_status = 0; |
m5171135 | 0:7d55d6ace996 | 208 | else my_status = 1; |
m5171135 | 0:7d55d6ace996 | 209 | } |
m5171135 | 0:7d55d6ace996 | 210 | } |
m5171135 | 0:7d55d6ace996 | 211 | } |