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@11:187ff22b343f, 2015-04-10 (annotated)

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