main.cpp

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