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 //2015/05/13
00017 //カルマンフィルタの共分散の値を0.0001以下にならないようにした
00018 //共分散の値を10進数に変換するようにした
00019 /**********************************************/
00020 
00021 #include "mbed.h"
00022 #include "XBee.h"
00023 #include "MBed_Adafruit_GPS.h"
00024 #include "AigamozuControlPackets.h"
00025 #include "agzIDLIST.h"
00026 #include "aigamozuSetting.h"
00027 #include "agz_common.h"
00028 #include "Kalman.h"
00029 
00030 
00031 #define SIGMA_MIN 0.0001
00032 
00033 //************ID Number*****************
00034 const char MyID = 'd';
00035 //************ID Number*****************
00036 
00037 /////////////////////////////////////////
00038 //
00039 //Pin Setting
00040 //
00041 /////////////////////////////////////////
00042 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
00043 
00044 
00045 /////////////////////////////////////////
00046 //
00047 //Connection Setting
00048 //
00049 /////////////////////////////////////////
00050 
00051 //Serial Connect Setting: PC <--> mbed
00052 Serial pc(USBTX, USBRX);    
00053 
00054 //Serial Connect Setting: GPS <--> mbed
00055 Serial * gps_Serial;
00056 
00057 //Serial Connect Setting: XBEE <--> mbed
00058 XBee xbee(p13,p14);
00059 ZBRxResponse zbRx = ZBRxResponse();
00060 
00061 //set up GPS module
00062 
00063 //set up AigamozuControlPackets library
00064 AigamozuControlPackets agz(agz_motorShield);
00065 
00066 
00067 /////////////////////////////////////////
00068 //
00069 //For Kalman data
00070 //
00071 /////////////////////////////////////////
00072 double sigmaGPS[2][2] = {{250,0},{0,250}};
00073 double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
00074 double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
00075 double y[2],x[2][2]={0};
00076 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS);
00077 
00078 
00079 /////////////////////////////////////////
00080 //
00081 //Send_Status
00082 //
00083 //リクエストがきたとき、自分の位置情報などを返信する
00084 /////////////////////////////////////////
00085 void Send_Status(char SenderIDc){
00086     XBeeAddress64 send_Address;
00087     if(SenderIDc == '0'){
00088         send_Address = manager_Address;
00089     }
00090     if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
00091         send_Address = robot_Address[SenderIDc - 'A'];
00092     }
00093     if(SenderIDc >= 'a' && SenderIDc <= 'z'){
00094         send_Address = base_Address[SenderIDc - 'a'];
00095     }
00096     //send normal data
00097     //Create GPS Infomation Packet
00098     agz.createReceiveStatusCommand(MyID,SenderIDc,
00099                                     agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00100                                     agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00101                                     agz.get_agzCov_lati(),agz.get_agzCov_longi());
00102     
00103     //debug***************************************************
00104     printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
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     for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
00110         printf("\n");
00111     //debug end***************************************************
00112     
00113     //Select Destination
00114     ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
00115     //Send -> Base
00116     xbee.send(tx64request);
00117 }
00118 
00119 /////////////////////////////////////////
00120 //
00121 //Get GPS function
00122 //
00123 /////////////////////////////////////////
00124 
00125 void Get_GPS(Adafruit_GPS *myGPS){
00126     static bool flag = true;
00127     
00128     if (myGPS->fix) {
00129         agz.nowStatus = GPS_AVAIL;
00130         
00131         if(flag){//初期値設定
00132             if(myGPS->latitudeH>=36 && myGPS->latitudeH<=38 && myGPS->longitudeH>=138 && myGPS->longitudeH<=141){
00133                 flag = false;
00134                 x[0][0]=(double)myGPS->latitudeL;                        
00135                 x[0][1]=(double)myGPS->longitudeL;   
00136             } 
00137         }
00138         
00139         if(myGPS->longitudeH<138 || myGPS->longitudeH>141 || myGPS->latitudeH<36 || myGPS->latitudeH>38){
00140             return;
00141         }
00142         //Kalman Filter
00143         Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS);
00144                 
00145         agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
00146         agz.reNewRobotPointKalman(myGPS->latitudeKH,myGPS->latitudeKL,myGPS->longitudeKH,myGPS->longitudeKL);
00147     }
00148     else agz.nowStatus = GPS_UNAVAIL;
00149     
00150 } 
00151 
00152 
00153 /////////////////////////////////////////
00154 //
00155 //Kalman Processing
00156 //
00157 /////////////////////////////////////////
00158     
00159 void get_K(){
00160   double temp[2][2]={
00161     {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
00162     {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
00163   };
00164   double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
00165   K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
00166   K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
00167 }
00168 
00169 
00170 void get_x(){
00171   x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
00172   x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
00173 }
00174 
00175 
00176 void get_sigma(){
00177     double temp[2][2];
00178     for(int i=0;i<2;i++) {
00179         for(int j=0;j<2;j++) {
00180             for(int k=0;k<2;k++) {
00181                 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
00182             }
00183         }
00184    }
00185     for(int i = 0;i < 2;i++){
00186         for(int j = 0;j < 2;j++){
00187             sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
00188         }
00189     }   
00190 }
00191 
00192 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS){
00193     y[0] = Latitude;
00194     y[1] = Longitude;
00195     //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
00196     get_K();
00197     //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
00198     get_x();
00199     //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
00200     get_sigma();
00201     
00202     
00203     //kousinn
00204     for(int i = 0;i < 2;i++){
00205         for(int j = 0;j < 2;j++){
00206             K[0][i][j]=K[1][i][j];
00207             x[0][i]=x[1][i];
00208             sigma[0][i][j]=sigma[1][i][j];
00209         }
00210     }
00211     
00212     if(sigma[0][0][0] < SIGMA_MIN)sigma[0][0][0]=SIGMA_MIN;
00213     if(sigma[0][1][1] < SIGMA_MIN)sigma[0][1][1]=SIGMA_MIN;
00214             
00215     myGPS->latitudeKH=myGPS->latitudeH;//latitude after filtering
00216     myGPS->longitudeKH=myGPS->longitudeH;//longitude after filtering
00217     myGPS->latitudeKL=(long)x[1][0];//latitude after filtering
00218     myGPS->longitudeKL=(long)x[1][1];//longitude after filtering
00219     
00220     agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]);
00221 }
00222 
00223 
00224 /////////////////////////////////////////
00225 //
00226 //Main Processing
00227 //
00228 /////////////////////////////////////////
00229 int main() {
00230     //start up time
00231     wait(3);
00232     //set pc frequency to 57600bps 
00233     pc.baud(PC_BAUD_RATE); 
00234     //set xbee frequency to 57600bps
00235     xbee.begin(XBEE_BAUD_RATE);    
00236         
00237 
00238     //GPS setting
00239     gps_Serial = new Serial(p28,p27);
00240     Adafruit_GPS myGPS(gps_Serial); 
00241     Timer refresh_Timer;
00242     const int refresh_Time = 1000; //refresh time in ms
00243     myGPS.begin(GPS_BAUD_RATE); 
00244     
00245     char SenderIDc;
00246     //GPS Send Command
00247     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
00248     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
00249     myGPS.sendCommand(PGCMD_ANTENNA);
00250     
00251     wait(2);
00252        
00253     //interrupt start
00254     refresh_Timer.start();
00255 
00256     printf("start\n");
00257     
00258     while (true) {
00259         
00260         //Check Xbee Buffer Available
00261         xbee.readPacket();
00262             
00263         if (xbee.getResponse().isAvailable()) {
00264             xbee.getResponse().getZBRxResponse(zbRx);
00265             uint8_t *buf = zbRx.getFrameData();
00266                 
00267             if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
00268                 xbee.getResponse().getZBRxResponse(zbRx);
00269                 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
00270                 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
00271                 SenderIDc = buf1[5];//送信元のIDを取得する
00272                 char Command_type =agz.checkCommnadType(buf);//コマンドタイプを取得する
00273                  
00274                 //Check Command Type 
00275                 switch(Command_type){
00276                     //Get Request command
00277                     case STATUS_REQUEST:{
00278                         Send_Status(SenderIDc);                     
00279                         break;          
00280                     }
00281                     default:{
00282                         break;
00283                     }
00284                 }//endswitch
00285             }//endifZB_RX_RESPONSE
00286         }//endifisAvailable
00287                        
00288         myGPS.read();
00289         //recive gps module
00290         //check if we recieved a new message from GPS, if so, attempt to parse it,
00291         if ( myGPS.newNMEAreceived() ) {
00292             if ( !myGPS.parse(myGPS.lastNMEA()) ) {
00293                 continue;   
00294             }       
00295         }
00296         
00297         
00298         if (refresh_Timer.read_ms() >= refresh_Time) {
00299             refresh_Timer.reset();
00300             Get_GPS(&myGPS);
00301 
00302         }
00303     }
00304 }