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