aigamozu
2015/05/11
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of
aigamozu_auto_for_robot_ver1
by 
aigamozu
Embed:
(wiki syntax)
Show/hide line numbers
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 /*********************/ 00014 // 2015/05/11 yokokawa mami 00015 // AGZ_ROBOTでbase[5], robot[2], managerを宣言しました。 00016 // XbeeAddressでbase_Address, robot_Address, manager_Addressを宣言しました。 00017 // case STATUS_REQUEST:での条件分岐 00018 // send_AddressのところにそのときのAddressが入るようにしました。 00019 // case RECEIVE_STATUS:での条件分岐 00020 // base, robot, managerに対応するように変更しました。 00021 /*********************/ 00022 00023 #include "mbed.h" 00024 #include "XBee.h" 00025 #include "MBed_Adafruit_GPS.h" 00026 #include "AigamozuControlPackets.h" 00027 #include "agzIDLIST.h" 00028 #include "aigamozuSetting.h" 00029 #include "agz_common.h" 00030 #include "Kalman.h" 00031 00032 ///////////////////////////////////////// 00033 // 00034 //Connection Setting 00035 // 00036 ///////////////////////////////////////// 00037 00038 //Serial Connect Setting: PC <--> mbed 00039 Serial pc(USBTX, USBRX); 00040 00041 //Serial Connect Setting: GPS <--> mbed 00042 Serial * gps_Serial; 00043 00044 //Serial Connect Setting: XBEE <--> mbed 00045 XBee xbee(p13,p14); 00046 ZBRxResponse zbRx = ZBRxResponse(); 00047 XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L); 00048 00049 AGZ_ROBOT base[5], robot[2], manager; 00050 00051 char MyID = 'Z'; 00052 00053 ///////////////////////////////////////// 00054 // 00055 //Pin Setting 00056 // 00057 ///////////////////////////////////////// 00058 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26); 00059 00060 00061 00062 ///////////////////////////////////////// 00063 // 00064 //Kalman Processing 00065 // 00066 ///////////////////////////////////////// 00067 00068 00069 double sigmaGPS[2][2] = {{250,0},{0,250}}; 00070 double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}}; 00071 double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}}; 00072 double y[2],x[2][2]={0}; 00073 00074 00075 void get_K(){ 00076 double temp[2][2]={ 00077 {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]}, 00078 {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]} 00079 }; 00080 double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1]; 00081 K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]); 00082 K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]); 00083 } 00084 00085 00086 void get_x(){ 00087 x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]); 00088 x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]); 00089 } 00090 00091 00092 void get_sigma(){ 00093 double temp[2][2]; 00094 for(int i=0;i<2;i++) { 00095 for(int j=0;j<2;j++) { 00096 for(int k=0;k<2;k++) { 00097 temp[i][j]+=K[1][i][k]*sigma[0][k][j]; 00098 } 00099 } 00100 } 00101 for(int i = 0;i < 2;i++){ 00102 for(int j = 0;j < 2;j++){ 00103 sigma[1][i][j] = sigma[0][i][j]-temp[i][j]; 00104 } 00105 } 00106 } 00107 00108 void Kalman(double Latitude,double Longitude){ 00109 y[0] = Latitude; 00110 y[1] = Longitude; 00111 //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS]; 00112 get_K(); 00113 //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]); 00114 get_x(); 00115 //sigma[t+1] = sigma[t]-K[t+1]*sigma[t]; 00116 get_sigma(); 00117 } 00118 00119 00120 ///////////////////////////////////////// 00121 // 00122 //Main Processing 00123 // 00124 ///////////////////////////////////////// 00125 int main() { 00126 //start up time 00127 wait(3); 00128 //set pc frequency to 57600bps 00129 pc.baud(PC_BAUD_RATE); 00130 //set xbee frequency to 57600bps 00131 xbee.begin(XBEE_BAUD_RATE); 00132 00133 //GPS setting 00134 gps_Serial = new Serial(p28,p27); 00135 Adafruit_GPS myGPS(gps_Serial); 00136 Timer refresh_Timer; 00137 const int refresh_Time = 2000; //refresh time in ms 00138 myGPS.begin(GPS_BAUD_RATE); 00139 00140 Timer collect_Timer; 00141 const int collect_Time = 200; //when we collect 4 GPS point, we use 00142 int collect_flag = 0; 00143 char collect_state = 'a'; 00144 XBeeAddress64 collect_Address[5] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 00145 XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L), 00146 XBeeAddress64(BASE5_32H,BASE5_32L)}; 00147 //XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L); 00148 int id, SenderIDc; 00149 bool flag = true; 00150 00151 00152 XBeeAddress64 base_Address[5] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 00153 XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L), 00154 XBeeAddress64(BASE5_32H,BASE5_32L)}; 00155 XBeeAddress64 robot_Address[2] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L)}; 00156 XBeeAddress64 manager_Address = XBeeAddress64(BASE1_32H,BASE1_32L); 00157 00158 XBeeAddress64 send_Address; 00159 00160 //GPS Send Command 00161 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 00162 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); 00163 myGPS.sendCommand(PGCMD_ANTENNA); 00164 00165 wait(2); 00166 00167 //interrupt start 00168 AigamozuControlPackets agz(agz_motorShield); 00169 refresh_Timer.start(); 00170 00171 printf("start\n"); 00172 00173 00174 while (true) { 00175 00176 //Check Xbee Buffer Available 00177 xbee.readPacket(); 00178 if (xbee.getResponse().isAvailable()) { 00179 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) { 00180 xbee.getResponse().getZBRxResponse(zbRx); 00181 uint8_t *buf = zbRx.getFrameData(); 00182 uint8_t *buf1 = &buf[11]; 00183 00184 //id = buf1[5] - 'A'; 00185 SenderIDc = buf1[5]; 00186 00187 00188 //Check Command Type 00189 switch(agz.checkCommnadType(buf)){ 00190 00191 //CommandType -> ChanegeMode 00192 case CHANGE_MODE :{ 00193 agz.changeMode(buf); 00194 break; 00195 } 00196 00197 //CommandType -> Manual 00198 case MANUAL:{ 00199 //Check now Mode 00200 if(agz.nowMode == MANUAL_MODE){ 00201 agz.changeSpeed(buf); 00202 } 00203 break; 00204 } 00205 00206 //CommandType -> Send Status 00207 case STATUS_REQUEST:{ 00208 /* 00209 if(SenderIDc == 'Z'){ 00210 //send normal data 00211 //Create GPS Infomation Packet 00212 agz.createReceiveStatusCommand(MyID,SenderIDc,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); 00213 //Select Destination 00214 ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength()); 00215 //Send -> Base 00216 xbee.send(tx64request); 00217 00218 //send Kalman data 00219 agz.createReceiveStatusCommandwithKalman(MyID,SenderIDc,myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL); 00220 //Select Destination 00221 ZBTxRequest tx64request1(robot_Address,agz.packetData,agz.getPacketLength()); 00222 //Send -> Base 00223 xbee.send(tx64request1); 00224 00225 }else{*/ 00226 00227 printf("%d: catch request\n", id); 00228 00229 //send normal data 00230 //Create GPS Infomation Packet 00231 if(SenderIDc == '0'){ 00232 send_Address = manager_Address; 00233 } 00234 if(SenderIDc >= 'A' && SenderIDc <= 'Z'){ 00235 send_Address = robot_Address[SenderIDc - 'A']; 00236 } 00237 if(SenderIDc >= 'a' && SenderIDc <= 'z'){ 00238 send_Address = base_Address[SenderIDc - 'a']; 00239 } 00240 00241 agz.createReceiveStatusCommand(MyID,SenderIDc,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); 00242 //Select Destination 00243 ZBTxRequest tx64request2(send_Address,agz.packetData,agz.getPacketLength()); 00244 //Send -> Base 00245 xbee.send(tx64request2); 00246 00247 //send Kalman data 00248 agz.createReceiveStatusCommandwithKalman(MyID,SenderIDc,myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL); 00249 //Select Destination 00250 ZBTxRequest tx64request3(collect_Address[id],agz.packetData,agz.getPacketLength()); 00251 //Send -> Base 00252 xbee.send(tx64request3); 00253 // } 00254 break; 00255 00256 } 00257 00258 case RECEIVE_STATUS:{ 00259 00260 //printf("Receive Status\n"); 00261 00262 if(SenderIDc == '0'){ 00263 id = 0; 00264 manager.set_state(buf1[9]); 00265 manager.set_LatitudeH(&buf1[13]); 00266 manager.set_LatitudeL(&buf1[17]); 00267 manager.set_LongitudeH(&buf1[21]); 00268 manager.set_LongitudeL(&buf1[25]); 00269 agz.reNewBasePoint(id,manager.get_LatitudeH(),manager.get_LatitudeL(),manager.get_LongitudeH(),manager.get_LongitudeL()); 00270 } 00271 if(SenderIDc >= 'A' && SenderIDc <= 'Z'){ 00272 id = SenderIDc - 'A'; 00273 robot[id].set_state(buf1[9]); 00274 robot[id].set_LatitudeH(&buf1[13]); 00275 robot[id].set_LatitudeL(&buf1[17]); 00276 robot[id].set_LongitudeH(&buf1[21]); 00277 robot[id].set_LongitudeL(&buf1[25]); 00278 agz.reNewBasePoint(id,robot[id].get_LatitudeH(),robot[id].get_LatitudeL(),robot[id].get_LongitudeH(),robot[id].get_LongitudeL()); 00279 } 00280 if(SenderIDc >= 'a' && SenderIDc <= 'z'){ 00281 id = SenderIDc - 'a'; 00282 base[id].set_state(buf1[9]); 00283 base[id].set_LatitudeH(&buf1[13]); 00284 base[id].set_LatitudeL(&buf1[17]); 00285 base[id].set_LongitudeH(&buf1[21]); 00286 base[id].set_LongitudeL(&buf1[25]); 00287 agz.reNewBasePoint(id,base[id].get_LatitudeH(),base[id].get_LatitudeL(),base[id].get_LongitudeH(),base[id].get_LongitudeL()); 00288 } 00289 /* 00290 printf("%d,", buf1[5]); 00291 printf(" %ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL()); 00292 */ 00293 break; 00294 } 00295 case RECEIVE_KALMAN:{ 00296 id = buf1[5] - 'A'; 00297 robot[id].set_state(buf1[9]); 00298 robot[id].set_LatitudeKH(&buf1[13]); 00299 robot[id].set_LatitudeKL(&buf1[17]); 00300 robot[id].set_LongitudeKH(&buf1[21]); 00301 robot[id].set_LongitudeKL(&buf1[25]); 00302 00303 agz.reNewBasePointKalman(id,robot[id].get_LatitudeKH(),robot[id].get_LatitudeKL(),robot[id].get_LongitudeKH(),robot[id].get_LongitudeKL()); 00304 00305 break; 00306 } 00307 default:{ 00308 break; 00309 } 00310 } 00311 00312 00313 } 00314 } 00315 00316 00317 myGPS.read(); 00318 //recive gps module 00319 //check if we recieved a new message from GPS, if so, attempt to parse it, 00320 if ( myGPS.newNMEAreceived() ) { 00321 if ( !myGPS.parse(myGPS.lastNMEA()) ) { 00322 continue; 00323 } 00324 } 00325 if (refresh_Timer.read_ms() >= refresh_Time) { 00326 refresh_Timer.reset(); 00327 if (myGPS.fix) { 00328 agz.nowStatus = GPS_AVAIL; 00329 00330 if(flag){ 00331 if(myGPS.longitudeH==139 && myGPS.latitudeH==37){ 00332 flag = false; 00333 x[0][0]=(double)myGPS.longitudeL; 00334 x[0][1]=(double)myGPS.latitudeL; 00335 } 00336 } 00337 if(myGPS.longitudeH!=139 || myGPS.latitudeH!=37) continue; 00338 00339 //Kalman Filter 00340 Kalman(myGPS.longitudeL,myGPS.latitudeL); 00341 //kousinn 00342 for(int i = 0;i < 2;i++){ 00343 for(int j = 0;j < 2;j++){ 00344 K[0][i][j]=K[1][i][j]; 00345 x[0][i]=x[1][i]; 00346 sigma[0][i][j]=sigma[1][i][j]; 00347 } 00348 } 00349 00350 myGPS.longitudeKH=myGPS.longitudeH;//longitude after filtering 00351 myGPS.latitudeKH=myGPS.latitudeH;//latitude after filtering 00352 myGPS.longitudeKL=(long)x[1][0];//longitude after filtering 00353 myGPS.latitudeKL=(long)x[1][1];//latitude after filtering 00354 00355 agz.reNewRobotPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); 00356 agz.reNewRobotPointKalman(myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL); 00357 //printf("state = %d\n",agz.nowMode); 00358 //printf("my %ld, %ld, %ld, %ld\n", myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); 00359 } 00360 else agz.nowStatus = GPS_UNAVAIL; 00361 00362 } 00363 00364 00365 //get base GPS 00366 //if this program is for base,this program don't execute 00367 00368 if( collect_Timer.read_ms() >= collect_Time){ 00369 collect_Timer.reset(); 00370 00371 //printf("Send Request:%d,%d\n",collect_flag,collect_state); 00372 00373 agz.createRequestCommand(MyID, collect_state); 00374 //Select Destination 00375 ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength()); 00376 //Send -> Base 00377 xbee.send(tx64request); 00378 00379 collect_flag++; 00380 collect_state++; 00381 00382 if(collect_flag == 4){ 00383 collect_state = 'a'; 00384 collect_flag = 0; 00385 } 00386 } 00387 00388 } 00389 }
Generated on Wed Jul 13 2022 02:47:46 by
1.7.2