main.cpp

00001 /**********************************************/
00002 //  
00003 //    
00004 //
00005 //  Program name: Aigamozu ROBOT
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 //2015/05/13
00021 //何回目のGPSでとられたGPSか確認するようにしました。
00022 //
00023 //2015/05/15
00024 //プログラムcreateReceiveStatusCommand()にて
00025 //Aigamozu_collect_dataにinかoutかを送るためにstate関連をいじったので必要に応じて直してください。
00026 //
00027 /**********************************************/
00028 
00029 #include "mbed.h"
00030 #include "XBee.h"
00031 #include "MBed_Adafruit_GPS.h"
00032 #include "AigamozuControlPackets.h"
00033 #include "agzIDLIST.h"
00034 #include "aigamozuSetting.h"
00035 #include "Kalman.h"
00036 
00037 #define SIGMA_MIN 0.0001
00038 
00039 //************ID Number*****************
00040 //Robot   ID: 'A' ~ 'Z'
00041 //Base    ID: 'a' ~ 'a'
00042 //manager ID: '0'(数字のゼロ)
00043 //
00044 const char MyID = 'B';
00045 //************ID Number*****************
00046 
00047 /////////////////////////////////////////
00048 //
00049 //Pin Setting
00050 //
00051 /////////////////////////////////////////
00052 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
00053 
00054 
00055 /////////////////////////////////////////
00056 //
00057 //Connection Setting
00058 //
00059 /////////////////////////////////////////
00060 
00061 //Serial Connect Setting: PC <--> mbed
00062 Serial pc(USBTX, USBRX);    
00063 
00064 //Serial Connect Setting: GPS <--> mbed
00065 Serial * gps_Serial;
00066 
00067 //Serial Connect Setting: XBEE <--> mbed
00068 XBee xbee(p13,p14);
00069 ZBRxResponse zbRx = ZBRxResponse();
00070 
00071 //set up GPS module
00072 
00073 //set up AigamozuControlPackets library
00074 AigamozuControlPackets agz(agz_motorShield);
00075 
00076 
00077 /////////////////////////////////////////
00078 //
00079 //For Kalman data
00080 //
00081 /////////////////////////////////////////
00082 double sigmaGPS[2][2] = {{250,0},{0,250}};
00083 double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
00084 double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
00085 double y[2],x[2][2]={0};
00086 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS);
00087 
00088 
00089 /////////////////////////////////////////
00090 //
00091 //Plus Speed
00092 //
00093 //MNUAL_MODEの時にスピードを変える
00094 /////////////////////////////////////////
00095 void Plus_Speed(uint8_t *packetdata){
00096        
00097     if(agz.nowMode == MANUAL_MODE){ 
00098         agz.changeSpeed(packetdata);
00099     }
00100     
00101 }
00102 
00103 /////////////////////////////////////////
00104 //
00105 //Send Status
00106 //
00107 //リクエストがきたとき、自分の位置情報などを返信する
00108 /////////////////////////////////////////
00109 void Send_Status(char SenderIDc){
00110     XBeeAddress64 send_Address;
00111     if(SenderIDc == '0'){
00112         send_Address = manager_Address;
00113     }
00114     if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
00115         send_Address = robot_Address[SenderIDc - 'A'];
00116     }
00117     if(SenderIDc >= 'a' && SenderIDc <= 'z'){
00118         send_Address = base_Address[SenderIDc - 'a'];
00119     }
00120     //send normal data
00121     //Create GPS Infomation Packet
00122     agz.createReceiveStatusCommand(MyID,SenderIDc, (int)agz.gpsAuto(),
00123                                     agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00124                                     agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00125                                     agz.get_agzCov_lati(),agz.get_agzCov_longi());
00126     
00127     //debug***************************************************
00128     printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
00129             agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00130             agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00131             agz.get_agzCov_lati(),agz.get_agzCov_longi()
00132             );
00133     for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
00134         printf("\n");
00135     //debug end***************************************************
00136     
00137     //Select Destination
00138     ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
00139     //Send -> Base
00140     xbee.send(tx64request);
00141     
00142 }
00143 
00144 /////////////////////////////////////////
00145 //
00146 //Get GPS function
00147 //
00148 /////////////////////////////////////////
00149 
00150 void Get_GPS(Adafruit_GPS *myGPS){
00151     static bool flag = true;
00152     
00153     if (myGPS->fix) {
00154         agz.nowStatus = GPS_AVAIL;
00155         
00156         if(flag){//初期値設定
00157             if(myGPS->latitudeH>=36 && myGPS->latitudeH<=38 && myGPS->longitudeH>=138 && myGPS->longitudeH<=141){
00158                 flag = false;
00159                 x[0][0]=(double)myGPS->latitudeL;                        
00160                 x[0][1]=(double)myGPS->longitudeL;   
00161             } 
00162         }
00163         
00164         if(myGPS->longitudeH<138 || myGPS->longitudeH>141 || myGPS->latitudeH<36 || myGPS->latitudeH>38){
00165             return;
00166         }
00167         //Kalman Filter
00168         Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS);
00169                 
00170         agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
00171         agz.reNewRobotPointKalman(myGPS->latitudeKH,myGPS->latitudeKL,myGPS->longitudeKH,myGPS->longitudeKL);
00172     }
00173     else agz.nowStatus = GPS_UNAVAIL;
00174     
00175 } 
00176 
00177 /////////////////////////////////////////
00178 //
00179 //New Mode
00180 //
00181 /////////////////////////////////////////
00182 
00183 void New_Mode(uint8_t *packetdata){
00184   
00185   //bool result;
00186    agz.changeMode(packetdata); 
00187   
00188 }
00189   
00190 /////////////////////////////////////////
00191 //
00192 //Get Status
00193 //
00194 /////////////////////////////////////////
00195 void Get_Status(char SenderIDc,uint8_t *packetdata){
00196     
00197     //マネージャからデータが来たとき
00198     if(SenderIDc == '0'){
00199         printf("get manager Status\n");
00200     }
00201     //他のロボットからデータが来たとき
00202     if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
00203         printf("get other robots Status\n");
00204     }
00205     //基地局からデータが来たとき
00206     if(SenderIDc >= 'a' && SenderIDc <= 'z'){
00207         printf("Get Base data\n");
00208         int id = SenderIDc - 'a';
00209         agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]);
00210         agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
00211         
00212         //debug
00213         for(int i = 0;i < 4;i++){
00214         printf("BASE:%d\n",i);
00215         printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\n",
00216             agz.get_basePoint_lati(i),agz.get_basePoint_longi(i),
00217             agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i)
00218             );
00219         }
00220     }
00221 }
00222 
00223 void Get_Status_Kalman(char SenderIDc,uint8_t *packetdata){
00224     
00225     //マネージャからデータが来たとき
00226     if(SenderIDc == '0'){
00227         printf("get manager Status Kalman\n");
00228     }
00229     //他のロボットからデータが来たとき
00230     if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
00231         printf("get other robots Status Kalman\n");
00232     }
00233     //基地局からデータが来たとき
00234     if(SenderIDc >= 'a' && SenderIDc <= 'z'){
00235         printf("Get Base data Kalman\n");
00236         int id = SenderIDc - 'a';
00237         agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
00238         
00239         //debug
00240         for(int i = 0;i < 4;i++){
00241         printf("BASE:%d\n",i);
00242         printf("latitudeK:%f,longitudeK:%f\n",
00243             agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i));
00244         }
00245     }
00246 }
00247 
00248 /////////////////////////////////////////
00249 //
00250 //Send_Request_to_base
00251 //
00252 /////////////////////////////////////////
00253 void Send_Request_Base(int basenumber){
00254     printf("send\n");
00255     agz.createRequestCommand(MyID, basenumber);
00256     //Select Destination
00257     ZBTxRequest tx64request(base_Address[basenumber],agz.packetData,agz.getPacketLength());
00258     //Send -> Base
00259     xbee.send(tx64request);
00260 }
00261 
00262 /////////////////////////////////////////
00263 //
00264 //auto_Move
00265 //
00266 //InAreaかOutAreaの判定
00267 //Kalmanを通した値を出力(Baseと自分)
00268 /////////////////////////////////////////
00269 
00270 void auto_Move(){
00271  
00272  bool result;
00273  int i;
00274  
00275  result = agz.gpsAuto();
00276  //agz.set_agzAutoGPS();
00277  //agz.set_agzKalmanGPS();
00278  
00279  if(result == true){
00280         printf("Out Area\n");
00281  }
00282 else if(result == false){
00283         printf("In Area\n");
00284 }  
00285 for(i = 0; i < 4; i++){
00286     printf("%d: %f, %f\n", i, agz.get_basePointKalman_lati(i), agz.get_basePointKalman_longi(i));
00287  }
00288  printf("robot: %f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
00289  
00290 }
00291 
00292 void print_gps(int count){
00293     
00294     printf("%d times:\n", count);
00295     printf("%f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
00296     
00297 }
00298 
00299 /////////////////////////////////////////
00300 //
00301 //Kalman Processing
00302 //
00303 /////////////////////////////////////////
00304     
00305 void get_K(){
00306   double temp[2][2]={
00307     {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
00308     {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
00309   };
00310   double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
00311   K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
00312   K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
00313 }
00314 
00315 
00316 void get_x(){
00317   x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
00318   x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
00319 }
00320 
00321 
00322 void get_sigma(){
00323     double temp[2][2];
00324     for(int i=0;i<2;i++) {
00325         for(int j=0;j<2;j++) {
00326             for(int k=0;k<2;k++) {
00327                 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
00328             }
00329         }
00330    }
00331     for(int i = 0;i < 2;i++){
00332         for(int j = 0;j < 2;j++){
00333             sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
00334         }
00335     }   
00336 }
00337 
00338 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS){
00339     y[0] = Latitude;
00340     y[1] = Longitude;
00341     //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
00342     get_K();
00343     //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
00344     get_x();
00345     //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
00346     get_sigma();
00347     
00348     
00349     //kousinn
00350     for(int i = 0;i < 2;i++){
00351         for(int j = 0;j < 2;j++){
00352             K[0][i][j]=K[1][i][j];
00353             x[0][i]=x[1][i];
00354             sigma[0][i][j]=sigma[1][i][j];
00355         }
00356     }
00357             
00358     if(sigma[0][0][0] < SIGMA_MIN)sigma[0][0][0]=SIGMA_MIN;
00359     if(sigma[0][1][1] < SIGMA_MIN)sigma[0][1][1]=SIGMA_MIN;
00360     
00361     myGPS->latitudeKH=myGPS->latitudeH;//latitude after filtering
00362     myGPS->longitudeKH=myGPS->longitudeH;//longitude after filtering
00363     myGPS->latitudeKL=(long)x[1][0];//latitude after filtering
00364     myGPS->longitudeKL=(long)x[1][1];//longitude after filtering
00365     
00366     agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]);
00367 }
00368 
00369 
00370 /////////////////////////////////////////
00371 //
00372 //Main Processing
00373 //
00374 /////////////////////////////////////////
00375 int main() {
00376     //start up time
00377     wait(3);
00378     //set pc frequency to 57600bps 
00379     pc.baud(PC_BAUD_RATE); 
00380     //set xbee frequency to 57600bps
00381     xbee.begin(XBEE_BAUD_RATE);    
00382         
00383 
00384     //GPS setting
00385     gps_Serial = new Serial(p28,p27);
00386     Adafruit_GPS myGPS(gps_Serial); 
00387     Timer refresh_Timer;
00388     const int refresh_Time = 1000; //refresh time in ms
00389     Timer auto_Timer;
00390     const int auto_Time = 2000; //refresh time in ms
00391     int count = 0;
00392     myGPS.begin(GPS_BAUD_RATE); 
00393     
00394     Timer collect_Timer;
00395     const int collect_Time = 2000; //when we collect 4 GPS point, we use 
00396     int collect_flag = 0;
00397     char collect_state = 'a';
00398     
00399     char SenderIDc;
00400     //GPS Send Command
00401     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
00402     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
00403     myGPS.sendCommand(PGCMD_ANTENNA);
00404     
00405     wait(2);
00406        
00407     //interrupt start
00408     refresh_Timer.start();
00409     auto_Timer.start();
00410     collect_Timer.start();
00411     
00412     printf("start\n");
00413     
00414     while (true) {
00415         
00416         //Check Xbee Buffer Available
00417         xbee.readPacket();
00418             
00419         if (xbee.getResponse().isAvailable()) {
00420             xbee.getResponse().getZBRxResponse(zbRx);
00421             uint8_t *buf = zbRx.getFrameData();
00422                 
00423             if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
00424                 xbee.getResponse().getZBRxResponse(zbRx);
00425                 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
00426                 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
00427                 SenderIDc = buf1[5];//送信元のIDを取得する
00428                 char Command_type =agz.checkCommnadType(buf);//コマンドタイプを取得する
00429                  
00430                 //Check Command Type 
00431                 switch(Command_type){
00432                     //Get Request command
00433                     case MANUAL:{
00434                         Plus_Speed(buf);
00435                         break;
00436                     }
00437                     case STATUS_REQUEST:{
00438                         Send_Status(SenderIDc);                     
00439                         break;          
00440                     }
00441                     case CHANGE_MODE:{
00442                         New_Mode(buf);
00443                         break;
00444                     }
00445                     case RECEIVE_STATUS:{
00446                         Get_Status(SenderIDc,buf1);
00447                         break;
00448                     }
00449                     case RECEIVE_KALMAN:{
00450                         Get_Status_Kalman(SenderIDc, buf1);
00451                         break;
00452                     }
00453                     default:{
00454                         break;
00455                     }
00456                 }//endswitch
00457             }//endifZB_RX_RESPONSE
00458         }//endifisAvailable
00459                        
00460         myGPS.read();
00461         //recive gps module
00462         //check if we recieved a new message from GPS, if so, attempt to parse it,
00463         if ( myGPS.newNMEAreceived() ) {
00464             if ( !myGPS.parse(myGPS.lastNMEA()) ) {
00465                 continue;   
00466             } 
00467             else{
00468                 count++;
00469             }    
00470         }
00471         //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
00472         if (refresh_Timer.read_ms() >= refresh_Time) {
00473             refresh_Timer.reset();
00474             //print_gps(count);
00475             Get_GPS(&myGPS);
00476 
00477         }
00478     
00479                  //get base GPS
00480         if( collect_Timer.read_ms() >= collect_Time){
00481             collect_Timer.reset();   
00482                      
00483             Send_Request_Base(collect_flag);
00484             
00485             collect_flag++;
00486             collect_state++;
00487 
00488             if(collect_flag == 4){
00489                 collect_state = 'a';
00490                 collect_flag = 0;
00491             }    
00492          } 
00493             
00494         if(agz.nowMode == AUTO_GPS_MODE && auto_Timer.read_ms() >= auto_Time){
00495             auto_Timer.reset();
00496             auto_Move(); 
00497         }
00498     }
00499 }