2015/05/12
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of Aigamozu_Robot_ver1_1 by
main.cpp
00001 /**********************************************/ 00002 // 00003 // 00004 // 00005 // Program name: Aigamozu BASE 00006 // Author: Mineta Kizuku 00007 // 00008 // 00009 /**********************************************/ 00010 00011 /**********************************************/ 00012 //更新情報 00013 //2015/05/11 00014 //ベースプログラムの作成 00015 // 00016 //void Get_GPS()内でLongitudeL, LatitudeL それぞれに+10する処理。 00017 //main()内でAutoModeのときでかつ2000ms立った時にGPSを取得する。 00018 // 00019 // 00020 /**********************************************/ 00021 00022 #include "mbed.h" 00023 #include "XBee.h" 00024 #include "MBed_Adafruit_GPS.h" 00025 #include "AigamozuControlPackets.h" 00026 #include "agzIDLIST.h" 00027 #include "aigamozuSetting.h" 00028 #include "agz_common.h" 00029 #include "Kalman.h" 00030 00031 //************ID Number***************** 00032 const char MyID = 'D'; 00033 //************ID Number***************** 00034 00035 ///////////////////////////////////////// 00036 // 00037 //Pin Setting 00038 // 00039 ///////////////////////////////////////// 00040 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26); 00041 00042 00043 ///////////////////////////////////////// 00044 // 00045 //Connection Setting 00046 // 00047 ///////////////////////////////////////// 00048 00049 //Serial Connect Setting: PC <--> mbed 00050 Serial pc(USBTX, USBRX); 00051 00052 //Serial Connect Setting: GPS <--> mbed 00053 Serial * gps_Serial; 00054 00055 //Serial Connect Setting: XBEE <--> mbed 00056 XBee xbee(p13,p14); 00057 ZBRxResponse zbRx = ZBRxResponse(); 00058 00059 //set up GPS module 00060 00061 //set up AigamozuControlPackets library 00062 AigamozuControlPackets agz(agz_motorShield); 00063 00064 00065 ///////////////////////////////////////// 00066 // 00067 //For Kalman data 00068 // 00069 ///////////////////////////////////////// 00070 double sigmaGPS[2][2] = {{250,0},{0,250}}; 00071 double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}}; 00072 double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}}; 00073 double y[2],x[2][2]={0}; 00074 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS); 00075 00076 ///////////////////////////////////////// 00077 // 00078 //Address List 00079 // 00080 ///////////////////////////////////////// 00081 XBeeAddress64 base_Address[BaseNumber] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 00082 XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L), 00083 XBeeAddress64(BASE5_32H,BASE5_32L)}; 00084 XBeeAddress64 robot_Address[RobotNumber] = {XBeeAddress64(ROBOT1_32H,ROBOT1_32L), XBeeAddress64(ROBOT1_32H,ROBOT1_32L)}; 00085 XBeeAddress64 manager_Address = XBeeAddress64(BASE1_32H,BASE1_32L); 00086 00087 00088 00089 ///////////////////////////////////////// 00090 // 00091 //Send_Status 00092 // 00093 //リクエストがきたとき、自分の位置情報などを返信する 00094 ///////////////////////////////////////// 00095 void Send_Status(char SenderIDc){ 00096 XBeeAddress64 send_Address; 00097 if(SenderIDc == '0'){ 00098 send_Address = manager_Address; 00099 } 00100 if(SenderIDc >= 'A' && SenderIDc <= 'Z'){ 00101 send_Address = robot_Address[SenderIDc - 'A']; 00102 } 00103 if(SenderIDc >= 'a' && SenderIDc <= 'z'){ 00104 send_Address = base_Address[SenderIDc - 'a']; 00105 } 00106 //send normal data 00107 //Create GPS Infomation Packet 00108 // agz.createReceiveStatusCommand(MyID,SenderIDc,agz.get_agzPoint_lati(),agz.get_agzPoint_longi(), 00109 // agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(), 00110 // agz.get_agzCov_lati(),agz.get_agzCov_longi()); 00111 //Select Destination 00112 // ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength()); 00113 //Send -> Base 00114 // xbee.send(tx64request); 00115 } 00116 00117 ///////////////////////////////////////// 00118 // 00119 //Get GPS function 00120 // 00121 ///////////////////////////////////////// 00122 00123 void Get_GPS(Adafruit_GPS *myGPS){ 00124 static bool flag = true; 00125 00126 if (myGPS->fix) { 00127 agz.nowStatus = GPS_AVAIL; 00128 00129 if(flag){//初期値設定 00130 if(myGPS->longitudeH>=138 && myGPS->longitudeH<=141 && myGPS->latitudeH>=36 && myGPS->latitudeH<=38){ 00131 flag = false; 00132 x[0][0]=(double)myGPS->latitudeL; 00133 x[0][1]=(double)myGPS->longitudeL; 00134 } 00135 } 00136 00137 if(myGPS->longitudeH<138 || myGPS->longitudeH>141 || myGPS->latitudeH<36 || myGPS->latitudeH>38){ 00138 return; 00139 } 00140 00141 //Kalman Filter 00142 Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS); 00143 00144 agz.reNewRobotPoint(myGPS->longitudeH,myGPS->longitudeL,myGPS->latitudeH,myGPS->latitudeL); 00145 agz.reNewRobotPointKalman(myGPS->longitudeKH,myGPS->longitudeKL,myGPS->latitudeKH,myGPS->latitudeKL); 00146 } 00147 else agz.nowStatus = GPS_UNAVAIL; 00148 00149 } 00150 00151 00152 ///////////////////////////////////////// 00153 // 00154 //Kalman Processing 00155 // 00156 ///////////////////////////////////////// 00157 00158 void get_K(){ 00159 double temp[2][2]={ 00160 {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]}, 00161 {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]} 00162 }; 00163 double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1]; 00164 K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]); 00165 K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]); 00166 } 00167 00168 00169 void get_x(){ 00170 x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]); 00171 x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]); 00172 } 00173 00174 00175 void get_sigma(){ 00176 double temp[2][2]; 00177 for(int i=0;i<2;i++) { 00178 for(int j=0;j<2;j++) { 00179 for(int k=0;k<2;k++) { 00180 temp[i][j]+=K[1][i][k]*sigma[0][k][j]; 00181 } 00182 } 00183 } 00184 for(int i = 0;i < 2;i++){ 00185 for(int j = 0;j < 2;j++){ 00186 sigma[1][i][j] = sigma[0][i][j]-temp[i][j]; 00187 } 00188 } 00189 } 00190 00191 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS){ 00192 y[0] = Latitude; 00193 y[1] = Longitude; 00194 //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS]; 00195 get_K(); 00196 //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]); 00197 get_x(); 00198 //sigma[t+1] = sigma[t]-K[t+1]*sigma[t]; 00199 get_sigma(); 00200 00201 00202 //kousinn 00203 for(int i = 0;i < 2;i++){ 00204 for(int j = 0;j < 2;j++){ 00205 K[0][i][j]=K[1][i][j]; 00206 x[0][i]=x[1][i]; 00207 sigma[0][i][j]=sigma[1][i][j]; 00208 } 00209 } 00210 00211 myGPS->latitudeKH=myGPS->latitudeH;//latitude after filtering 00212 myGPS->longitudeKH=myGPS->longitudeH;//longitude after filtering 00213 myGPS->latitudeKL=(long)x[1][0];//latitude after filtering 00214 myGPS->longitudeKL=(long)x[1][1];//longitude after filtering 00215 00216 agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]); 00217 } 00218 00219 void auto_Move(){ 00220 00221 bool result; 00222 int i; 00223 00224 result = agz.gpsAuto(); 00225 agz.set_agzAutoGPS(); 00226 agz.set_agzKalmanGPS(); 00227 00228 if(result == true){ 00229 printf("Out Area\n"); 00230 } 00231 else if(result == false){ 00232 printf("In Area\n"); 00233 } 00234 for(i = 0; i < 4; i++){ 00235 printf("%d: %f, %f\n", i, agz.get_basePointKalman_lati(i), agz.get_basePointKalman_longi(i)); 00236 } 00237 printf("robot: %f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi()); 00238 00239 } 00240 00241 ///////////////////////////////////////// 00242 // 00243 //Main Processing 00244 // 00245 ///////////////////////////////////////// 00246 int main() { 00247 //start up time 00248 wait(3); 00249 //set pc frequency to 57600bps 00250 pc.baud(PC_BAUD_RATE); 00251 //set xbee frequency to 57600bps 00252 xbee.begin(XBEE_BAUD_RATE); 00253 00254 00255 //GPS setting 00256 gps_Serial = new Serial(p28,p27); 00257 Adafruit_GPS myGPS(gps_Serial); 00258 Timer refresh_Timer; 00259 const int refresh_Time = 2000; //refresh time in ms 00260 myGPS.begin(GPS_BAUD_RATE); 00261 00262 char SenderIDc; 00263 00264 Timer auto_Timer; 00265 const int auto_Time = 2000; 00266 agz.nowMode = AUTO_MODE; 00267 00268 //GPS Send Command 00269 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 00270 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); 00271 myGPS.sendCommand(PGCMD_ANTENNA); 00272 00273 wait(2); 00274 00275 //interrupt start 00276 refresh_Timer.start(); 00277 auto_Timer.start(); 00278 00279 while (true) { 00280 00281 //Check Xbee Buffer Available 00282 xbee.readPacket(); 00283 00284 if (xbee.getResponse().isAvailable()) { 00285 xbee.getResponse().getZBRxResponse(zbRx); 00286 uint8_t *buf = zbRx.getFrameData(); 00287 00288 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) { 00289 xbee.getResponse().getZBRxResponse(zbRx); 00290 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する 00291 uint8_t *buf1 = &buf[11];//データの部分のみを格納する 00292 SenderIDc = buf1[5];//送信元のIDを取得する 00293 char Command_type =agz.checkCommnadType(buf);//コマンドタイプを取得する 00294 00295 //Check Command Type 00296 switch(Command_type){ 00297 //Get Request command 00298 case STATUS_REQUEST:{ 00299 Send_Status(SenderIDc); 00300 break; 00301 } 00302 default:{ 00303 break; 00304 } 00305 }//endswitch 00306 }//endifZB_RX_RESPONSE 00307 }//endifisAvailable 00308 00309 00310 00311 myGPS.read(); 00312 //recive gps module 00313 //check if we recieved a new message from GPS, if so, attempt to parse it, 00314 if ( myGPS.newNMEAreceived() ) { 00315 if ( !myGPS.parse(myGPS.lastNMEA()) ) { 00316 continue; 00317 } 00318 } 00319 00320 00321 if (refresh_Timer.read_ms() >= refresh_Time) { 00322 refresh_Timer.reset(); 00323 Get_GPS(&myGPS); 00324 00325 } 00326 00327 if(agz.nowMode == AUTO_MODE && auto_Timer.read_ms() >= auto_Time){ 00328 auto_Timer.reset(); 00329 auto_Move(); 00330 } 00331 00332 } 00333 }
Generated on Mon Jul 18 2022 02:21:21 by 1.7.2