main.cpp

00001 /**********************************************/
00002 //  
00003 //    
00004 //
00005 //  Program name: Aigamozu ROBOT
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 "Kalman.h"
00027 
00028 //************ID Number*****************
00029 //Robot   ID: 'A' ~ 'Z'
00030 //Base    ID: 'a' ~ 'a'
00031 //manager ID: '0'(数字のゼロ)
00032 //
00033 const char MyID = 'd';
00034 //************ID Number*****************
00035 
00036 /////////////////////////////////////////
00037 //
00038 //Pin Setting
00039 //
00040 /////////////////////////////////////////
00041 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
00042 
00043 
00044 /////////////////////////////////////////
00045 //
00046 //Connection Setting
00047 //
00048 /////////////////////////////////////////
00049 
00050 //Serial Connect Setting: PC <--> mbed
00051 Serial pc(USBTX, USBRX);    
00052 
00053 //Serial Connect Setting: GPS <--> mbed
00054 Serial * gps_Serial;
00055 
00056 //Serial Connect Setting: XBEE <--> mbed
00057 XBee xbee(p13,p14);
00058 ZBRxResponse zbRx = ZBRxResponse();
00059 
00060 //set up GPS module
00061 
00062 //set up AigamozuControlPackets library
00063 AigamozuControlPackets agz(agz_motorShield);
00064 
00065 
00066 /////////////////////////////////////////
00067 //
00068 //For Kalman data
00069 //
00070 /////////////////////////////////////////
00071 double sigmaGPS[2][2] = {{250,0},{0,250}};
00072 double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
00073 double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
00074 double y[2],x[2][2]={0};
00075 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS);
00076 
00077 /////////////////////////////////////////
00078 //
00079 //Address List
00080 //
00081 /////////////////////////////////////////
00082 XBeeAddress64 base_Address[BaseNumber] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 
00083                                  XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L),
00084                                  XBeeAddress64(BASE5_32H,BASE5_32L)};
00085 XBeeAddress64 robot_Address[RobotNumber] = {XBeeAddress64(ROBOT1_32H,ROBOT1_32L), XBeeAddress64(ROBOT1_32H,ROBOT1_32L)};
00086 XBeeAddress64 manager_Address = XBeeAddress64(BASE1_32H,BASE1_32L);
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,
00109                                     agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00110                                     agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00111                                     agz.get_agzCov_lati(),agz.get_agzCov_longi());
00112     
00113     //debug***************************************************
00114     printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
00115             agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00116             agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00117             agz.get_agzCov_lati(),agz.get_agzCov_longi()
00118             );
00119     for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
00120         printf("\n");
00121     //debug end***************************************************
00122     
00123     //Select Destination
00124     ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
00125     //Send -> Base
00126     xbee.send(tx64request);
00127 }
00128 
00129 /////////////////////////////////////////
00130 //
00131 //Get GPS function
00132 //
00133 /////////////////////////////////////////
00134 
00135 void Get_GPS(Adafruit_GPS *myGPS){
00136     static bool flag = true;
00137     
00138     if (myGPS->fix) {
00139         agz.nowStatus = GPS_AVAIL;
00140         
00141         if(flag){//初期値設定
00142             if(myGPS->latitudeH>=36 && myGPS->latitudeH<=38 && myGPS->longitudeH>=138 && myGPS->longitudeH<=141){
00143                 flag = false;
00144                 x[0][0]=(double)myGPS->latitudeL;                        
00145                 x[0][1]=(double)myGPS->longitudeL;   
00146             } 
00147         }
00148         
00149         if(myGPS->longitudeH<138 || myGPS->longitudeH>141 || myGPS->latitudeH<36 || myGPS->latitudeH>38){
00150             return;
00151         }
00152         //Kalman Filter
00153         Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS);
00154                 
00155         agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
00156         agz.reNewRobotPointKalman(myGPS->latitudeKH,myGPS->latitudeKL,myGPS->longitudeKH,myGPS->longitudeKL);
00157     }
00158     else agz.nowStatus = GPS_UNAVAIL;
00159     
00160 } 
00161   
00162 /////////////////////////////////////////
00163 //
00164 //Get Status
00165 //
00166 /////////////////////////////////////////
00167 void Get_Status(char SenderIDc,uint8_t *packetdata){
00168     
00169     //マネージャからデータが来たとき
00170     if(SenderIDc == '0'){
00171         printf("get manager Status\n");
00172     }
00173     //他のロボットからデータが来たとき
00174     if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
00175         printf("get other robots Status\n");
00176     }
00177     //基地局からデータが来たとき
00178     if(SenderIDc >= 'a' && SenderIDc <= 'z'){
00179         printf("Get Base data\n");
00180         int id = SenderIDc - 'a';
00181         agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]);
00182         agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
00183         
00184         //debug
00185         for(int i = 0;i < 4;i++){
00186         printf("BASE:%d\n",i);
00187         printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\n",
00188             agz.get_basePoint_lati(i),agz.get_basePoint_longi(i),
00189             agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i)
00190             );
00191         }
00192     }
00193 }
00194 
00195 /////////////////////////////////////////
00196 //
00197 //Send_Request_to_base
00198 //
00199 /////////////////////////////////////////
00200 void Send_Request_Base(int basenumber){
00201     printf("send\n");
00202     agz.createRequestCommand(MyID, basenumber);
00203     //Select Destination
00204     ZBTxRequest tx64request(base_Address[basenumber],agz.packetData,agz.getPacketLength());
00205     //Send -> Base
00206     xbee.send(tx64request);
00207 }
00208 
00209 
00210 /////////////////////////////////////////
00211 //
00212 //Kalman Processing
00213 //
00214 /////////////////////////////////////////
00215     
00216 void get_K(){
00217   double temp[2][2]={
00218     {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
00219     {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
00220   };
00221   double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
00222   K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
00223   K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
00224 }
00225 
00226 
00227 void get_x(){
00228   x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
00229   x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
00230 }
00231 
00232 
00233 void get_sigma(){
00234     double temp[2][2];
00235     for(int i=0;i<2;i++) {
00236         for(int j=0;j<2;j++) {
00237             for(int k=0;k<2;k++) {
00238                 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
00239             }
00240         }
00241    }
00242     for(int i = 0;i < 2;i++){
00243         for(int j = 0;j < 2;j++){
00244             sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
00245         }
00246     }   
00247 }
00248 
00249 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS){
00250     y[0] = Latitude;
00251     y[1] = Longitude;
00252     //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
00253     get_K();
00254     //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
00255     get_x();
00256     //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
00257     get_sigma();
00258     
00259     
00260     //kousinn
00261     for(int i = 0;i < 2;i++){
00262         for(int j = 0;j < 2;j++){
00263             K[0][i][j]=K[1][i][j];
00264             x[0][i]=x[1][i];
00265             sigma[0][i][j]=sigma[1][i][j];
00266         }
00267     }
00268             
00269     myGPS->latitudeKH=myGPS->latitudeH;//latitude after filtering
00270     myGPS->longitudeKH=myGPS->longitudeH;//longitude after filtering
00271     myGPS->latitudeKL=(long)x[1][0];//latitude after filtering
00272     myGPS->longitudeKL=(long)x[1][1];//longitude after filtering
00273     
00274     agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]);
00275 }
00276 
00277 
00278 /////////////////////////////////////////
00279 //
00280 //Main Processing
00281 //
00282 /////////////////////////////////////////
00283 int main() {
00284     //start up time
00285     wait(3);
00286     //set pc frequency to 57600bps 
00287     pc.baud(PC_BAUD_RATE); 
00288     //set xbee frequency to 57600bps
00289     xbee.begin(XBEE_BAUD_RATE);    
00290         
00291 
00292     //GPS setting
00293     gps_Serial = new Serial(p28,p27);
00294     Adafruit_GPS myGPS(gps_Serial); 
00295     Timer refresh_Timer;
00296     const int refresh_Time = 2000; //refresh time in ms
00297     Timer collect_Timer;
00298     const int collect_Time = 2000; //refresh time in ms
00299     myGPS.begin(GPS_BAUD_RATE); 
00300     
00301     char SenderIDc;
00302     int basenumber=0;
00303     //GPS Send Command
00304     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
00305     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
00306     myGPS.sendCommand(PGCMD_ANTENNA);
00307     
00308     wait(2);
00309        
00310     //interrupt start
00311     refresh_Timer.start();
00312     collect_Timer.start();
00313     
00314     printf("start\n");
00315     
00316     while (true) {
00317         
00318         //Check Xbee Buffer Available
00319         xbee.readPacket();
00320             
00321         if (xbee.getResponse().isAvailable()) {
00322             xbee.getResponse().getZBRxResponse(zbRx);
00323             uint8_t *buf = zbRx.getFrameData();
00324                 
00325             if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
00326                 xbee.getResponse().getZBRxResponse(zbRx);
00327                 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
00328                 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
00329                 SenderIDc = buf1[5];//送信元のIDを取得する
00330                 char Command_type =agz.checkCommnadType(buf);//コマンドタイプを取得する
00331                  
00332                 //Check Command Type 
00333                 switch(Command_type){
00334                     //Get Request command
00335                     case STATUS_REQUEST:{
00336                         Send_Status(SenderIDc);                     
00337                         break;          
00338                     }
00339                     case RECEIVE_STATUS:{
00340                         Get_Status(SenderIDc,buf1);
00341                         break;
00342                     }
00343                     default:{
00344                         break;
00345                     }
00346                 }//endswitch
00347             }//endifZB_RX_RESPONSE
00348         }//endifisAvailable
00349                        
00350         myGPS.read();
00351         //recive gps module
00352         //check if we recieved a new message from GPS, if so, attempt to parse it,
00353         if ( myGPS.newNMEAreceived() ) {
00354             if ( !myGPS.parse(myGPS.lastNMEA()) ) {
00355                 continue;   
00356             }       
00357         }
00358         //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
00359         if (refresh_Timer.read_ms() >= refresh_Time) {
00360             refresh_Timer.reset();
00361             Get_GPS(&myGPS);
00362 
00363         }
00364         
00365         //一定時間ごとに基地局のGPSデータを取得し、AigamozuControlPacketsないのagzBasePointとagzBasePointKalmanに格納する
00366         if (collect_Timer.read_ms() >= collect_Time) {
00367             collect_Timer.reset();
00368             Send_Request_Base(basenumber);
00369             basenumber++;
00370             if(basenumber > 4)basenumber=0;
00371         }
00372     }
00373 }