aigamozu / Mbed 2 deprecated aigamozu_program_ver2_yokokawa

yokokawa

Dependencies:   ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed

Fork of aigamozu_program_ver2 by aigamozu

main.cpp@7:bfc65eac624e, 2014-06-09 (annotated)

Committer:
m5171135
Date:
Mon Jun 09 00:16:45 2014 +0000
Revision:
7:bfc65eac624e
Parent:
6:21e2792e66c7
Child:
8:760cb743e4fc
ver.13

Who changed what in which revision?

UserRevisionLine numberNew 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 7:bfc65eac624e 7 // ver -> 1.3
m5171135 7:bfc65eac624e 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 5:940424ec98a8 107 agz.createReceiveStatusCommand('B','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 4:39d2aadf3068 135 if (myGPS.fix) agz.nowStatus = GPS_AVAIL;
m5171135 4:39d2aadf3068 136 else agz.nowStatus = GPS_UNAVAIL;
m5171135 0:7d55d6ace996 137 }
m5171135 0:7d55d6ace996 138 }
m5171135 0:7d55d6ace996 139 }