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