main.cpp

00001 /**********************************************/
00002 //  
00003 //    
00004 //
00005 //  Program name: Aigamozu Robot Control
00006 //  Author: Atsunori Maruyama
00007 //  Ver ->  1.3
00008 //  Day ->  2014/06/09
00009 //  
00010 //
00011 /**********************************************/
00012 
00013 #include "mbed.h"
00014 #include "XBee.h"
00015 #include "MBed_Adafruit_GPS.h"
00016 #include "AigamozuControlPackets.h"
00017 #include "agzIDLIST.h"
00018 #include "aigamozuSetting.h"
00019 #include "agz_common.h"
00020 #include "Kalman.h"
00021 
00022 /////////////////////////////////////////
00023 //
00024 //Connection Setting
00025 //
00026 /////////////////////////////////////////
00027 
00028 //Serial Connect Setting: PC <--> mbed
00029 Serial pc(USBTX, USBRX);    
00030 
00031 //Serial Connect Setting: GPS <--> mbed
00032 Serial * gps_Serial;
00033 
00034 //Serial Connect Setting: XBEE <--> mbed
00035 XBee xbee(p13,p14);
00036 ZBRxResponse zbRx = ZBRxResponse();
00037 XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L);
00038 
00039 AGZ_ROBOT robot[4];
00040 
00041 /////////////////////////////////////////
00042 //
00043 //Pin Setting
00044 //
00045 /////////////////////////////////////////
00046 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
00047 
00048 
00049 
00050 /////////////////////////////////////////
00051 //
00052 //Kalman Processing
00053 //
00054 /////////////////////////////////////////
00055 
00056 
00057 double sigmaGPS[2][2] = {{250,0},{0,250}};
00058 double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
00059 double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
00060 double y[2],x[2][2]={0};
00061 
00062     
00063 void get_K(){
00064   double temp[2][2]={
00065     {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
00066     {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
00067   };
00068   double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
00069   K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
00070   K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
00071 }
00072 
00073 
00074 void get_x(){
00075   x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
00076   x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
00077 }
00078 
00079 
00080 void get_sigma(){
00081     double temp[2][2];
00082     for(int i=0;i<2;i++) {
00083         for(int j=0;j<2;j++) {
00084             for(int k=0;k<2;k++) {
00085                 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
00086             }
00087         }
00088    }
00089     for(int i = 0;i < 2;i++){
00090         for(int j = 0;j < 2;j++){
00091             sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
00092         }
00093     }   
00094 }
00095 
00096 void Kalman(double Latitude,double Longitude){
00097     y[0] = Latitude;
00098     y[1] = Longitude;
00099     //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
00100     get_K();
00101     //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
00102     get_x();
00103     //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
00104     get_sigma();
00105 }
00106 
00107 
00108 /////////////////////////////////////////
00109 //
00110 //Main Processing
00111 //
00112 /////////////////////////////////////////
00113 int main() {
00114     //start up time
00115     wait(3);
00116     //set pc frequency to 57600bps 
00117     pc.baud(PC_BAUD_RATE); 
00118     //set xbee frequency to 57600bps
00119     xbee.begin(XBEE_BAUD_RATE);    
00120     
00121     //GPS setting
00122     gps_Serial = new Serial(p28,p27);
00123     Adafruit_GPS myGPS(gps_Serial); 
00124     Timer refresh_Timer;
00125     const int refresh_Time = 2000; //refresh time in ms
00126     myGPS.begin(GPS_BAUD_RATE); 
00127     
00128     Timer collect_Timer;
00129     const int collect_Time = 200; //when we collect 4 GPS point, we use 
00130     int collect_flag = 0;
00131     char collect_state = 'a';
00132     XBeeAddress64 collect_Address[4] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 
00133                                         XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L)};
00134     XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
00135     int id;
00136     bool flag = true;
00137     
00138     //GPS Send Command
00139     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
00140     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
00141     myGPS.sendCommand(PGCMD_ANTENNA);
00142     
00143     wait(2);
00144        
00145     //interrupt start
00146     AigamozuControlPackets agz(agz_motorShield);
00147     refresh_Timer.start();
00148     
00149     printf("test\n");
00150         
00151     
00152     while (true) {
00153         
00154         //Check Xbee Buffer Available
00155         xbee.readPacket();
00156             if (xbee.getResponse().isAvailable()) {
00157                 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
00158                 xbee.getResponse().getZBRxResponse(zbRx);
00159                 uint8_t *buf = zbRx.getFrameData();
00160                 uint8_t *buf1 = &buf[11]; 
00161                  
00162                  
00163                  //Check Command Type 
00164                  switch(agz.checkCommnadType(buf)){
00165                     
00166                     //CommandType -> ChanegeMode
00167                     case CHANGE_MODE :{
00168                         agz.changeMode(buf);                                        
00169                         break;
00170                         }
00171 
00172                     //CommandType -> Manual
00173                     case MANUAL:{  
00174                         //Check now Mode
00175                         if(agz.nowMode == MANUAL_MODE){ 
00176                             agz.changeSpeed(buf);
00177                             }
00178                         break;
00179                         }
00180                     
00181                     //CommandType -> Send Status
00182                     case STATUS_REQUEST:{
00183                             //send normal data
00184                             //Create GPS Infomation Packet
00185                             agz.createReceiveStatusCommand('A','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
00186                             //Select Destination
00187                             ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength());
00188                             //Send -> Base
00189                             xbee.send(tx64request);
00190                             
00191                             //send Kalman data
00192                             agz.createReceiveStatusCommandwithKalman('A','a',myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
00193                             //Select Destination
00194                             ZBTxRequest tx64request1(robot_Address,agz.packetData,agz.getPacketLength());
00195                             //Send -> Base
00196                             xbee.send(tx64request);
00197                             break;         
00198           
00199                         }
00200                         
00201                     case RECEIVE_STATUS:{
00202                             
00203                         //printf("Receive Status\n");      
00204                            
00205                         id = buf1[5] - 'A';
00206                         robot[id].set_state(buf1[9]);
00207                         robot[id].set_LatitudeH(&buf1[13]);
00208                         robot[id].set_LatitudeL(&buf1[17]);
00209                         robot[id].set_LongitudeH(&buf1[21]);
00210                         robot[id].set_LongitudeL(&buf1[25]);
00211                         
00212                         agz.reNewBasePoint(id,robot[id].get_LatitudeH(),robot[id].get_LatitudeL(),robot[id].get_LongitudeH(),robot[id].get_LongitudeL());
00213                     /*   
00214                         printf("%d,", buf1[5]);                    
00215                         printf(" %ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL());
00216                     */
00217                         break;
00218                     }
00219                     case RECEIVE_KALMAN:{
00220                         id = buf1[5] - 'A';
00221                         robot[id].set_state(buf1[9]);
00222                         robot[id].set_LatitudeKH(&buf1[13]);
00223                         robot[id].set_LatitudeKL(&buf1[17]);
00224                         robot[id].set_LongitudeKH(&buf1[21]);
00225                         robot[id].set_LongitudeKL(&buf1[25]);
00226                         
00227                         agz.reNewBasePointKalman(id,robot[id].get_LatitudeKH(),robot[id].get_LatitudeKL(),robot[id].get_LongitudeKH(),robot[id].get_LongitudeKL());
00228  
00229                         break;    
00230                     }
00231                     default:{
00232                         break;
00233                     }
00234                 }
00235                     
00236             
00237                 }
00238             }
00239 
00240             
00241         myGPS.read();
00242         //recive gps module
00243         //check if we recieved a new message from GPS, if so, attempt to parse it,
00244         if ( myGPS.newNMEAreceived() ) {
00245             if ( !myGPS.parse(myGPS.lastNMEA()) ) {
00246                 continue;   
00247             }       
00248         }
00249         if (refresh_Timer.read_ms() >= refresh_Time) {
00250             refresh_Timer.reset();
00251             if (myGPS.fix) {
00252                 agz.nowStatus = GPS_AVAIL;
00253                 
00254                 if(flag){
00255                     if(myGPS.longitudeH==139 && myGPS.latitudeH==37){
00256                         flag = false;
00257                         x[0][0]=(double)myGPS.longitudeL;                        
00258                         x[0][1]=(double)myGPS.latitudeL;   
00259                     } 
00260                 }
00261                 if(myGPS.longitudeH!=139 || myGPS.latitudeH!=37) continue;
00262                 
00263                 //Kalman Filter
00264                 Kalman(myGPS.longitudeL,myGPS.latitudeL);
00265                 //kousinn
00266                 for(int i = 0;i < 2;i++){
00267                     for(int j = 0;j < 2;j++){
00268                         K[0][i][j]=K[1][i][j];
00269                         x[0][i]=x[1][i];
00270                         sigma[0][i][j]=sigma[1][i][j];
00271                     }
00272                 }
00273                 
00274                 myGPS.longitudeKH=myGPS.longitudeH;//longitude after filtering
00275                 myGPS.latitudeKH=myGPS.latitudeH;//latitude after filtering
00276                 myGPS.longitudeKL=(long)x[1][0];//longitude after filtering
00277                 myGPS.latitudeKL=(long)x[1][1];//latitude after filtering
00278                 
00279                 agz.reNewRobotPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
00280                 agz.reNewRobotPointKalman(myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
00281                 //printf("state = %d\n",agz.nowMode);
00282                 //printf("my %ld, %ld, %ld, %ld\n", myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
00283             }
00284             else agz.nowStatus = GPS_UNAVAIL;
00285             
00286         } 
00287         
00288         
00289         //get base GPS
00290         if( collect_Timer.read_ms() >= collect_Time){
00291             collect_Timer.reset();
00292             
00293             //printf("Send Request:%d,%d\n",collect_flag,collect_state);
00294                       
00295             agz.createRequestCommand('A', collect_state);
00296             //Select Destination
00297             ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength());
00298             //Send -> Base
00299             xbee.send(tx64request);
00300             
00301             collect_flag++;
00302             collect_state++;
00303 
00304             if(collect_flag == 4){
00305                 collect_state = 'a';
00306                 collect_flag = 0;
00307             }    
00308          } 
00309     } 
00310 }