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