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