yokokawa
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of aigamozu_program_ver2 by
main.cpp@6:21e2792e66c7, 2014-06-08 (annotated)
- Committer:
- m5171135
- Date:
- Sun Jun 08 15:09:17 2014 +0000
- Revision:
- 6:21e2792e66c7
- Parent:
- 5:940424ec98a8
- Child:
- 7:bfc65eac624e
ver 2.0
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 | 5:940424ec98a8 | 7 | // ver -> 1.1 |
m5171135 | 5:940424ec98a8 | 8 | // day -> 2014/06/01 |
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 | Ticker hogehoge; |
m5171135 | 6:21e2792e66c7 | 43 | VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26); |
m5171135 | 0:7d55d6ace996 | 44 | |
m5171135 | 0:7d55d6ace996 | 45 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 46 | // |
m5171135 | 0:7d55d6ace996 | 47 | //Each Value Setting |
m5171135 | 0:7d55d6ace996 | 48 | // |
m5171135 | 0:7d55d6ace996 | 49 | ///////////////////////////////////////// |
m5171135 | 2:95955f38f47a | 50 | int count = 0; |
m5171135 | 2:95955f38f47a | 51 | |
m5171135 | 0:7d55d6ace996 | 52 | //my status |
m5171135 | 0:7d55d6ace996 | 53 | //0: StndbyMode |
m5171135 | 0:7d55d6ace996 | 54 | //1: ManualMode |
m5171135 | 0:7d55d6ace996 | 55 | //2: AuthmaticMode(Random) |
m5171135 | 0:7d55d6ace996 | 56 | unsigned char my_status = 0; |
m5171135 | 0:7d55d6ace996 | 57 | |
m5171135 | 0:7d55d6ace996 | 58 | //0 bit: Motor Satatus |
m5171135 | 0:7d55d6ace996 | 59 | //1 bit: GPS Status |
m5171135 | 0:7d55d6ace996 | 60 | //2 bit: Sensor Status |
m5171135 | 0:7d55d6ace996 | 61 | //3 bit: Battery Status |
m5171135 | 0:7d55d6ace996 | 62 | unsigned char my_mode = 0; |
m5171135 | 0:7d55d6ace996 | 63 | |
m5171135 | 0:7d55d6ace996 | 64 | //ManualValue |
m5171135 | 0:7d55d6ace996 | 65 | int manual_count = 0; |
m5171135 | 0:7d55d6ace996 | 66 | int manual_flag = 0; |
m5171135 | 0:7d55d6ace996 | 67 | |
m5171135 | 5:940424ec98a8 | 68 | |
m5171135 | 0:7d55d6ace996 | 69 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 70 | // |
m5171135 | 0:7d55d6ace996 | 71 | //Main Processing |
m5171135 | 0:7d55d6ace996 | 72 | // |
m5171135 | 0:7d55d6ace996 | 73 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 74 | int main() { |
m5171135 | 0:7d55d6ace996 | 75 | //start up time |
m5171135 | 0:7d55d6ace996 | 76 | wait(3); |
m5171135 | 0:7d55d6ace996 | 77 | //set pc frequency to 57600bps |
m5171135 | 2:95955f38f47a | 78 | pc.baud(PC_BAUD_RATE); |
m5171135 | 0:7d55d6ace996 | 79 | //set xbee frequency to 57600bps |
m5171135 | 2:95955f38f47a | 80 | xbee.begin(XBEE_BAUD_RATE); |
m5171135 | 0:7d55d6ace996 | 81 | |
m5171135 | 0:7d55d6ace996 | 82 | //GPS setting |
m5171135 | 2:95955f38f47a | 83 | gps_Serial = new Serial(p28,p27); |
m5171135 | 2:95955f38f47a | 84 | Adafruit_GPS myGPS(gps_Serial); |
m5171135 | 2:95955f38f47a | 85 | Timer refresh_Timer; |
m5171135 | 0:7d55d6ace996 | 86 | const int refresh_Time = 2000; //refresh time in ms |
m5171135 | 2:95955f38f47a | 87 | myGPS.begin(GPS_BAUD_RATE); |
m5171135 | 0:7d55d6ace996 | 88 | |
m5171135 | 2:95955f38f47a | 89 | //GPS Send Command |
m5171135 | 2:95955f38f47a | 90 | myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); |
m5171135 | 0:7d55d6ace996 | 91 | myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); |
m5171135 | 0:7d55d6ace996 | 92 | myGPS.sendCommand(PGCMD_ANTENNA); |
m5171135 | 0:7d55d6ace996 | 93 | |
m5171135 | 0:7d55d6ace996 | 94 | wait(2); |
m5171135 | 0:7d55d6ace996 | 95 | |
m5171135 | 0:7d55d6ace996 | 96 | //interrupt start |
m5171135 | 6:21e2792e66c7 | 97 | AigamozuControlPackets agz(agz_motorShield); |
m5171135 | 0:7d55d6ace996 | 98 | refresh_Timer.start(); |
m5171135 | 4:39d2aadf3068 | 99 | |
m5171135 | 4:39d2aadf3068 | 100 | |
m5171135 | 2:95955f38f47a | 101 | while (true) { |
m5171135 | 0:7d55d6ace996 | 102 | |
m5171135 | 4:39d2aadf3068 | 103 | //Check Xbee Buffer Available |
m5171135 | 0:7d55d6ace996 | 104 | xbee.readPacket(); |
m5171135 | 0:7d55d6ace996 | 105 | if (xbee.getResponse().isAvailable()) { |
m5171135 | 0:7d55d6ace996 | 106 | if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) { |
m5171135 | 0:7d55d6ace996 | 107 | xbee.getResponse().getZBRxResponse(zbRx); |
m5171135 | 5:940424ec98a8 | 108 | uint8_t *buf = zbRx.getFrameData(); |
m5171135 | 0:7d55d6ace996 | 109 | |
m5171135 | 2:95955f38f47a | 110 | |
m5171135 | 4:39d2aadf3068 | 111 | //Check Command Type |
m5171135 | 3:1229ca3df855 | 112 | switch(agz.checkCommnadType(buf)){ |
m5171135 | 4:39d2aadf3068 | 113 | //CommandType -> ChanegeMode |
m5171135 | 4:39d2aadf3068 | 114 | case CHANGE_MODE :{ |
m5171135 | 6:21e2792e66c7 | 115 | agz.changeMode(buf); |
m5171135 | 0:7d55d6ace996 | 116 | break; |
m5171135 | 0:7d55d6ace996 | 117 | } |
m5171135 | 4:39d2aadf3068 | 118 | |
m5171135 | 4:39d2aadf3068 | 119 | //CommandType -> Manual |
m5171135 | 3:1229ca3df855 | 120 | case MANUAL:{ |
m5171135 | 4:39d2aadf3068 | 121 | //Check now Mode |
m5171135 | 4:39d2aadf3068 | 122 | if(agz.nowMode == MANUAL_MODE){ |
m5171135 | 6:21e2792e66c7 | 123 | agz.changeSpeed(buf); |
m5171135 | 0:7d55d6ace996 | 124 | } |
m5171135 | 0:7d55d6ace996 | 125 | break; |
m5171135 | 0:7d55d6ace996 | 126 | } |
m5171135 | 0:7d55d6ace996 | 127 | |
m5171135 | 4:39d2aadf3068 | 128 | //CommandType -> Send Status |
m5171135 | 3:1229ca3df855 | 129 | case STATUS_REQUEST:{ |
m5171135 | 4:39d2aadf3068 | 130 | //Create GPS Infomation Packet |
m5171135 | 5:940424ec98a8 | 131 | agz.createReceiveStatusCommand('B','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); |
m5171135 | 4:39d2aadf3068 | 132 | //Select Destination |
m5171135 | 5:940424ec98a8 | 133 | ZBTxRequest tx64request(remoteAddress,agz.packetData,agz.getPacketLength()); |
m5171135 | 4:39d2aadf3068 | 134 | //Send -> Base |
m5171135 | 0:7d55d6ace996 | 135 | xbee.send(tx64request); |
m5171135 | 4:39d2aadf3068 | 136 | break; |
m5171135 | 4:39d2aadf3068 | 137 | |
m5171135 | 0:7d55d6ace996 | 138 | } |
m5171135 | 4:39d2aadf3068 | 139 | |
m5171135 | 0:7d55d6ace996 | 140 | default: |
m5171135 | 0:7d55d6ace996 | 141 | { |
m5171135 | 0:7d55d6ace996 | 142 | break; |
m5171135 | 0:7d55d6ace996 | 143 | } |
m5171135 | 0:7d55d6ace996 | 144 | } |
m5171135 | 0:7d55d6ace996 | 145 | } |
m5171135 | 0:7d55d6ace996 | 146 | } |
m5171135 | 0:7d55d6ace996 | 147 | |
m5171135 | 0:7d55d6ace996 | 148 | myGPS.read(); |
m5171135 | 0:7d55d6ace996 | 149 | //recive gps module |
m5171135 | 0:7d55d6ace996 | 150 | //check if we recieved a new message from GPS, if so, attempt to parse it, |
m5171135 | 0:7d55d6ace996 | 151 | if ( myGPS.newNMEAreceived() ) { |
m5171135 | 0:7d55d6ace996 | 152 | if ( !myGPS.parse(myGPS.lastNMEA()) ) { |
m5171135 | 0:7d55d6ace996 | 153 | continue; |
m5171135 | 0:7d55d6ace996 | 154 | } |
m5171135 | 0:7d55d6ace996 | 155 | } |
m5171135 | 0:7d55d6ace996 | 156 | |
m5171135 | 0:7d55d6ace996 | 157 | if (refresh_Timer.read_ms() >= refresh_Time) { |
m5171135 | 0:7d55d6ace996 | 158 | refresh_Timer.reset(); |
m5171135 | 4:39d2aadf3068 | 159 | if (myGPS.fix) agz.nowStatus = GPS_AVAIL; |
m5171135 | 4:39d2aadf3068 | 160 | else agz.nowStatus = GPS_UNAVAIL; |
m5171135 | 0:7d55d6ace996 | 161 | } |
m5171135 | 0:7d55d6ace996 | 162 | } |
m5171135 | 0:7d55d6ace996 | 163 | } |