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@12:3e7ad5c07976, 2015-04-10 (annotated)

Committer:
s1200058
Date:
Fri Apr 10 13:19:19 2015 +0000
Revision:
12:3e7ad5c07976
Parent:
11:187ff22b343f 
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
m5171135 4:39d2aadf3068 105 //CommandType -> ChanegeMode
m5171135 4:39d2aadf3068 106 case CHANGE_MODE :{
m5171135 6:21e2792e66c7 107 agz.changeMode(buf);
m5171135 0:7d55d6ace996 108 break;
m5171135 0:7d55d6ace996 109 }
m5171135 4:39d2aadf3068 110
m5171135 4:39d2aadf3068 111 //CommandType -> Manual
m5171135 3:1229ca3df855 112 case MANUAL:{
m5171135 4:39d2aadf3068 113 //Check now Mode
m5171135 4:39d2aadf3068 114 if(agz.nowMode == MANUAL_MODE){
m5171135 6:21e2792e66c7 115 agz.changeSpeed(buf);
m5171135 0:7d55d6ace996 116 }
m5171135 0:7d55d6ace996 117 break;
m5171135 0:7d55d6ace996 118 }
m5171135 0:7d55d6ace996 119
m5171135 4:39d2aadf3068 120 //CommandType -> Send Status
m5171135 3:1229ca3df855 121 case STATUS_REQUEST:{
m5171135 4:39d2aadf3068 122 //Create GPS Infomation Packet
m5171135 9:357fa70be9c7 123 agz.createReceiveStatusCommand('B','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
m5171135 4:39d2aadf3068 124 //Select Destination
s1200058 12:3e7ad5c07976 125 ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength());
m5171135 4:39d2aadf3068 126 //Send -> Base
m5171135 0:7d55d6ace996 127 xbee.send(tx64request);
m5171135 4:39d2aadf3068 128 break;
m5171135 4:39d2aadf3068 129
m5171135 0:7d55d6ace996 130 }
s1200058 10:4a6059c5c84c 131
s1200058 12:3e7ad5c07976 132 case RECEIVE_STATUS:{
s1200058 10:4a6059c5c84c 133
s1200058 12:3e7ad5c07976 134 printf("Receive Status\n");
s1200058 12:3e7ad5c07976 135
s1200058 12:3e7ad5c07976 136 id = buf1[6] - 'A';
s1200058 12:3e7ad5c07976 137 robot[id].set_state(buf1[9]);
s1200058 12:3e7ad5c07976 138 robot[id].set_LatitudeH(&buf1[13]);
s1200058 12:3e7ad5c07976 139 robot[id].set_LatitudeL(&buf1[17]);
s1200058 12:3e7ad5c07976 140 robot[id].set_LongitudeH(&buf1[21]);
s1200058 12:3e7ad5c07976 141 robot[id].set_LongitudeL(&buf1[25]);
s1200058 12:3e7ad5c07976 142
s1200058 12:3e7ad5c07976 143 printf("%d\n", id);
s1200058 12:3e7ad5c07976 144 printf("%ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL());
s1200058 12:3e7ad5c07976 145 break;
s1200058 12:3e7ad5c07976 146 }
m5171135 4:39d2aadf3068 147
s1200058 12:3e7ad5c07976 148 default:{
m5171135 0:7d55d6ace996 149 break;
m5171135 0:7d55d6ace996 150 }
s1200058 12:3e7ad5c07976 151 }
s1200058 10:4a6059c5c84c 152
s1200058 10:4a6059c5c84c 153
m5171135 0:7d55d6ace996 154 }
m5171135 0:7d55d6ace996 155 }
m5171135 0:7d55d6ace996 156
m5171135 0:7d55d6ace996 157 myGPS.read();
m5171135 0:7d55d6ace996 158 //recive gps module
m5171135 0:7d55d6ace996 159 //check if we recieved a new message from GPS, if so, attempt to parse it,
m5171135 0:7d55d6ace996 160 if ( myGPS.newNMEAreceived() ) {
m5171135 0:7d55d6ace996 161 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
m5171135 0:7d55d6ace996 162 continue;
m5171135 0:7d55d6ace996 163 }
m5171135 0:7d55d6ace996 164 }
m5171135 0:7d55d6ace996 165
m5171135 0:7d55d6ace996 166 if (refresh_Timer.read_ms() >= refresh_Time) {
m5171135 0:7d55d6ace996 167 refresh_Timer.reset();
m5171135 8:760cb743e4fc 168 if (myGPS.fix) {
m5171135 8:760cb743e4fc 169 agz.nowStatus = GPS_AVAIL;
m5171135 8:760cb743e4fc 170 agz.reNewPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
m5171135 8:760cb743e4fc 171 }
m5171135 4:39d2aadf3068 172 else agz.nowStatus = GPS_UNAVAIL;
m5171135 9:357fa70be9c7 173 }
m5171135 9:357fa70be9c7 174
m5171135 9:357fa70be9c7 175
s1200058 10:4a6059c5c84c 176 if( collect_Timer.read_ms() >= collect_Time){
s1200058 10:4a6059c5c84c 177 collect_Timer.reset();
s1200058 10:4a6059c5c84c 178
s1200058 10:4a6059c5c84c 179 printf("Send Request\n");
s1200058 11:187ff22b343f 180
s1200058 11:187ff22b343f 181 agz.createRequestCommand('A', collect_state);
s1200058 10:4a6059c5c84c 182 //Select Destination
s1200058 10:4a6059c5c84c 183 ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength());
s1200058 10:4a6059c5c84c 184 //Send -> Base
s1200058 10:4a6059c5c84c 185 xbee.send(tx64request);
s1200058 10:4a6059c5c84c 186
s1200058 10:4a6059c5c84c 187 collect_flag++;
s1200058 10:4a6059c5c84c 188 collect_state++;
s1200058 10:4a6059c5c84c 189
s1200058 10:4a6059c5c84c 190 if(collect_flag == 4){
s1200058 10:4a6059c5c84c 191 collect_state = 'a';
s1200058 10:4a6059c5c84c 192 collect_flag = 0;
s1200058 10:4a6059c5c84c 193 }
s1200058 10:4a6059c5c84c 194 }
s1200058 10:4a6059c5c84c 195 }
m5171135 0:7d55d6ace996 196 }