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 //Plus Speed
00091 //
00092 //MNUAL_MODEの時にスピードを変える
00093 /////////////////////////////////////////
00094 void Plus_Speed(uint8_t *packetdata){
00095        
00096     if(agz.nowMode == MANUAL_MODE){ 
00097         agz.changeSpeed(packetdata);
00098     }
00099     
00100 }
00101 
00102 /////////////////////////////////////////
00103 //
00104 //Send Status
00105 //
00106 //リクエストがきたとき、自分の位置情報などを返信する
00107 /////////////////////////////////////////
00108 void Send_Status(char SenderIDc){
00109     XBeeAddress64 send_Address;
00110     if(SenderIDc == '0'){
00111         send_Address = manager_Address;
00112     }
00113     if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
00114         send_Address = robot_Address[SenderIDc - 'A'];
00115     }
00116     if(SenderIDc >= 'a' && SenderIDc <= 'z'){
00117         send_Address = base_Address[SenderIDc - 'a'];
00118     }
00119     //send normal data
00120     //Create GPS Infomation Packet
00121     agz.createReceiveStatusCommand(MyID,SenderIDc,
00122                                     agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00123                                     agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00124                                     agz.get_agzCov_lati(),agz.get_agzCov_longi());
00125     
00126     //debug***************************************************
00127     printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
00128             agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00129             agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00130             agz.get_agzCov_lati(),agz.get_agzCov_longi()
00131             );
00132     for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
00133         printf("\n");
00134     //debug end***************************************************
00135     
00136     //Select Destination
00137     ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
00138     //Send -> Base
00139     xbee.send(tx64request);
00140 }
00141 
00142 /////////////////////////////////////////
00143 //
00144 //Get GPS function
00145 //
00146 /////////////////////////////////////////
00147 
00148 void Get_GPS(Adafruit_GPS *myGPS){
00149     static bool flag = true;
00150     
00151     if (myGPS->fix) {
00152         agz.nowStatus = GPS_AVAIL;
00153         
00154         if(flag){//初期値設定
00155             if(myGPS->latitudeH>=36 && myGPS->latitudeH<=38 && myGPS->longitudeH>=138 && myGPS->longitudeH<=141){
00156                 flag = false;
00157                 x[0][0]=(double)myGPS->latitudeL;                        
00158                 x[0][1]=(double)myGPS->longitudeL;   
00159             } 
00160         }
00161         
00162         if(myGPS->longitudeH<138 || myGPS->longitudeH>141 || myGPS->latitudeH<36 || myGPS->latitudeH>38){
00163             return;
00164         }
00165         //Kalman Filter
00166         Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS);
00167                 
00168         agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
00169         agz.reNewRobotPointKalman(myGPS->latitudeKH,myGPS->latitudeKL,myGPS->longitudeKH,myGPS->longitudeKL);
00170     }
00171     else agz.nowStatus = GPS_UNAVAIL;
00172     
00173 } 
00174 
00175 /////////////////////////////////////////
00176 //
00177 //New Mode
00178 //
00179 /////////////////////////////////////////
00180 
00181 void New_Mode(uint8_t *packetdata){
00182   
00183   agz.changeMode(packetdata); 
00184   
00185 }
00186   
00187 /////////////////////////////////////////
00188 //
00189 //Get Status
00190 //
00191 /////////////////////////////////////////
00192 void Get_Status(char SenderIDc,uint8_t *packetdata){
00193     
00194     //マネージャからデータが来たとき
00195     if(SenderIDc == '0'){
00196         printf("get manager Status\n");
00197     }
00198     //他のロボットからデータが来たとき
00199     if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
00200         printf("get other robots Status\n");
00201     }
00202     //基地局からデータが来たとき
00203     if(SenderIDc >= 'a' && SenderIDc <= 'z'){
00204         printf("Get Base data\n");
00205         int id = SenderIDc - 'a';
00206         agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]);
00207         agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
00208         
00209         //debug
00210         for(int i = 0;i < 4;i++){
00211         printf("BASE:%d\n",i);
00212         printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\n",
00213             agz.get_basePoint_lati(i),agz.get_basePoint_longi(i),
00214             agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i)
00215             );
00216         }
00217     }
00218 }
00219 
00220 void Get_Status_Kalman(char SenderIDc,uint8_t *packetdata){
00221     
00222     //マネージャからデータが来たとき
00223     if(SenderIDc == '0'){
00224         printf("get manager Status Kalman\n");
00225     }
00226     //他のロボットからデータが来たとき
00227     if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
00228         printf("get other robots Status Kalman\n");
00229     }
00230     //基地局からデータが来たとき
00231     if(SenderIDc >= 'a' && SenderIDc <= 'z'){
00232         printf("Get Base data Kalman\n");
00233         int id = SenderIDc - 'a';
00234         agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
00235         
00236         //debug
00237         for(int i = 0;i < 4;i++){
00238         printf("BASE:%d\n",i);
00239         printf("latitudeK:%f,longitudeK:%f\n",
00240             agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i));
00241         }
00242     }
00243 }
00244 
00245 /////////////////////////////////////////
00246 //
00247 //Send_Request_to_base
00248 //
00249 /////////////////////////////////////////
00250 void Send_Request_Base(int basenumber){
00251     printf("send\n");
00252     agz.createRequestCommand(MyID, basenumber);
00253     //Select Destination
00254     ZBTxRequest tx64request(base_Address[basenumber],agz.packetData,agz.getPacketLength());
00255     //Send -> Base
00256     xbee.send(tx64request);
00257 }
00258 
00259 /////////////////////////////////////////
00260 //
00261 //auto_Move
00262 //
00263 //InAreaかOutAreaの判定
00264 //Kalmanを通した値を出力(Baseと自分)
00265 /////////////////////////////////////////
00266 
00267 void auto_Move(){
00268  
00269  bool result;
00270  int i;
00271  
00272  result = agz.gpsAuto();
00273  agz.set_agzAutoGPS();
00274  agz.set_agzKalmanGPS();
00275  
00276  if(result == true){
00277         printf("Out Area\n");
00278  }
00279 else if(result == false){
00280         printf("In Area\n");
00281 }  
00282 for(i = 0; i < 4; i++){
00283     printf("%d: %f, %f\n", i, agz.get_basePointKalman_lati(i), agz.get_basePointKalman_longi(i));
00284  }
00285  printf("robot: %f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
00286  
00287 }
00288 
00289 
00290 /////////////////////////////////////////
00291 //
00292 //Kalman Processing
00293 //
00294 /////////////////////////////////////////
00295     
00296 void get_K(){
00297   double temp[2][2]={
00298     {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
00299     {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
00300   };
00301   double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
00302   K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
00303   K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
00304 }
00305 
00306 
00307 void get_x(){
00308   x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
00309   x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
00310 }
00311 
00312 
00313 void get_sigma(){
00314     double temp[2][2];
00315     for(int i=0;i<2;i++) {
00316         for(int j=0;j<2;j++) {
00317             for(int k=0;k<2;k++) {
00318                 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
00319             }
00320         }
00321    }
00322     for(int i = 0;i < 2;i++){
00323         for(int j = 0;j < 2;j++){
00324             sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
00325         }
00326     }   
00327 }
00328 
00329 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS){
00330     y[0] = Latitude;
00331     y[1] = Longitude;
00332     //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
00333     get_K();
00334     //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
00335     get_x();
00336     //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
00337     get_sigma();
00338     
00339     
00340     //kousinn
00341     for(int i = 0;i < 2;i++){
00342         for(int j = 0;j < 2;j++){
00343             K[0][i][j]=K[1][i][j];
00344             x[0][i]=x[1][i];
00345             sigma[0][i][j]=sigma[1][i][j];
00346         }
00347     }
00348             
00349     myGPS->latitudeKH=myGPS->latitudeH;//latitude after filtering
00350     myGPS->longitudeKH=myGPS->longitudeH;//longitude after filtering
00351     myGPS->latitudeKL=(long)x[1][0];//latitude after filtering
00352     myGPS->longitudeKL=(long)x[1][1];//longitude after filtering
00353     
00354     agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]);
00355 }
00356 
00357 
00358 /////////////////////////////////////////
00359 //
00360 //Main Processing
00361 //
00362 /////////////////////////////////////////
00363 int main() {
00364     //start up time
00365     wait(3);
00366     //set pc frequency to 57600bps 
00367     pc.baud(PC_BAUD_RATE); 
00368     //set xbee frequency to 57600bps
00369     xbee.begin(XBEE_BAUD_RATE);    
00370         
00371 
00372     //GPS setting
00373     gps_Serial = new Serial(p28,p27);
00374     Adafruit_GPS myGPS(gps_Serial); 
00375     Timer refresh_Timer;
00376     const int refresh_Time = 2000; //refresh time in ms
00377     Timer auto_Timer;
00378     const int auto_Time = 2000; //refresh time in ms
00379     myGPS.begin(GPS_BAUD_RATE); 
00380     
00381     char SenderIDc;
00382     //GPS Send Command
00383     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
00384     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
00385     myGPS.sendCommand(PGCMD_ANTENNA);
00386     
00387     wait(2);
00388        
00389     //interrupt start
00390     refresh_Timer.start();
00391     auto_Timer.start();
00392     
00393     printf("start\n");
00394     
00395     while (true) {
00396         
00397         //Check Xbee Buffer Available
00398         xbee.readPacket();
00399             
00400         if (xbee.getResponse().isAvailable()) {
00401             xbee.getResponse().getZBRxResponse(zbRx);
00402             uint8_t *buf = zbRx.getFrameData();
00403                 
00404             if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
00405                 xbee.getResponse().getZBRxResponse(zbRx);
00406                 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
00407                 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
00408                 SenderIDc = buf1[5];//送信元のIDを取得する
00409                 char Command_type =agz.checkCommnadType(buf);//コマンドタイプを取得する
00410                  
00411                 //Check Command Type 
00412                 switch(Command_type){
00413                     //Get Request command
00414                     case MANUAL:{
00415                         Plus_Speed(buf1);
00416                         break;
00417                     }
00418                     case STATUS_REQUEST:{
00419                         Send_Status(SenderIDc);                     
00420                         break;          
00421                     }
00422                     case CHANGE_MODE:{
00423                         New_Mode(buf1);
00424                         break;
00425                     }
00426                     case RECEIVE_STATUS:{
00427                         Get_Status(SenderIDc,buf1);
00428                         break;
00429                     }
00430                     case RECEIVE_KALMAN:{
00431                         Get_Status_Kalman(SenderIDc, buf1);
00432                         break;
00433                     }
00434                     default:{
00435                         break;
00436                     }
00437                 }//endswitch
00438             }//endifZB_RX_RESPONSE
00439         }//endifisAvailable
00440                        
00441         myGPS.read();
00442         //recive gps module
00443         //check if we recieved a new message from GPS, if so, attempt to parse it,
00444         if ( myGPS.newNMEAreceived() ) {
00445             if ( !myGPS.parse(myGPS.lastNMEA()) ) {
00446                 continue;   
00447             }       
00448         }
00449         //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
00450         if (refresh_Timer.read_ms() >= refresh_Time) {
00451             refresh_Timer.reset();
00452             Get_GPS(&myGPS);
00453 
00454         }
00455                 
00456         if(agz.nowMode == AUTO_MODE && auto_Timer.read_ms() >= auto_Time){
00457             auto_Timer.reset();
00458             auto_Move(); 
00459         }
00460     }
00461 }