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