aigamozu / Mbed 2 deprecated aigamozu_auto_ver1

auto

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

main.cpp@2:e9a8664b52ff, 2015-04-13 (annotated)

Committer:
kityann
Date:
Mon Apr 13 10:09:42 2015 +0000
Revision:
2:e9a8664b52ff
Parent:
1:a5f98c7e1feb
Child:
3:7affc8af6db6
agz

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kityann 0:d8f3aa3b6876 1 /**********************************************/
kityann 0:d8f3aa3b6876 2 //
kityann 0:d8f3aa3b6876 3 //
kityann 0:d8f3aa3b6876 4 //
kityann 0:d8f3aa3b6876 5 // Program name: Aigamozu Robot Control
kityann 0:d8f3aa3b6876 6 // Author: Atsunori Maruyama
kityann 0:d8f3aa3b6876 7 // Ver -> 1.3
kityann 0:d8f3aa3b6876 8 // Day -> 2014/06/09
kityann 0:d8f3aa3b6876 9 //
kityann 0:d8f3aa3b6876 10 //
kityann 0:d8f3aa3b6876 11 /**********************************************/
kityann 0:d8f3aa3b6876 12
kityann 0:d8f3aa3b6876 13 #include "mbed.h"
kityann 0:d8f3aa3b6876 14 #include "XBee.h"
kityann 0:d8f3aa3b6876 15 #include "MBed_Adafruit_GPS.h"
kityann 0:d8f3aa3b6876 16 #include "AigamozuControlPackets.h"
kityann 0:d8f3aa3b6876 17 #include "agzIDLIST.h"
kityann 0:d8f3aa3b6876 18 #include "aigamozuSetting.h"
kityann 1:a5f98c7e1feb 19 #include "agz_common.h"
kityann 0:d8f3aa3b6876 20
kityann 0:d8f3aa3b6876 21 /////////////////////////////////////////
kityann 0:d8f3aa3b6876 22 //
kityann 0:d8f3aa3b6876 23 //Connection Setting
kityann 0:d8f3aa3b6876 24 //
kityann 0:d8f3aa3b6876 25 /////////////////////////////////////////
kityann 0:d8f3aa3b6876 26
kityann 0:d8f3aa3b6876 27 //Serial Connect Setting: PC <--> mbed
kityann 0:d8f3aa3b6876 28 Serial pc(USBTX, USBRX);
kityann 0:d8f3aa3b6876 29
kityann 0:d8f3aa3b6876 30 //Serial Connect Setting: GPS <--> mbed
kityann 0:d8f3aa3b6876 31 Serial * gps_Serial;
kityann 0:d8f3aa3b6876 32
kityann 0:d8f3aa3b6876 33 //Serial Connect Setting: XBEE <--> mbed
kityann 0:d8f3aa3b6876 34 XBee xbee(p13,p14);
kityann 0:d8f3aa3b6876 35 ZBRxResponse zbRx = ZBRxResponse();
kityann 1:a5f98c7e1feb 36 XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L);
kityann 0:d8f3aa3b6876 37
kityann 1:a5f98c7e1feb 38 AGZ_ROBOT robot[4];
kityann 0:d8f3aa3b6876 39
kityann 0:d8f3aa3b6876 40 /////////////////////////////////////////
kityann 0:d8f3aa3b6876 41 //
kityann 0:d8f3aa3b6876 42 //Pin Setting
kityann 0:d8f3aa3b6876 43 //
kityann 0:d8f3aa3b6876 44 /////////////////////////////////////////
kityann 0:d8f3aa3b6876 45 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:d8f3aa3b6876 46
kityann 0:d8f3aa3b6876 47 /////////////////////////////////////////
kityann 0:d8f3aa3b6876 48 //
kityann 0:d8f3aa3b6876 49 //Main Processing
kityann 0:d8f3aa3b6876 50 //
kityann 0:d8f3aa3b6876 51 /////////////////////////////////////////
kityann 0:d8f3aa3b6876 52 int main() {
kityann 0:d8f3aa3b6876 53 //start up time
kityann 0:d8f3aa3b6876 54 wait(3);
kityann 0:d8f3aa3b6876 55 //set pc frequency to 57600bps
kityann 0:d8f3aa3b6876 56 pc.baud(PC_BAUD_RATE);
kityann 0:d8f3aa3b6876 57 //set xbee frequency to 57600bps
kityann 0:d8f3aa3b6876 58 xbee.begin(XBEE_BAUD_RATE);
kityann 0:d8f3aa3b6876 59
kityann 0:d8f3aa3b6876 60 //GPS setting
kityann 0:d8f3aa3b6876 61 gps_Serial = new Serial(p28,p27);
kityann 0:d8f3aa3b6876 62 Adafruit_GPS myGPS(gps_Serial);
kityann 0:d8f3aa3b6876 63 Timer refresh_Timer;
kityann 0:d8f3aa3b6876 64 const int refresh_Time = 2000; //refresh time in ms
kityann 0:d8f3aa3b6876 65 myGPS.begin(GPS_BAUD_RATE);
kityann 0:d8f3aa3b6876 66
kityann 1:a5f98c7e1feb 67 Timer collect_Timer;
kityann 1:a5f98c7e1feb 68 const int collect_Time = 200; //when we collect 4 GPS point, we use
kityann 1:a5f98c7e1feb 69 int collect_flag = 0;
kityann 1:a5f98c7e1feb 70 char collect_state = 'a';
kityann 1:a5f98c7e1feb 71 XBeeAddress64 collect_Address[4] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L),
kityann 1:a5f98c7e1feb 72 XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L)};
kityann 1:a5f98c7e1feb 73 XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
kityann 1:a5f98c7e1feb 74 int id;
kityann 1:a5f98c7e1feb 75
kityann 1:a5f98c7e1feb 76
kityann 0:d8f3aa3b6876 77 //GPS Send Command
kityann 0:d8f3aa3b6876 78 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:d8f3aa3b6876 79 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:d8f3aa3b6876 80 myGPS.sendCommand(PGCMD_ANTENNA);
kityann 0:d8f3aa3b6876 81
kityann 0:d8f3aa3b6876 82 wait(2);
kityann 0:d8f3aa3b6876 83
kityann 0:d8f3aa3b6876 84 //interrupt start
kityann 0:d8f3aa3b6876 85 AigamozuControlPackets agz(agz_motorShield);
kityann 0:d8f3aa3b6876 86 refresh_Timer.start();
kityann 0:d8f3aa3b6876 87
kityann 1:a5f98c7e1feb 88 printf("test\n");
kityann 1:a5f98c7e1feb 89
kityann 0:d8f3aa3b6876 90
kityann 0:d8f3aa3b6876 91 while (true) {
kityann 1:a5f98c7e1feb 92
kityann 0:d8f3aa3b6876 93 //Check Xbee Buffer Available
kityann 0:d8f3aa3b6876 94 xbee.readPacket();
kityann 0:d8f3aa3b6876 95 if (xbee.getResponse().isAvailable()) {
kityann 0:d8f3aa3b6876 96 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
kityann 0:d8f3aa3b6876 97 xbee.getResponse().getZBRxResponse(zbRx);
kityann 1:a5f98c7e1feb 98 uint8_t *buf = zbRx.getFrameData();
kityann 1:a5f98c7e1feb 99 uint8_t *buf1 = &buf[11];
kityann 1:a5f98c7e1feb 100
kityann 0:d8f3aa3b6876 101
kityann 0:d8f3aa3b6876 102 //Check Command Type
kityann 0:d8f3aa3b6876 103 switch(agz.checkCommnadType(buf)){
kityann 0:d8f3aa3b6876 104
kityann 0:d8f3aa3b6876 105 //CommandType -> ChanegeMode
kityann 0:d8f3aa3b6876 106 case CHANGE_MODE :{
kityann 0:d8f3aa3b6876 107 agz.changeMode(buf);
kityann 0:d8f3aa3b6876 108 break;
kityann 0:d8f3aa3b6876 109 }
kityann 0:d8f3aa3b6876 110
kityann 0:d8f3aa3b6876 111 //CommandType -> Manual
kityann 0:d8f3aa3b6876 112 case MANUAL:{
kityann 0:d8f3aa3b6876 113 //Check now Mode
kityann 0:d8f3aa3b6876 114 if(agz.nowMode == MANUAL_MODE){
kityann 0:d8f3aa3b6876 115 agz.changeSpeed(buf);
kityann 0:d8f3aa3b6876 116 }
kityann 0:d8f3aa3b6876 117 break;
kityann 0:d8f3aa3b6876 118 }
kityann 0:d8f3aa3b6876 119
kityann 0:d8f3aa3b6876 120 //CommandType -> Send Status
kityann 0:d8f3aa3b6876 121 case STATUS_REQUEST:{
kityann 0:d8f3aa3b6876 122 //Create GPS Infomation Packet
kityann 2:e9a8664b52ff 123 agz.createReceiveStatusCommand('D','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
kityann 0:d8f3aa3b6876 124 //Select Destination
kityann 1:a5f98c7e1feb 125 ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength());
kityann 0:d8f3aa3b6876 126 //Send -> Base
kityann 0:d8f3aa3b6876 127 xbee.send(tx64request);
kityann 0:d8f3aa3b6876 128 break;
kityann 0:d8f3aa3b6876 129
kityann 0:d8f3aa3b6876 130 }
kityann 1:a5f98c7e1feb 131
kityann 1:a5f98c7e1feb 132 case RECEIVE_STATUS:{
kityann 1:a5f98c7e1feb 133
kityann 1:a5f98c7e1feb 134 printf("Receive Status\n");
kityann 1:a5f98c7e1feb 135
kityann 2:e9a8664b52ff 136 id = buf1[5] - 'A';
kityann 1:a5f98c7e1feb 137 robot[id].set_state(buf1[9]);
kityann 1:a5f98c7e1feb 138 robot[id].set_LatitudeH(&buf1[13]);
kityann 1:a5f98c7e1feb 139 robot[id].set_LatitudeL(&buf1[17]);
kityann 1:a5f98c7e1feb 140 robot[id].set_LongitudeH(&buf1[21]);
kityann 1:a5f98c7e1feb 141 robot[id].set_LongitudeL(&buf1[25]);
kityann 2:e9a8664b52ff 142
kityann 2:e9a8664b52ff 143 agz.reNewBasePoint(id,robot[id].get_LatitudeH(),robot[id].get_LatitudeL(),robot[id].get_LongitudeH(),robot[id].get_LongitudeL());
kityann 1:a5f98c7e1feb 144
kityann 1:a5f98c7e1feb 145 printf("%d\n", id);
kityann 1:a5f98c7e1feb 146 printf("%ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL());
kityann 1:a5f98c7e1feb 147 break;
kityann 1:a5f98c7e1feb 148 }
kityann 0:d8f3aa3b6876 149
kityann 1:a5f98c7e1feb 150 default:{
kityann 0:d8f3aa3b6876 151 break;
kityann 0:d8f3aa3b6876 152 }
kityann 0:d8f3aa3b6876 153 }
kityann 1:a5f98c7e1feb 154
kityann 1:a5f98c7e1feb 155
kityann 1:a5f98c7e1feb 156 }
kityann 0:d8f3aa3b6876 157 }
kityann 1:a5f98c7e1feb 158
kityann 0:d8f3aa3b6876 159
kityann 0:d8f3aa3b6876 160 myGPS.read();
kityann 0:d8f3aa3b6876 161 //recive gps module
kityann 0:d8f3aa3b6876 162 //check if we recieved a new message from GPS, if so, attempt to parse it,
kityann 0:d8f3aa3b6876 163 if ( myGPS.newNMEAreceived() ) {
kityann 0:d8f3aa3b6876 164 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
kityann 0:d8f3aa3b6876 165 continue;
kityann 0:d8f3aa3b6876 166 }
kityann 0:d8f3aa3b6876 167 }
kityann 0:d8f3aa3b6876 168 if (refresh_Timer.read_ms() >= refresh_Time) {
kityann 0:d8f3aa3b6876 169 refresh_Timer.reset();
kityann 0:d8f3aa3b6876 170 if (myGPS.fix) {
kityann 0:d8f3aa3b6876 171 agz.nowStatus = GPS_AVAIL;
kityann 2:e9a8664b52ff 172 agz.reNewRobotPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
kityann 2:e9a8664b52ff 173 printf("state = %d\n",agz.nowMode);
kityann 1:a5f98c7e1feb 174 printf("my %ld, %ld, %ld, %ld\n", myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
kityann 0:d8f3aa3b6876 175 }
kityann 0:d8f3aa3b6876 176 else agz.nowStatus = GPS_UNAVAIL;
kityann 1:a5f98c7e1feb 177
kityann 0:d8f3aa3b6876 178 }
kityann 0:d8f3aa3b6876 179
kityann 1:a5f98c7e1feb 180 //get base GPS
kityann 1:a5f98c7e1feb 181 if( collect_Timer.read_ms() >= collect_Time){
kityann 1:a5f98c7e1feb 182 collect_Timer.reset();
kityann 0:d8f3aa3b6876 183
kityann 2:e9a8664b52ff 184 printf("Send Request:%d,%d\n",collect_flag,collect_state);
kityann 1:a5f98c7e1feb 185
kityann 1:a5f98c7e1feb 186 agz.createRequestCommand('A', collect_state);
kityann 1:a5f98c7e1feb 187 //Select Destination
kityann 1:a5f98c7e1feb 188 ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength());
kityann 1:a5f98c7e1feb 189 //Send -> Base
kityann 1:a5f98c7e1feb 190 xbee.send(tx64request);
kityann 1:a5f98c7e1feb 191
kityann 1:a5f98c7e1feb 192 collect_flag++;
kityann 1:a5f98c7e1feb 193 collect_state++;
kityann 1:a5f98c7e1feb 194
kityann 1:a5f98c7e1feb 195 if(collect_flag == 4){
kityann 1:a5f98c7e1feb 196 collect_state = 'a';
kityann 1:a5f98c7e1feb 197 collect_flag = 0;
kityann 1:a5f98c7e1feb 198 }
kityann 1:a5f98c7e1feb 199 }
kityann 1:a5f98c7e1feb 200 }
kityann 0:d8f3aa3b6876 201 }