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 = 'C';
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 //Get GPS function
00146 //
00147 /////////////////////////////////////////
00148 
00149 void Get_GPS(Adafruit_GPS *myGPS){
00150     static bool flag = true;
00151     
00152     if (myGPS->fix) {
00153         agz.nowStatus = GPS_AVAIL;
00154         
00155         if(flag){//初期値設定
00156             if(myGPS->latitudeH>=36 && myGPS->latitudeH<=38 && myGPS->longitudeH>=138 && myGPS->longitudeH<=141){
00157                 flag = false;
00158                 x[0][0]=(double)myGPS->latitudeL;                        
00159                 x[0][1]=(double)myGPS->longitudeL;   
00160             } 
00161         }
00162         
00163         if(myGPS->longitudeH<138 || myGPS->longitudeH>141 || myGPS->latitudeH<36 || myGPS->latitudeH>38){
00164             return;
00165         }
00166         //Kalman Filter
00167         Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS);
00168                 
00169         agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
00170         agz.reNewRobotPointKalman(myGPS->latitudeKH,myGPS->latitudeKL,myGPS->longitudeKH,myGPS->longitudeKL);
00171     }
00172     else agz.nowStatus = GPS_UNAVAIL;
00173     
00174 } 
00175 
00176 /////////////////////////////////////////
00177 //
00178 //New Mode
00179 //
00180 /////////////////////////////////////////
00181 
00182 void New_Mode(uint8_t *packetdata){
00183   
00184   //bool result;
00185     agz.changeMode(&packetdata); 
00186   
00187 }
00188   
00189 /////////////////////////////////////////
00190 //
00191 //Get Status
00192 //
00193 /////////////////////////////////////////
00194 void Get_Status(char SenderIDc,uint8_t *packetdata){
00195     
00196     //マネージャからデータが来たとき
00197     if(SenderIDc == '0'){
00198         printf("get manager Status\n");
00199     }
00200     //他のロボットからデータが来たとき
00201     if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
00202         printf("get other robots Status\n");
00203     }
00204     //基地局からデータが来たとき
00205     if(SenderIDc >= 'a' && SenderIDc <= 'z'){
00206         printf("Get Base data\n");
00207         int id = SenderIDc - 'a';
00208         agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]);
00209         agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
00210         
00211         //debug
00212         for(int i = 0;i < 4;i++){
00213         printf("BASE:%d\n",i);
00214         printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\n",
00215             agz.get_basePoint_lati(i),agz.get_basePoint_longi(i),
00216             agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i)
00217             );
00218         }
00219     }
00220 }
00221 
00222 void Get_Status_Kalman(char SenderIDc,uint8_t *packetdata){
00223     
00224     //マネージャからデータが来たとき
00225     if(SenderIDc == '0'){
00226         printf("get manager Status Kalman\n");
00227     }
00228     //他のロボットからデータが来たとき
00229     if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
00230         printf("get other robots Status Kalman\n");
00231     }
00232     //基地局からデータが来たとき
00233     if(SenderIDc >= 'a' && SenderIDc <= 'z'){
00234         printf("Get Base data Kalman\n");
00235         int id = SenderIDc - 'a';
00236         agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
00237         
00238         //debug
00239         for(int i = 0;i < 4;i++){
00240         printf("BASE:%d\n",i);
00241         printf("latitudeK:%f,longitudeK:%f\n",
00242             agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i));
00243         }
00244     }
00245 }
00246 
00247 /////////////////////////////////////////
00248 //
00249 //Send_Request_to_base
00250 //
00251 /////////////////////////////////////////
00252 void Send_Request_Base(int basenumber){
00253     printf("send\n");
00254     agz.createRequestCommand(MyID, basenumber);
00255     //Select Destination
00256     ZBTxRequest tx64request(base_Address[basenumber],agz.packetData,agz.getPacketLength());
00257     //Send -> Base
00258     xbee.send(tx64request);
00259 }
00260 
00261 /////////////////////////////////////////
00262 //
00263 //auto_Move
00264 //
00265 //InAreaかOutAreaの判定
00266 //Kalmanを通した値を出力(Baseと自分)
00267 /////////////////////////////////////////
00268 
00269 void auto_Move(){
00270  
00271  bool result;
00272  int i;
00273  
00274  result = agz.gpsAuto();
00275  agz.set_agzAutoGPS();
00276  agz.set_agzKalmanGPS();
00277  
00278  if(result == true){
00279         printf("Out Area\n");
00280  }
00281 else if(result == false){
00282         printf("In Area\n");
00283 }  
00284 for(i = 0; i < 4; i++){
00285     printf("%d: %f, %f\n", i, agz.get_basePointKalman_lati(i), agz.get_basePointKalman_longi(i));
00286  }
00287  printf("robot: %f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
00288  
00289 }
00290 
00291 void print_gps(int count){
00292     
00293     printf("%d times:\n", count);
00294     printf("%f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
00295     
00296 }
00297 
00298 /////////////////////////////////////////
00299 //
00300 //Kalman Processing
00301 //
00302 /////////////////////////////////////////
00303     
00304 void get_K(){
00305   double temp[2][2]={
00306     {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
00307     {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
00308   };
00309   double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
00310   K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
00311   K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
00312 }
00313 
00314 
00315 void get_x(){
00316   x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
00317   x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
00318 }
00319 
00320 
00321 void get_sigma(){
00322     double temp[2][2];
00323     for(int i=0;i<2;i++) {
00324         for(int j=0;j<2;j++) {
00325             for(int k=0;k<2;k++) {
00326                 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
00327             }
00328         }
00329    }
00330     for(int i = 0;i < 2;i++){
00331         for(int j = 0;j < 2;j++){
00332             sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
00333         }
00334     }   
00335 }
00336 
00337 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS){
00338     y[0] = Latitude;
00339     y[1] = Longitude;
00340     //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
00341     get_K();
00342     //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
00343     get_x();
00344     //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
00345     get_sigma();
00346     
00347     
00348     //kousinn
00349     for(int i = 0;i < 2;i++){
00350         for(int j = 0;j < 2;j++){
00351             K[0][i][j]=K[1][i][j];
00352             x[0][i]=x[1][i];
00353             sigma[0][i][j]=sigma[1][i][j];
00354         }
00355     }
00356             
00357     if(sigma[0][0][0] < SIGMA_MIN)sigma[0][0][0]=SIGMA_MIN;
00358     if(sigma[0][1][1] < SIGMA_MIN)sigma[0][1][1]=SIGMA_MIN;
00359     
00360     myGPS->latitudeKH=myGPS->latitudeH;//latitude after filtering
00361     myGPS->longitudeKH=myGPS->longitudeH;//longitude after filtering
00362     myGPS->latitudeKL=(long)x[1][0];//latitude after filtering
00363     myGPS->longitudeKL=(long)x[1][1];//longitude after filtering
00364     
00365     agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]);
00366 }
00367 
00368 
00369 /////////////////////////////////////////
00370 //
00371 //Main Processing
00372 //
00373 /////////////////////////////////////////
00374 int main() {
00375     //start up time
00376     wait(3);
00377     //set pc frequency to 57600bps 
00378     pc.baud(PC_BAUD_RATE); 
00379     //set xbee frequency to 57600bps
00380     xbee.begin(XBEE_BAUD_RATE);    
00381         
00382 
00383     //GPS setting
00384     gps_Serial = new Serial(p28,p27);
00385     Adafruit_GPS myGPS(gps_Serial); 
00386     Timer refresh_Timer;
00387     const int refresh_Time = 1000; //refresh time in ms
00388     Timer auto_Timer;
00389     const int auto_Time = 2000; //refresh time in ms
00390     int count = 0;
00391     myGPS.begin(GPS_BAUD_RATE); 
00392     
00393     Timer collect_Timer;
00394     const int collect_Time = 2000; //when we collect 4 GPS point, we use 
00395     int collect_flag = 0;
00396     char collect_state = 'a';
00397     
00398     char SenderIDc;
00399     //GPS Send Command
00400     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
00401     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
00402     myGPS.sendCommand(PGCMD_ANTENNA);
00403     
00404     wait(2);
00405        
00406     //interrupt start
00407     refresh_Timer.start();
00408     auto_Timer.start();
00409     collect_Timer.start();
00410     
00411     printf("start\n");
00412     
00413     while (true) {
00414         
00415         //Check Xbee Buffer Available
00416         xbee.readPacket();
00417             
00418         if (xbee.getResponse().isAvailable()) {
00419             xbee.getResponse().getZBRxResponse(zbRx);
00420             uint8_t *buf = zbRx.getFrameData();
00421                 
00422             if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
00423                 xbee.getResponse().getZBRxResponse(zbRx);
00424                 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
00425                 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
00426                 SenderIDc = buf1[5];//送信元のIDを取得する
00427                 char Command_type =agz.checkCommnadType(buf);//コマンドタイプを取得する
00428                  
00429                 //Check Command Type 
00430                 switch(Command_type){
00431                     //Get Request command
00432                     case MANUAL:{
00433                         Plus_Speed(buf);
00434                         break;
00435                     }
00436                     case STATUS_REQUEST:{
00437                         Send_Status(SenderIDc);                     
00438                         break;          
00439                     }
00440                     case CHANGE_MODE:{
00441                         New_Mode(buf);
00442                         break;
00443                     }
00444                     case RECEIVE_STATUS:{
00445                         Get_Status(SenderIDc,buf1);
00446                         break;
00447                     }
00448                     case RECEIVE_KALMAN:{
00449                         Get_Status_Kalman(SenderIDc, buf1);
00450                         break;
00451                     }
00452                     default:{
00453                         break;
00454                     }
00455                 }//endswitch
00456             }//endifZB_RX_RESPONSE
00457         }//endifisAvailable
00458                        
00459         myGPS.read();
00460         //recive gps module
00461         //check if we recieved a new message from GPS, if so, attempt to parse it,
00462         if ( myGPS.newNMEAreceived() ) {
00463             if ( !myGPS.parse(myGPS.lastNMEA()) ) {
00464                 continue;   
00465             } 
00466             else{
00467                 count++;
00468             }    
00469         }
00470         //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
00471         if (refresh_Timer.read_ms() >= refresh_Time) {
00472             refresh_Timer.reset();
00473             //print_gps(count);
00474             Get_GPS(&myGPS);
00475 
00476         }
00477         
00478                 //get base GPS
00479         if( collect_Timer.read_ms() >= collect_Time){
00480             collect_Timer.reset();   
00481                      
00482             Send_Request_Base(collect_flag);
00483             
00484             collect_flag++;
00485             collect_state++;
00486 
00487             if(collect_flag == 4){
00488                 collect_state = 'a';
00489                 collect_flag = 0;
00490             }    
00491          } 
00492                 
00493         if(agz.nowMode == AUTO_GPS_MODE && auto_Timer.read_ms() >= auto_Time){
00494             auto_Timer.reset();
00495             auto_Move(); 
00496         }
00497     }
00498 }