robot
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 = 10000; //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 printf("%c\n", buf[14]); 00106 00107 //CommandType -> ChanegeMode 00108 case CHANGE_MODE :{ 00109 agz.changeMode(buf); 00110 break; 00111 } 00112 00113 //CommandType -> Manual 00114 case MANUAL:{ 00115 //Check now Mode 00116 if(agz.nowMode == MANUAL_MODE){ 00117 agz.changeSpeed(buf); 00118 } 00119 break; 00120 } 00121 00122 //CommandType -> Send Status 00123 case STATUS_REQUEST:{ 00124 //Create GPS Infomation Packet 00125 agz.createReceiveStatusCommand('B','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); 00126 //Select Destination 00127 ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength()); 00128 //Send -> Base 00129 xbee.send(tx64request); 00130 break; 00131 00132 } 00133 00134 case RECEIVE_STATUS:{ 00135 00136 printf("ok\n"); 00137 00138 id = buf1[6] - 'A'; 00139 robot[id].set_state(buf1[9]); 00140 robot[id].set_LatitudeH(&buf1[13]); 00141 robot[id].set_LatitudeL(&buf1[17]); 00142 robot[id].set_LongitudeH(&buf1[21]); 00143 robot[id].set_LongitudeL(&buf1[25]); 00144 00145 printf("%d\n", id); 00146 printf("%ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL()); 00147 break; 00148 } 00149 00150 default: 00151 { 00152 break; 00153 } 00154 } 00155 00156 00157 } 00158 } 00159 00160 myGPS.read(); 00161 //recive gps module 00162 //check if we recieved a new message from GPS, if so, attempt to parse it, 00163 if ( myGPS.newNMEAreceived() ) { 00164 if ( !myGPS.parse(myGPS.lastNMEA()) ) { 00165 continue; 00166 } 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.reNewPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); 00174 } 00175 else agz.nowStatus = GPS_UNAVAIL; 00176 } 00177 00178 00179 if( collect_Timer.read_ms() >= collect_Time){ 00180 collect_Timer.reset(); 00181 00182 printf("Send Request\n"); 00183 00184 agz.createRequestCommand('A', collect_state); 00185 //Select Destination 00186 ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength()); 00187 //Send -> Base 00188 xbee.send(tx64request); 00189 00190 collect_flag++; 00191 collect_state++; 00192 00193 if(collect_flag == 4){ 00194 printf("%d %c\n", collect_flag, collect_state); 00195 collect_state = 'a'; 00196 collect_flag = 0; 00197 } 00198 } 00199 } 00200 }
Generated on Sat Jul 23 2022 01:28:01 by 1.7.2