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,agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00106                                     agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00107                                     agz.get_agzCov_lati(),agz.get_agzCov_longi());
00108     //Select Destination
00109     ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
00110     //Send -> Base
00111     xbee.send(tx64request);
00112 }
00113 
00114 /////////////////////////////////////////
00115 //
00116 //Get GPS function
00117 //
00118 /////////////////////////////////////////
00119 
00120 void Get_GPS(Adafruit_GPS *myGPS){
00121     static bool flag = true;
00122     
00123     if (myGPS->fix) {
00124         agz.nowStatus = GPS_AVAIL;
00125         
00126         if(flag){//初期値設定
00127             if(myGPS->longitudeH>=138 && myGPS->longitudeH<=141 && myGPS->latitudeH>=36 && myGPS->latitudeH<=38){
00128                 flag = false;
00129                 x[0][0]=(double)myGPS->latitudeL;                        
00130                 x[0][1]=(double)myGPS->longitudeL;   
00131             } 
00132         }
00133         
00134         if(myGPS->longitudeH<138 || myGPS->longitudeH>141 || myGPS->latitudeH<36 || myGPS->latitudeH>38){
00135             return;
00136         }
00137         //Kalman Filter
00138         Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS);
00139                 
00140         agz.reNewRobotPoint(myGPS->longitudeH,myGPS->longitudeL,myGPS->latitudeH,myGPS->latitudeL);
00141         agz.reNewRobotPointKalman(myGPS->longitudeKH,myGPS->longitudeKL,myGPS->latitudeKH,myGPS->latitudeKL);
00142     }
00143     else agz.nowStatus = GPS_UNAVAIL;
00144     
00145 } 
00146 
00147 
00148 /////////////////////////////////////////
00149 //
00150 //Kalman Processing
00151 //
00152 /////////////////////////////////////////
00153     
00154 void get_K(){
00155   double temp[2][2]={
00156     {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
00157     {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
00158   };
00159   double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
00160   K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
00161   K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
00162 }
00163 
00164 
00165 void get_x(){
00166   x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
00167   x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
00168 }
00169 
00170 
00171 void get_sigma(){
00172     double temp[2][2];
00173     for(int i=0;i<2;i++) {
00174         for(int j=0;j<2;j++) {
00175             for(int k=0;k<2;k++) {
00176                 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
00177             }
00178         }
00179    }
00180     for(int i = 0;i < 2;i++){
00181         for(int j = 0;j < 2;j++){
00182             sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
00183         }
00184     }   
00185 }
00186 
00187 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS){
00188     y[0] = Latitude;
00189     y[1] = Longitude;
00190     //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
00191     get_K();
00192     //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
00193     get_x();
00194     //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
00195     get_sigma();
00196     
00197     
00198     //kousinn
00199     for(int i = 0;i < 2;i++){
00200         for(int j = 0;j < 2;j++){
00201             K[0][i][j]=K[1][i][j];
00202             x[0][i]=x[1][i];
00203             sigma[0][i][j]=sigma[1][i][j];
00204         }
00205     }
00206             
00207     myGPS->latitudeKH=myGPS->latitudeH;//latitude after filtering
00208     myGPS->longitudeKH=myGPS->longitudeH;//longitude after filtering
00209     myGPS->latitudeKL=(long)x[1][0];//latitude after filtering
00210     myGPS->longitudeKL=(long)x[1][1];//longitude after filtering
00211     
00212     agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]);
00213 }
00214 
00215 
00216 /////////////////////////////////////////
00217 //
00218 //Main Processing
00219 //
00220 /////////////////////////////////////////
00221 int main() {
00222     //start up time
00223     wait(3);
00224     //set pc frequency to 57600bps 
00225     pc.baud(PC_BAUD_RATE); 
00226     //set xbee frequency to 57600bps
00227     xbee.begin(XBEE_BAUD_RATE);    
00228         
00229 
00230     //GPS setting
00231     gps_Serial = new Serial(p28,p27);
00232     Adafruit_GPS myGPS(gps_Serial); 
00233     Timer refresh_Timer;
00234     const int refresh_Time = 2000; //refresh time in ms
00235     myGPS.begin(GPS_BAUD_RATE); 
00236     
00237     char SenderIDc;
00238     //GPS Send Command
00239     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
00240     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
00241     myGPS.sendCommand(PGCMD_ANTENNA);
00242     
00243     wait(2);
00244        
00245     //interrupt start
00246     refresh_Timer.start();
00247 
00248     printf("start\n");
00249     
00250     while (true) {
00251         
00252         //Check Xbee Buffer Available
00253         xbee.readPacket();
00254             
00255         if (xbee.getResponse().isAvailable()) {
00256             xbee.getResponse().getZBRxResponse(zbRx);
00257             uint8_t *buf = zbRx.getFrameData();
00258                 
00259             if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
00260                 xbee.getResponse().getZBRxResponse(zbRx);
00261                 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
00262                 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
00263                 SenderIDc = buf1[5];//送信元のIDを取得する
00264                 char Command_type =agz.checkCommnadType(buf);//コマンドタイプを取得する
00265                  
00266                 //Check Command Type 
00267                 switch(Command_type){
00268                     //Get Request command
00269                     case STATUS_REQUEST:{
00270                         Send_Status(SenderIDc);                            
00271                         break;          
00272                     }
00273                     default:{
00274                         break;
00275                     }
00276                 }//endswitch
00277             }//endifZB_RX_RESPONSE
00278         }//endifisAvailable
00279                        
00280         myGPS.read();
00281         //recive gps module
00282         //check if we recieved a new message from GPS, if so, attempt to parse it,
00283         if ( myGPS.newNMEAreceived() ) {
00284             if ( !myGPS.parse(myGPS.lastNMEA()) ) {
00285                 continue;   
00286             }       
00287         }
00288         
00289         
00290         if (refresh_Timer.read_ms() >= refresh_Time) {
00291             refresh_Timer.reset();
00292             Get_GPS(&myGPS);
00293             
00294             printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
00295                     agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00296                     agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00297                     agz.get_agzCov_lati(),agz.get_agzCov_longi()
00298                     );
00299             
00300         }
00301     }
00302 }