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 = 'D';
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 
00337 void get_sigma(){
00338     double temp[2][2];
00339     for(int i=0;i<2;i++) {
00340         for(int j=0;j<2;j++) {
00341             for(int k=0;k<2;k++) {
00342                 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
00343             }
00344         }
00345    }
00346     for(int i = 0;i < 2;i++){
00347         for(int j = 0;j < 2;j++){
00348             sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
00349         }
00350     }   
00351 }
00352 
00353 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS){
00354     y[0] = Latitude;
00355     y[1] = Longitude;
00356     //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
00357     get_K();
00358     //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
00359     get_x();
00360     //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
00361     get_sigma();
00362     
00363     
00364     //kousinn
00365     for(int i = 0;i < 2;i++){
00366         for(int j = 0;j < 2;j++){
00367             K[0][i][j]=K[1][i][j];
00368             x[0][i]=x[1][i];
00369             sigma[0][i][j]=sigma[1][i][j];
00370         }
00371     }
00372     
00373     if(sigma[0][0][0] < SIGMA_MIN)sigma[0][0][0]=SIGMA_MIN;
00374     if(sigma[0][1][1] < SIGMA_MIN)sigma[0][1][1]=SIGMA_MIN;
00375             
00376     myGPS->latitudeKH=myGPS->latitudeH;//latitude after filtering
00377     myGPS->longitudeKH=myGPS->longitudeH;//longitude after filtering
00378     myGPS->latitudeKL=(long)x[1][0];//latitude after filtering
00379     myGPS->longitudeKL=(long)x[1][1];//longitude after filtering
00380     
00381     agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]);
00382 }
00383 
00384 
00385 
00386 /////////////////////////////////////////
00387 //
00388 //Main Processing
00389 //
00390 /////////////////////////////////////////
00391 int main() {
00392     //start up time
00393     wait(3);
00394     //set pc frequency to 57600bps 
00395     pc.baud(PC_BAUD_RATE); 
00396     //set xbee frequency to 57600bps
00397     xbee.begin(XBEE_BAUD_RATE);    
00398         
00399 
00400     //GPS setting
00401     gps_Serial = new Serial(p28,p27);
00402     Adafruit_GPS myGPS(gps_Serial); 
00403     Timer refresh_Timer;
00404     const int refresh_Time = 1000; //refresh time in ms
00405     Timer auto_Timer;
00406     const int auto_Time = 2000; //refresh time in ms
00407     int count = 0;
00408     myGPS.begin(GPS_BAUD_RATE); 
00409     
00410     Timer collect_Timer;
00411     const int collect_Time = 2000; //when we collect 4 GPS point, we use 
00412     int collect_flag = 0;
00413     char collect_state = 'a';
00414     
00415     char SenderIDc;
00416     //GPS Send Command
00417     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
00418     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
00419     myGPS.sendCommand(PGCMD_ANTENNA);
00420     
00421     wait(2);
00422        
00423     //interrupt start
00424     refresh_Timer.start();
00425     auto_Timer.start();
00426     collect_Timer.start();
00427     
00428     printf("start\n");
00429     
00430     while (true) {
00431         
00432         //Check Xbee Buffer Available
00433         xbee.readPacket();
00434             
00435         if (xbee.getResponse().isAvailable()) {
00436             xbee.getResponse().getZBRxResponse(zbRx);
00437             uint8_t *buf = zbRx.getFrameData();
00438                 
00439             if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
00440                 xbee.getResponse().getZBRxResponse(zbRx);
00441                 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
00442                 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
00443                 SenderIDc = buf1[5];//送信元のIDを取得する
00444                 char Command_type = agz.checkCommnadType(buf);//コマンドタイプを取得する
00445                  
00446                 //Check Command Type 
00447                 switch(Command_type){
00448                     //Get Request command
00449                     case MANUAL:{
00450                         Plus_Speed(buf);
00451                         break;
00452                     }
00453                     case STATUS_REQUEST:{
00454                         Send_Status(SenderIDc); 
00455                         printf("%c\n", SenderIDc);                  
00456                         break;          
00457                     }
00458                     case CHANGE_MODE:{
00459                         New_Mode(buf);
00460                         break;
00461                     }
00462                     case RECEIVE_STATUS:{
00463                         Get_Status(SenderIDc,buf1);
00464                         break;
00465                     }
00466                     case RECEIVE_KALMAN:{
00467                         Get_Status_Kalman(SenderIDc, buf1);
00468                         break;
00469                     }
00470                     default:{
00471                         break;
00472                     }
00473                 }//endswitch
00474             }//endifZB_RX_RESPONSE
00475         }//endifisAvailable
00476                        
00477         myGPS.read();
00478         //recive gps module
00479         //check if we recieved a new message from GPS, if so, attempt to parse it,
00480         if ( myGPS.newNMEAreceived() ) {
00481             if ( !myGPS.parse(myGPS.lastNMEA()) ) {
00482                 continue;   
00483             } 
00484             else{
00485                 count++;
00486             }    
00487         }
00488         //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
00489         if (refresh_Timer.read_ms() >= refresh_Time) {
00490             refresh_Timer.reset();
00491             //print_gps(count);
00492             Get_GPS(&myGPS);
00493 
00494         }
00495     
00496                  //get base GPS
00497         if( collect_Timer.read_ms() >= collect_Time){
00498             collect_Timer.reset();   
00499                      
00500             Send_Request_Base(collect_flag);
00501             
00502             collect_flag++;
00503             collect_state++;
00504 
00505             if(collect_flag == 4){
00506                 collect_state = 'a';
00507                 collect_flag = 0;
00508             }    
00509          } 
00510             
00511         if(agz.nowMode == AUTO_GPS_MODE && auto_Timer.read_ms() >= auto_Time){
00512             auto_Timer.reset();
00513             auto_Move(); 
00514         }
00515     }
00516 }