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