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@10:4a6059c5c84c, 2015-04-09 (annotated)

Committer:
s1200058
Date:
Thu Apr 09 11:18:07 2015 +0000
Revision:
10:4a6059c5c84c
Parent:
9:357fa70be9c7
Child:
11:187ff22b343f
from yokokawa;

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 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 10:4a6059c5c84c 73 int id;
s1200058 10:4a6059c5c84c 74
s1200058 10:4a6059c5c84c 75
m5171135 2:95955f38f47a 76 //GPS Send Command
m5171135 2:95955f38f47a 77 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
m5171135 0:7d55d6ace996 78 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
m5171135 0:7d55d6ace996 79 myGPS.sendCommand(PGCMD_ANTENNA);
m5171135 0:7d55d6ace996 80
m5171135 0:7d55d6ace996 81 wait(2);
m5171135 0:7d55d6ace996 82
m5171135 0:7d55d6ace996 83 //interrupt start
m5171135 6:21e2792e66c7 84 AigamozuControlPackets agz(agz_motorShield);
m5171135 0:7d55d6ace996 85 refresh_Timer.start();
m5171135 4:39d2aadf3068 86
s1200058 10:4a6059c5c84c 87 printf("test\n");
s1200058 10:4a6059c5c84c 88
m5171135 4:39d2aadf3068 89
m5171135 2:95955f38f47a 90 while (true) {
m5171135 0:7d55d6ace996 91
m5171135 4:39d2aadf3068 92 //Check Xbee Buffer Available
m5171135 0:7d55d6ace996 93 xbee.readPacket();
m5171135 0:7d55d6ace996 94 if (xbee.getResponse().isAvailable()) {
m5171135 0:7d55d6ace996 95 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
m5171135 0:7d55d6ace996 96 xbee.getResponse().getZBRxResponse(zbRx);
s1200058 10:4a6059c5c84c 97 uint8_t *buf = zbRx.getFrameData();
s1200058 10:4a6059c5c84c 98 uint8_t *buf1 = &buf[11];
m5171135 0:7d55d6ace996 99
m5171135 2:95955f38f47a 100
m5171135 4:39d2aadf3068 101 //Check Command Type
m5171135 3:1229ca3df855 102 switch(agz.checkCommnadType(buf)){
m5171135 7:bfc65eac624e 103
m5171135 4:39d2aadf3068 104 //CommandType -> ChanegeMode
m5171135 4:39d2aadf3068 105 case CHANGE_MODE :{
m5171135 6:21e2792e66c7 106 agz.changeMode(buf);
m5171135 0:7d55d6ace996 107 break;
m5171135 0:7d55d6ace996 108 }
m5171135 4:39d2aadf3068 109
m5171135 4:39d2aadf3068 110 //CommandType -> Manual
m5171135 3:1229ca3df855 111 case MANUAL:{
m5171135 4:39d2aadf3068 112 //Check now Mode
m5171135 4:39d2aadf3068 113 if(agz.nowMode == MANUAL_MODE){
m5171135 6:21e2792e66c7 114 agz.changeSpeed(buf);
m5171135 0:7d55d6ace996 115 }
m5171135 0:7d55d6ace996 116 break;
m5171135 0:7d55d6ace996 117 }
m5171135 0:7d55d6ace996 118
m5171135 4:39d2aadf3068 119 //CommandType -> Send Status
m5171135 3:1229ca3df855 120 case STATUS_REQUEST:{
m5171135 4:39d2aadf3068 121 //Create GPS Infomation Packet
m5171135 9:357fa70be9c7 122 agz.createReceiveStatusCommand('B','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
m5171135 4:39d2aadf3068 123 //Select Destination
m5171135 5:940424ec98a8 124 ZBTxRequest tx64request(remoteAddress,agz.packetData,agz.getPacketLength());
m5171135 4:39d2aadf3068 125 //Send -> Base
m5171135 0:7d55d6ace996 126 xbee.send(tx64request);
m5171135 4:39d2aadf3068 127 break;
m5171135 4:39d2aadf3068 128
m5171135 0:7d55d6ace996 129 }
s1200058 10:4a6059c5c84c 130
s1200058 10:4a6059c5c84c 131 case REQUEST_COMMAND:{
s1200058 10:4a6059c5c84c 132
s1200058 10:4a6059c5c84c 133 id = buf1[6] - 'A';
s1200058 10:4a6059c5c84c 134 robot[id].set_state(buf1[9]);
s1200058 10:4a6059c5c84c 135 robot[id].set_LatitudeH(&buf1[13]);
s1200058 10:4a6059c5c84c 136 robot[id].set_LatitudeL(&buf1[17]);
s1200058 10:4a6059c5c84c 137 robot[id].set_LongitudeH(&buf1[21]);
s1200058 10:4a6059c5c84c 138 robot[id].set_LongitudeL(&buf1[25]);
s1200058 10:4a6059c5c84c 139
s1200058 10:4a6059c5c84c 140 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 141 break;
s1200058 10:4a6059c5c84c 142 }
m5171135 4:39d2aadf3068 143
m5171135 0:7d55d6ace996 144 default:
m5171135 0:7d55d6ace996 145 {
m5171135 0:7d55d6ace996 146 break;
m5171135 0:7d55d6ace996 147 }
m5171135 0:7d55d6ace996 148 }
s1200058 10:4a6059c5c84c 149
s1200058 10:4a6059c5c84c 150
m5171135 0:7d55d6ace996 151 }
m5171135 0:7d55d6ace996 152 }
m5171135 0:7d55d6ace996 153
m5171135 0:7d55d6ace996 154 myGPS.read();
m5171135 0:7d55d6ace996 155 //recive gps module
m5171135 0:7d55d6ace996 156 //check if we recieved a new message from GPS, if so, attempt to parse it,
m5171135 0:7d55d6ace996 157 if ( myGPS.newNMEAreceived() ) {
m5171135 0:7d55d6ace996 158 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
m5171135 0:7d55d6ace996 159 continue;
m5171135 0:7d55d6ace996 160 }
m5171135 0:7d55d6ace996 161 }
m5171135 0:7d55d6ace996 162
m5171135 0:7d55d6ace996 163 if (refresh_Timer.read_ms() >= refresh_Time) {
m5171135 0:7d55d6ace996 164 refresh_Timer.reset();
m5171135 8:760cb743e4fc 165 if (myGPS.fix) {
m5171135 8:760cb743e4fc 166 agz.nowStatus = GPS_AVAIL;
m5171135 8:760cb743e4fc 167 agz.reNewPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
m5171135 8:760cb743e4fc 168 }
m5171135 4:39d2aadf3068 169 else agz.nowStatus = GPS_UNAVAIL;
m5171135 9:357fa70be9c7 170 }
m5171135 9:357fa70be9c7 171
m5171135 9:357fa70be9c7 172
s1200058 10:4a6059c5c84c 173 if( collect_Timer.read_ms() >= collect_Time){
s1200058 10:4a6059c5c84c 174 collect_Timer.reset();
s1200058 10:4a6059c5c84c 175
s1200058 10:4a6059c5c84c 176 printf("Send Request\n");
m5171135 8:760cb743e4fc 177
s1200058 10:4a6059c5c84c 178 //Create GPS Infomation Packet
s1200058 10:4a6059c5c84c 179 agz.createReceiveStatusCommand('B', collect_state,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
s1200058 10:4a6059c5c84c 180 //Select Destination
s1200058 10:4a6059c5c84c 181 ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength());
s1200058 10:4a6059c5c84c 182 //Send -> Base
s1200058 10:4a6059c5c84c 183 xbee.send(tx64request);
s1200058 10:4a6059c5c84c 184
s1200058 10:4a6059c5c84c 185 collect_flag++;
s1200058 10:4a6059c5c84c 186 collect_state++;
s1200058 10:4a6059c5c84c 187
s1200058 10:4a6059c5c84c 188 if(collect_flag == 4){
s1200058 10:4a6059c5c84c 189 collect_state = 'a';
s1200058 10:4a6059c5c84c 190 collect_flag = 0;
s1200058 10:4a6059c5c84c 191 }
s1200058 10:4a6059c5c84c 192 }
s1200058 10:4a6059c5c84c 193 }
m5171135 0:7d55d6ace996 194 }