main.cpp

00001 /**********************************************/
00002 //  
00003 //    
00004 //
00005 //  Program name: Aigamozu BASE
00006 //  Author: Mineta Kizuku
00007 //  
00008 //
00009 /**********************************************/
00010 
00011 /**********************************************/
00012 //更新情報
00013 //2015/05/11
00014 //ベースプログラムの作成
00015 //
00016 //2015/05/12
00017 //ロボットプログラムへの変更
00018 //
00019 //main()内でAutoModeのときでかつ2000ms経った時にGPSを取得する。
00020 //auto_Move()の作成
00021 ////GPSをセットする。
00022 ////gpsAutoを使いInAreaかOutAreaの判定。
00023 ////GPSをプリントする。
00024 //
00025 //AigamozuControlPackets内
00026 ////GPSの設定
00027 ////get_KAlman, set_autoGPS, set_KalmanGPS系の関数の作成。
00028 ////set_GPS系関数内
00029 ////呼び出すごとに+=0.0001等を設定
00030 //
00031 /**********************************************/
00032 
00033 #include "mbed.h"
00034 #include "XBee.h"
00035 #include "MBed_Adafruit_GPS.h"
00036 #include "AigamozuControlPackets.h"
00037 #include "agzIDLIST.h"
00038 #include "aigamozuSetting.h"
00039 #include "agz_common.h"
00040 #include "Kalman.h"
00041 
00042 //************ID Number*****************
00043 const char MyID = 'D';
00044 //************ID Number*****************
00045 
00046 /////////////////////////////////////////
00047 //
00048 //Pin Setting
00049 //
00050 /////////////////////////////////////////
00051 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
00052 
00053 
00054 /////////////////////////////////////////
00055 //
00056 //Connection Setting
00057 //
00058 /////////////////////////////////////////
00059 
00060 //Serial Connect Setting: PC <--> mbed
00061 Serial pc(USBTX, USBRX);    
00062 
00063 //Serial Connect Setting: GPS <--> mbed
00064 Serial * gps_Serial;
00065 
00066 //Serial Connect Setting: XBEE <--> mbed
00067 XBee xbee(p13,p14);
00068 ZBRxResponse zbRx = ZBRxResponse();
00069 
00070 //set up GPS module
00071 
00072 //set up AigamozuControlPackets library
00073 AigamozuControlPackets agz(agz_motorShield);
00074 
00075 
00076 /////////////////////////////////////////
00077 //
00078 //For Kalman data
00079 //
00080 /////////////////////////////////////////
00081 double sigmaGPS[2][2] = {{250,0},{0,250}};
00082 double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
00083 double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
00084 double y[2],x[2][2]={0};
00085 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS);
00086 
00087 /////////////////////////////////////////
00088 //
00089 //Address List
00090 //
00091 /////////////////////////////////////////
00092 XBeeAddress64 base_Address[BaseNumber] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 
00093                                  XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L),
00094                                  XBeeAddress64(BASE5_32H,BASE5_32L)};
00095 XBeeAddress64 robot_Address[RobotNumber] = {XBeeAddress64(ROBOT1_32H,ROBOT1_32L), XBeeAddress64(ROBOT1_32H,ROBOT1_32L)};
00096 XBeeAddress64 manager_Address = XBeeAddress64(BASE1_32H,BASE1_32L);
00097 
00098 
00099 
00100 /////////////////////////////////////////
00101 //
00102 //Send_Status
00103 //
00104 //リクエストがきたとき、自分の位置情報などを返信する
00105 /////////////////////////////////////////
00106 void Send_Status(char SenderIDc){
00107     XBeeAddress64 send_Address;
00108     if(SenderIDc == '0'){
00109         send_Address = manager_Address;
00110     }
00111     if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
00112         send_Address = robot_Address[SenderIDc - 'A'];
00113     }
00114     if(SenderIDc >= 'a' && SenderIDc <= 'z'){
00115         send_Address = base_Address[SenderIDc - 'a'];
00116     }
00117     //send normal data
00118     //Create GPS Infomation Packet
00119 //    agz.createReceiveStatusCommand(MyID,SenderIDc,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     //Select Destination
00123 //    ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
00124     //Send -> Base
00125 //    xbee.send(tx64request);
00126 }
00127 
00128 /////////////////////////////////////////
00129 //
00130 //Get GPS function
00131 //
00132 /////////////////////////////////////////
00133 
00134 void Get_GPS(Adafruit_GPS *myGPS){
00135     static bool flag = true;
00136     
00137     if (myGPS->fix) {
00138         agz.nowStatus = GPS_AVAIL;
00139         
00140         if(flag){//初期値設定
00141             if(myGPS->longitudeH>=138 && myGPS->longitudeH<=141 && myGPS->latitudeH>=36 && myGPS->latitudeH<=38){
00142                 flag = false;
00143                 x[0][0]=(double)myGPS->latitudeL;                        
00144                 x[0][1]=(double)myGPS->longitudeL;   
00145             } 
00146         }
00147         
00148         if(myGPS->longitudeH<138 || myGPS->longitudeH>141 || myGPS->latitudeH<36 || myGPS->latitudeH>38){
00149             return;
00150         }
00151         
00152         //Kalman Filter
00153         Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS);
00154                 
00155         agz.reNewRobotPoint(myGPS->longitudeH,myGPS->longitudeL,myGPS->latitudeH,myGPS->latitudeL);
00156         agz.reNewRobotPointKalman(myGPS->longitudeKH,myGPS->longitudeKL,myGPS->latitudeKH,myGPS->latitudeKL);
00157     }
00158     else agz.nowStatus = GPS_UNAVAIL;
00159     
00160 } 
00161 
00162 
00163 /////////////////////////////////////////
00164 //
00165 //Kalman Processing
00166 //
00167 /////////////////////////////////////////
00168     
00169 void get_K(){
00170   double temp[2][2]={
00171     {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
00172     {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
00173   };
00174   double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
00175   K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
00176   K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
00177 }
00178 
00179 
00180 void get_x(){
00181   x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
00182   x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
00183 }
00184 
00185 
00186 void get_sigma(){
00187     double temp[2][2];
00188     for(int i=0;i<2;i++) {
00189         for(int j=0;j<2;j++) {
00190             for(int k=0;k<2;k++) {
00191                 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
00192             }
00193         }
00194    }
00195     for(int i = 0;i < 2;i++){
00196         for(int j = 0;j < 2;j++){
00197             sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
00198         }
00199     }   
00200 }
00201 
00202 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS){
00203     y[0] = Latitude;
00204     y[1] = Longitude;
00205     //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
00206     get_K();
00207     //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
00208     get_x();
00209     //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
00210     get_sigma();
00211     
00212     
00213     //kousinn
00214     for(int i = 0;i < 2;i++){
00215         for(int j = 0;j < 2;j++){
00216             K[0][i][j]=K[1][i][j];
00217             x[0][i]=x[1][i];
00218             sigma[0][i][j]=sigma[1][i][j];
00219         }
00220     }
00221             
00222     myGPS->latitudeKH=myGPS->latitudeH;//latitude after filtering
00223     myGPS->longitudeKH=myGPS->longitudeH;//longitude after filtering
00224     myGPS->latitudeKL=(long)x[1][0];//latitude after filtering
00225     myGPS->longitudeKL=(long)x[1][1];//longitude after filtering
00226     
00227     agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]);
00228 }
00229 
00230 void auto_Move(){
00231  
00232  bool result;
00233  int i;
00234  
00235  result = agz.gpsAuto();
00236  agz.set_agzAutoGPS();
00237  agz.set_agzKalmanGPS();
00238  
00239  if(result == true){
00240         printf("Out Area\n");
00241  }
00242 else if(result == false){
00243         printf("In Area\n");
00244 }  
00245 for(i = 0; i < 4; i++){
00246     printf("%d: %f, %f\n", i, agz.get_basePointKalman_lati(i), agz.get_basePointKalman_longi(i));
00247  }
00248  printf("robot: %f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
00249  
00250 }
00251 
00252 /////////////////////////////////////////
00253 //
00254 //Main Processing
00255 //
00256 /////////////////////////////////////////
00257 int main() {
00258     //start up time
00259     wait(3);
00260     //set pc frequency to 57600bps 
00261     pc.baud(PC_BAUD_RATE); 
00262     //set xbee frequency to 57600bps
00263     xbee.begin(XBEE_BAUD_RATE);    
00264         
00265 
00266     //GPS setting
00267     gps_Serial = new Serial(p28,p27);
00268     Adafruit_GPS myGPS(gps_Serial); 
00269     Timer refresh_Timer;
00270     const int refresh_Time = 2000; //refresh time in ms
00271     myGPS.begin(GPS_BAUD_RATE); 
00272     
00273     char SenderIDc;
00274     
00275     Timer auto_Timer;
00276     const int auto_Time = 2000;
00277     agz.nowMode = AUTO_MODE;
00278     
00279     //GPS Send Command
00280     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
00281     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
00282     myGPS.sendCommand(PGCMD_ANTENNA);
00283     
00284     wait(2);
00285        
00286     //interrupt start
00287     refresh_Timer.start();
00288     auto_Timer.start();
00289     
00290     while (true) {
00291         
00292         //Check Xbee Buffer Available
00293         xbee.readPacket();
00294             
00295         if (xbee.getResponse().isAvailable()) {
00296             xbee.getResponse().getZBRxResponse(zbRx);
00297             uint8_t *buf = zbRx.getFrameData();
00298                 
00299             if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
00300                 xbee.getResponse().getZBRxResponse(zbRx);
00301                 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
00302                 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
00303                 SenderIDc = buf1[5];//送信元のIDを取得する
00304                 char Command_type =agz.checkCommnadType(buf);//コマンドタイプを取得する
00305                  
00306                 //Check Command Type 
00307                 switch(Command_type){
00308                     //Get Request command
00309                     case STATUS_REQUEST:{
00310                         Send_Status(SenderIDc);                            
00311                         break;          
00312                     }
00313                     default:{
00314                         break;
00315                     }
00316                 }//endswitch
00317             }//endifZB_RX_RESPONSE
00318         }//endifisAvailable
00319         
00320         
00321                        
00322         myGPS.read();
00323         //recive gps module
00324         //check if we recieved a new message from GPS, if so, attempt to parse it,
00325         if ( myGPS.newNMEAreceived() ) {
00326             if ( !myGPS.parse(myGPS.lastNMEA()) ) {
00327                 continue;   
00328             }       
00329         }
00330         
00331         
00332         if (refresh_Timer.read_ms() >= refresh_Time) {
00333             refresh_Timer.reset();
00334             Get_GPS(&myGPS);
00335             
00336         }
00337         
00338         if(agz.nowMode == AUTO_MODE && auto_Timer.read_ms() >= auto_Time){
00339             auto_Timer.reset();
00340             auto_Move(); 
00341         }
00342         
00343     }
00344 }