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<=140){
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>140 || 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",id);
00195         printf("status:%d, latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\n",
00196             agz.get_base_status(id), agz.get_basePoint_lati(id),agz.get_basePoint_longi(id),
00197             agz.get_basePointKalman_lati(id),agz.get_basePointKalman_longi(id)
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 }