2015/05/15
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
main.cpp
00001 /**********************************************/ 00002 // 00003 // 00004 // 00005 // Program name: Aigamozu_collect_data 00006 // Author: Mineta Kizuku 00007 // 00008 // Yokokawa Mami 00009 // 00010 // 00011 /**********************************************/ 00012 00013 /**********************************************/ 00014 //更新情報 00015 // 00016 //2015/05/13 00017 //カルマンフィルタの共分散の値を0.0001以下にならないようにした 00018 //共分散の値を10進数に変換するようにした 00019 // 00020 //2015/05/15 00021 //Robot側からのinかoutかの判定を受け取り、それと緯度と経度をprintするようにしました。 00022 //Robot側のプログラムcreateReceiveStatusCommand()にてstate関連をいじったので必要に応じて直してください。 00023 // 00024 // 00025 /**********************************************/ 00026 00027 #include "mbed.h" 00028 #include "XBee.h" 00029 #include "MBed_Adafruit_GPS.h" 00030 #include "AigamozuControlPackets.h" 00031 #include "agzIDLIST.h" 00032 #include "aigamozuSetting.h" 00033 #include "agz_common.h" 00034 #include "Kalman.h" 00035 00036 00037 #define SIGMA_MIN 0.0001 00038 00039 //************ID Number***************** 00040 const char MyID = 'h'; 00041 //************ID Number***************** 00042 00043 ///////////////////////////////////////// 00044 // 00045 //Pin Setting 00046 // 00047 ///////////////////////////////////////// 00048 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26); 00049 00050 00051 ///////////////////////////////////////// 00052 // 00053 //Connection Setting 00054 // 00055 ///////////////////////////////////////// 00056 00057 //Serial Connect Setting: PC <--> mbed 00058 Serial pc(USBTX, USBRX); 00059 00060 //Serial Connect Setting: GPS <--> mbed 00061 Serial * gps_Serial; 00062 00063 //Serial Connect Setting: XBEE <--> mbed 00064 XBee xbee(p13,p14); 00065 ZBRxResponse zbRx = ZBRxResponse(); 00066 00067 //set up GPS module 00068 00069 //set up AigamozuControlPackets library 00070 AigamozuControlPackets agz(agz_motorShield); 00071 00072 00073 ///////////////////////////////////////// 00074 // 00075 //For Kalman data 00076 // 00077 ///////////////////////////////////////// 00078 double sigmaGPS[2][2] = {{250,0},{0,250}}; 00079 double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}}; 00080 double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}}; 00081 double y[2],x[2][2]={0}; 00082 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS); 00083 00084 00085 ///////////////////////////////////////// 00086 // 00087 //Send_Status 00088 // 00089 //リクエストがきたとき、自分の位置情報などを返信する 00090 ///////////////////////////////////////// 00091 void Send_Status(char SenderIDc){ 00092 XBeeAddress64 send_Address; 00093 if(SenderIDc == '0'){ 00094 send_Address = manager_Address; 00095 } 00096 if(SenderIDc >= 'A' && SenderIDc <= 'Z'){ 00097 send_Address = robot_Address[SenderIDc - 'A']; 00098 } 00099 if(SenderIDc >= 'a' && SenderIDc <= 'z'){ 00100 send_Address = base_Address[SenderIDc - 'a']; 00101 } 00102 //send normal data 00103 //Create GPS Infomation Packet 00104 agz.createReceiveStatusCommand(MyID,SenderIDc, 00105 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(), 00106 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(), 00107 agz.get_agzCov_lati(),agz.get_agzCov_longi()); 00108 00109 //debug*************************************************** 00110 printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n", 00111 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(), 00112 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(), 00113 agz.get_agzCov_lati(),agz.get_agzCov_longi() 00114 ); 00115 for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]); 00116 printf("\n"); 00117 //debug end*************************************************** 00118 00119 //Select Destination 00120 ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength()); 00121 //Send -> Base 00122 xbee.send(tx64request); 00123 } 00124 00125 ///////////////////////////////////////// 00126 // 00127 //Get GPS function 00128 // 00129 ///////////////////////////////////////// 00130 00131 void Get_GPS(Adafruit_GPS *myGPS){ 00132 static bool flag = true; 00133 00134 if (myGPS->fix) { 00135 agz.nowStatus = GPS_AVAIL; 00136 00137 if(flag){//初期値設定 00138 if(myGPS->latitudeH>=36 && myGPS->latitudeH<=38 && myGPS->longitudeH>=138 && myGPS->longitudeH<=141){ 00139 flag = false; 00140 x[0][0]=(double)myGPS->latitudeL; 00141 x[0][1]=(double)myGPS->longitudeL; 00142 } 00143 } 00144 00145 if(myGPS->longitudeH<138 || myGPS->longitudeH>141 || myGPS->latitudeH<36 || myGPS->latitudeH>38){ 00146 return; 00147 } 00148 //Kalman Filter 00149 Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS); 00150 00151 agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL); 00152 agz.reNewRobotPointKalman(myGPS->latitudeKH,myGPS->latitudeKL,myGPS->longitudeKH,myGPS->longitudeKL); 00153 } 00154 else agz.nowStatus = GPS_UNAVAIL; 00155 00156 } 00157 00158 ///////////////////////////////////////// 00159 // 00160 //Send_Request_to_robot 00161 // 00162 ///////////////////////////////////////// 00163 void Send_Request_Robot(int robotnumber){ 00164 printf("send\n"); 00165 agz.createRequestCommand(MyID, robotnumber); 00166 //Select Destination 00167 ZBTxRequest tx64request(robot_Address[robotnumber],agz.packetData,agz.getPacketLength()); 00168 //Send -> Base 00169 xbee.send(tx64request); 00170 } 00171 00172 00173 ///////////////////////////////////////// 00174 // 00175 //Get Status 00176 // 00177 ///////////////////////////////////////// 00178 void Get_Status(char SenderIDc,uint8_t *packetdata){ 00179 00180 //マネージャからデータが来たとき 00181 if(SenderIDc == '0'){ 00182 printf("get manager Status\n"); 00183 } 00184 //他のロボットからデータが来たとき 00185 if(SenderIDc >= 'A' && SenderIDc <= 'Z'){ 00186 printf("Get Robot data\n"); 00187 int id = SenderIDc - 'A'; 00188 agz.set_base_status(id, &packetdata[9]); 00189 agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]); 00190 agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]); 00191 00192 //debug 00193 for(int i = 0; i < 1; i++){ 00194 printf("ROBOT:%d\n",i); 00195 printf("status:%d, latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\n", 00196 agz.get_base_status(i), agz.get_basePoint_lati(i),agz.get_basePoint_longi(i), 00197 agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i) 00198 ); 00199 } 00200 } 00201 //基地局からデータが来たとき 00202 if(SenderIDc >= 'a' && SenderIDc <= 'z'){ 00203 } 00204 } 00205 00206 ///////////////////////////////////////// 00207 // 00208 //Kalman Processing 00209 // 00210 ///////////////////////////////////////// 00211 00212 void get_K(){ 00213 double temp[2][2]={ 00214 {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]}, 00215 {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]} 00216 }; 00217 double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1]; 00218 K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]); 00219 K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]); 00220 } 00221 00222 00223 void get_x(){ 00224 x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]); 00225 x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]); 00226 } 00227 00228 00229 void get_sigma(){ 00230 double temp[2][2]; 00231 for(int i=0;i<2;i++) { 00232 for(int j=0;j<2;j++) { 00233 for(int k=0;k<2;k++) { 00234 temp[i][j]+=K[1][i][k]*sigma[0][k][j]; 00235 } 00236 } 00237 } 00238 for(int i = 0;i < 2;i++){ 00239 for(int j = 0;j < 2;j++){ 00240 sigma[1][i][j] = sigma[0][i][j]-temp[i][j]; 00241 } 00242 } 00243 } 00244 00245 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS){ 00246 y[0] = Latitude; 00247 y[1] = Longitude; 00248 //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS]; 00249 get_K(); 00250 //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]); 00251 get_x(); 00252 //sigma[t+1] = sigma[t]-K[t+1]*sigma[t]; 00253 get_sigma(); 00254 00255 00256 //kousinn 00257 for(int i = 0;i < 2;i++){ 00258 for(int j = 0;j < 2;j++){ 00259 K[0][i][j]=K[1][i][j]; 00260 x[0][i]=x[1][i]; 00261 sigma[0][i][j]=sigma[1][i][j]; 00262 } 00263 } 00264 00265 if(sigma[0][0][0] < SIGMA_MIN)sigma[0][0][0]=SIGMA_MIN; 00266 if(sigma[0][1][1] < SIGMA_MIN)sigma[0][1][1]=SIGMA_MIN; 00267 00268 myGPS->latitudeKH=myGPS->latitudeH;//latitude after filtering 00269 myGPS->longitudeKH=myGPS->longitudeH;//longitude after filtering 00270 myGPS->latitudeKL=(long)x[1][0];//latitude after filtering 00271 myGPS->longitudeKL=(long)x[1][1];//longitude after filtering 00272 00273 agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]); 00274 } 00275 00276 00277 ///////////////////////////////////////// 00278 // 00279 //Main Processing 00280 // 00281 ///////////////////////////////////////// 00282 int main() { 00283 //start up time 00284 wait(3); 00285 //set pc frequency to 57600bps 00286 pc.baud(PC_BAUD_RATE); 00287 //set xbee frequency to 57600bps 00288 xbee.begin(XBEE_BAUD_RATE); 00289 00290 00291 //GPS setting 00292 gps_Serial = new Serial(p28,p27); 00293 Adafruit_GPS myGPS(gps_Serial); 00294 Timer refresh_Timer; 00295 const int refresh_Time = 1000; //refresh time in ms 00296 myGPS.begin(GPS_BAUD_RATE); 00297 Timer collect_Timer; 00298 const int collect_Time = 2000; //when we collect 4 GPS point, we use 00299 int collect_flag = 0; 00300 char collect_state = 'a'; 00301 00302 char SenderIDc; 00303 //GPS Send Command 00304 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 00305 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); 00306 myGPS.sendCommand(PGCMD_ANTENNA); 00307 00308 wait(2); 00309 00310 //interrupt start 00311 refresh_Timer.start(); 00312 collect_Timer.start(); 00313 00314 printf("start\n"); 00315 00316 while (true) { 00317 00318 //Check Xbee Buffer Available 00319 xbee.readPacket(); 00320 00321 if (xbee.getResponse().isAvailable()) { 00322 xbee.getResponse().getZBRxResponse(zbRx); 00323 uint8_t *buf = zbRx.getFrameData(); 00324 00325 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) { 00326 xbee.getResponse().getZBRxResponse(zbRx); 00327 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する 00328 uint8_t *buf1 = &buf[11];//データの部分のみを格納する 00329 SenderIDc = buf1[5];//送信元のIDを取得する 00330 char Command_type =agz.checkCommnadType(buf);//コマンドタイプを取得する 00331 00332 //Check Command Type 00333 switch(Command_type){ 00334 //Get Request command 00335 case STATUS_REQUEST:{ 00336 Send_Status(SenderIDc); 00337 break; 00338 } 00339 case RECEIVE_STATUS:{ 00340 Get_Status(SenderIDc,buf1); 00341 break; 00342 } 00343 default:{ 00344 break; 00345 } 00346 }//endswitch 00347 }//endifZB_RX_RESPONSE 00348 }//endifisAvailable 00349 00350 myGPS.read(); 00351 //recive gps module 00352 //check if we recieved a new message from GPS, if so, attempt to parse it, 00353 if ( myGPS.newNMEAreceived() ) { 00354 if ( !myGPS.parse(myGPS.lastNMEA()) ) { 00355 continue; 00356 } 00357 } 00358 00359 if( collect_Timer.read_ms() >= collect_Time){ 00360 collect_Timer.reset(); 00361 00362 Send_Request_Robot(collect_flag); 00363 00364 collect_flag++; 00365 collect_state++; 00366 00367 if(collect_flag == 4){ 00368 collect_state = 'a'; 00369 collect_flag = 0; 00370 } 00371 } 00372 00373 00374 if (refresh_Timer.read_ms() >= refresh_Time) { 00375 refresh_Timer.reset(); 00376 Get_GPS(&myGPS); 00377 00378 } 00379 } 00380 }
Generated on Tue Jul 19 2022 00:50:48 by 1.7.2