auto
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed agz_common
main.cpp
00001 /**********************************************/ 00002 // 00003 // 00004 // 00005 // Program name: Aigamozu Robot Control 00006 // Author: Atsunori Maruyama 00007 // Ver -> 1.3 00008 // Day -> 2014/06/09 00009 // 00010 // 00011 /**********************************************/ 00012 00013 #include "mbed.h" 00014 #include "XBee.h" 00015 #include "MBed_Adafruit_GPS.h" 00016 #include "AigamozuControlPackets.h" 00017 #include "agzIDLIST.h" 00018 #include "aigamozuSetting.h" 00019 #include "agz_common.h" 00020 00021 ///////////////////////////////////////// 00022 // 00023 //Connection Setting 00024 // 00025 ///////////////////////////////////////// 00026 00027 //Serial Connect Setting: PC <--> mbed 00028 Serial pc(USBTX, USBRX); 00029 00030 //Serial Connect Setting: GPS <--> mbed 00031 Serial * gps_Serial; 00032 00033 //Serial Connect Setting: XBEE <--> mbed 00034 XBee xbee(p13,p14); 00035 ZBRxResponse zbRx = ZBRxResponse(); 00036 XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L); 00037 00038 AGZ_ROBOT robot[4]; 00039 00040 ///////////////////////////////////////// 00041 // 00042 //Pin Setting 00043 // 00044 ///////////////////////////////////////// 00045 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26); 00046 00047 ///////////////////////////////////////// 00048 // 00049 //Main Processing 00050 // 00051 ///////////////////////////////////////// 00052 int main() { 00053 //start up time 00054 wait(3); 00055 //set pc frequency to 57600bps 00056 pc.baud(PC_BAUD_RATE); 00057 //set xbee frequency to 57600bps 00058 xbee.begin(XBEE_BAUD_RATE); 00059 00060 //GPS setting 00061 gps_Serial = new Serial(p28,p27); 00062 Adafruit_GPS myGPS(gps_Serial); 00063 Timer refresh_Timer; 00064 const int refresh_Time = 2000; //refresh time in ms 00065 myGPS.begin(GPS_BAUD_RATE); 00066 00067 Timer collect_Timer; 00068 const int collect_Time = 200; //when we collect 4 GPS point, we use 00069 int collect_flag = 0; 00070 char collect_state = 'a'; 00071 XBeeAddress64 collect_Address[4] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 00072 XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L)}; 00073 XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L); 00074 int id; 00075 00076 00077 //GPS Send Command 00078 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 00079 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); 00080 myGPS.sendCommand(PGCMD_ANTENNA); 00081 00082 wait(2); 00083 00084 //interrupt start 00085 AigamozuControlPackets agz(agz_motorShield); 00086 refresh_Timer.start(); 00087 00088 printf("test\n"); 00089 00090 00091 while (true) { 00092 00093 //Check Xbee Buffer Available 00094 xbee.readPacket(); 00095 if (xbee.getResponse().isAvailable()) { 00096 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) { 00097 xbee.getResponse().getZBRxResponse(zbRx); 00098 uint8_t *buf = zbRx.getFrameData(); 00099 uint8_t *buf1 = &buf[11]; 00100 00101 00102 //Check Command Type 00103 switch(agz.checkCommnadType(buf)){ 00104 00105 //CommandType -> ChanegeMode 00106 case CHANGE_MODE :{ 00107 agz.changeMode(buf); 00108 break; 00109 } 00110 00111 //CommandType -> Manual 00112 case MANUAL:{ 00113 //Check now Mode 00114 if(agz.nowMode == MANUAL_MODE){ 00115 agz.changeSpeed(buf); 00116 } 00117 break; 00118 } 00119 00120 //CommandType -> Send Status 00121 case STATUS_REQUEST:{ 00122 //Create GPS Infomation Packet 00123 agz.createReceiveStatusCommand('D','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); 00124 //Select Destination 00125 ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength()); 00126 //Send -> Base 00127 xbee.send(tx64request); 00128 break; 00129 00130 } 00131 00132 case RECEIVE_STATUS:{ 00133 00134 //printf("Receive Status\n"); 00135 00136 id = buf1[5] - 'A'; 00137 robot[id].set_state(buf1[9]); 00138 robot[id].set_LatitudeH(&buf1[13]); 00139 robot[id].set_LatitudeL(&buf1[17]); 00140 robot[id].set_LongitudeH(&buf1[21]); 00141 robot[id].set_LongitudeL(&buf1[25]); 00142 00143 agz.reNewBasePoint(id,robot[id].get_LatitudeH(),robot[id].get_LatitudeL(),robot[id].get_LongitudeH(),robot[id].get_LongitudeL()); 00144 /* 00145 printf("%d,", buf1[5]); 00146 printf(" %ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL()); 00147 */ 00148 break; 00149 } 00150 00151 default:{ 00152 break; 00153 } 00154 } 00155 00156 00157 } 00158 } 00159 00160 00161 myGPS.read(); 00162 //recive gps module 00163 //check if we recieved a new message from GPS, if so, attempt to parse it, 00164 if ( myGPS.newNMEAreceived() ) { 00165 if ( !myGPS.parse(myGPS.lastNMEA()) ) { 00166 continue; 00167 } 00168 } 00169 if (refresh_Timer.read_ms() >= refresh_Time) { 00170 refresh_Timer.reset(); 00171 if (myGPS.fix) { 00172 agz.nowStatus = GPS_AVAIL; 00173 agz.reNewRobotPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); 00174 //printf("state = %d\n",agz.nowMode); 00175 //printf("my %ld, %ld, %ld, %ld\n", myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); 00176 } 00177 else agz.nowStatus = GPS_UNAVAIL; 00178 00179 } 00180 00181 //get base GPS 00182 if( collect_Timer.read_ms() >= collect_Time){ 00183 collect_Timer.reset(); 00184 00185 //printf("Send Request:%d,%d\n",collect_flag,collect_state); 00186 00187 agz.createRequestCommand('A', collect_state); 00188 //Select Destination 00189 ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength()); 00190 //Send -> Base 00191 xbee.send(tx64request); 00192 00193 collect_flag++; 00194 collect_state++; 00195 00196 if(collect_flag == 4){ 00197 collect_state = 'a'; 00198 collect_flag = 0; 00199 } 00200 } 00201 } 00202 }
Generated on Sat Jul 30 2022 15:40:41 by 1.7.2