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