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 //  中澤遥菜
00014 //  変更点：メイン関数の中の処理を関数化しました。
00015 //  setting(), interrupt_start(VNH5019 motorShield),
00016 //  GPS_Setting(int refresh_Time, int collect_Time),
00017 //  GPS_SendCommand(), check_CommandType(utf8_t command_type),
00018 //  LatLong_afterFiltering(double longitude, double latitude, double x0, double x1),
00019 //  send_GPSdata(), get_GPSdata(), getKalman()
00020 //
00021 /**********************************************/
00022 
00023 #include "mbed.h"
00024 #include "XBee.h"
00025 #include "MBed_Adafruit_GPS.h"
00026 #include "AigamozuControlPackets.h"
00027 #include "agzIDLIST.h"
00028 #include "aigamozuSetting.h"
00029 #include "agz_common.h"
00030 #include "Kalman.h"
00031 
00032 /////////////////////////////////////////
00033 //
00034 //Connection Setting
00035 //
00036 /////////////////////////////////////////
00037 
00038 //Serial Connect Setting: PC <--> mbed
00039 Serial pc(USBTX, USBRX);    
00040 
00041 //Serial Connect Setting: GPS <--> mbed
00042 Serial * gps_Serial;
00043 
00044 //Serial Connect Setting: XBEE <--> mbed
00045 XBee xbee(p13,p14);
00046 ZBRxResponse zbRx = ZBRxResponse();
00047 XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L);
00048 
00049 AGZ_ROBOT robot[5];
00050 
00051 char Base_ID = 'A';
00052 
00053 /////////////////////////////////////////
00054 //
00055 //Pin Setting
00056 //
00057 /////////////////////////////////////////
00058 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
00059 
00060 
00061 
00062 /////////////////////////////////////////
00063 //
00064 //Kalman Processing
00065 //
00066 /////////////////////////////////////////
00067 
00068 
00069 double sigmaGPS[2][2] = {{250,0},{0,250}};
00070 double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
00071 double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
00072 double y[2],x[2][2]={0};
00073 
00074     
00075 void get_K(){
00076   double temp[2][2]={
00077     {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
00078     {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
00079   };
00080   double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
00081   K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
00082   K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
00083 }
00084 
00085 
00086 void get_x(){
00087   x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
00088   x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
00089 }
00090 
00091 
00092 void get_sigma(){
00093     double temp[2][2];
00094     for(int i=0;i<2;i++) {
00095         for(int j=0;j<2;j++) {
00096             for(int k=0;k<2;k++) {
00097                 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
00098             }
00099         }
00100    }
00101     for(int i = 0;i < 2;i++){
00102         for(int j = 0;j < 2;j++){
00103             sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
00104         }
00105     }   
00106 }
00107 
00108 void Kalman(double Latitude,double Longitude){
00109     y[0] = Latitude;
00110     y[1] = Longitude;
00111     //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
00112     get_K();
00113     //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
00114     get_x();
00115     //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
00116     get_sigma();
00117 }
00118 
00119 /////////////////////////////////////////
00120 //
00121 //setting
00122 //
00123 /////////////////////////////////////////
00124 void setting(){
00125      //start up time
00126     wait(3);
00127     //set pc frequency to 57600bps 
00128     pc.baud(PC_BAUD_RATE); 
00129     //set xbee frequency to 57600bps
00130     xbee.begin(XBEE_BAUD_RATE);    
00131     }
00132 
00133 /////////////////////////////////////////
00134 //
00135 //interrupt start
00136 //
00137 /////////////////////////////////////////
00138 void interrupt_start(VNH5019 motorShield, Timer refresh){
00139     AigamozuControlPackets agz(motorShield);
00140     refresh.start();
00141     
00142     printf("start\n");
00143 }
00144 
00145 /////////////////////////////////////////
00146 //
00147 //GPS Setting
00148 //
00149 /////////////////////////////////////////
00150 XBeeAddress64 collect_Address[5];
00151 XBeeAddress64 robot_Address
00152 
00153 void GPS_Setting(Timer refresh_Timer, Timer collect_Timer){
00154     gps_Serial = new Serial(p28,p27);
00155     Adafruit_GPS myGPS(gps_Serial); 
00156     Timer refresh_Timer;
00157     const int refresh_Time = refresh_Timer; //refresh time in ms
00158     myGPS.begin(GPS_BAUD_RATE); 
00159     
00160     Timer collect_Timer;
00161     const int collect_Time = collect_Timer; //when we collect 4 GPS point, we use 
00162     collect_Address[5] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 
00163                                         XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L), XBeeAddress64(BASE5_32H,BASE5_32L)};
00164     robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
00165     }
00166 
00167 /////////////////////////////////////////
00168 //
00169 //GPS Send Command
00170 //
00171 /////////////////////////////////////////
00172 void GPS_SendCommand(){
00173     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
00174     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
00175     myGPS.sendCommand(PGCMD_ANTENNA);
00176     }
00177     
00178 /////////////////////////////////////////
00179 //
00180 //Check Command Type
00181 //
00182 /////////////////////////////////////////
00183 void check_CommandType(utf8_t command_type, AigamozuControlPackets ai(motorShield), ){
00184     switch(command_type){
00185                     
00186                     //CommandType -> ChanegeMode
00187                     case CHANGE_MODE :{
00188                         agz.changeMode(buf);                                        
00189                         break;
00190                         }
00191 
00192                     //CommandType -> Manual
00193                     case MANUAL:{  
00194                         //Check now Mode
00195                         if(agz.nowMode == MANUAL_MODE){ 
00196                             agz.changeSpeed(buf);
00197                             }
00198                         break;
00199                         }
00200                     
00201                     //CommandType -> Send Status
00202                     case STATUS_REQUEST:{
00203                            
00204                             //send normal data
00205                             //Create GPS Infomation Packet
00206                             agz.createReceiveStatusCommand(Base_ID,'a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
00207                             //Select Destination
00208                             ZBTxRequest tx64request2(collect_Address[id],ai.packetData,ai.getPacketLength());
00209                             //Send -> Base
00210                             xbee.send(tx64request);
00211                             
00212                             //send Kalman data
00213                             agz.createReceiveStatusCommandwithKalman(Base_ID,'a',myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
00214                             //Select Destination
00215                             ZBTxRequest tx64request3(collect_Address[id],agz.packetData,agz.getPacketLength());
00216                             //Send -> Base
00217                             xbee.send(tx64request);
00218                             break;         
00219           
00220                         }
00221 
00222                     default:{
00223                         break;
00224                     }
00225                 }
00226             }
00227 
00228 /////////////////////////////////////////
00229 //
00230 //latitude and longitude after filtering
00231 //
00232 /////////////////////////////////////////
00233 void LatLong_afterFiltering(double longitude, double latitude, double x0, double x1){
00234     myGPS.longitudeKH=longitude;//longitude after filtering
00235     myGPS.latitudeKH=latitude;//latitude after filtering
00236     myGPS.longitudeKL=(long)x0;//longitude after filtering
00237     myGPS.latitudeKL=(long)x1;//latitude after filtering
00238 }
00239 
00240 /////////////////////////////////////////
00241 //
00242 //send_GPSdata()
00243 //
00244 /////////////////////////////////////////
00245 void send_GPSdata(){
00246     if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
00247         xbee.getResponse().getZBRxResponse(zbRx);
00248         uint8_t *buf = zbRx.getFrameData();
00249         uint8_t *buf1 = &buf[11]; 
00250         id = buf1[5] - 'A';+
00251                            
00252         //Check Command Type 
00253         check_CommandType(agz.checkCommnadType(buf));
00254             
00255     }
00256 }
00257 
00258 /////////////////////////////////////////
00259 //
00260 //get_GPSdata
00261 //
00262 /////////////////////////////////////////
00263 void get_GPSdata(){
00264     if(flag){
00265         if(myGPS.longitudeH==139 && myGPS.latitudeH==37){
00266         flag = false;
00267         x[0][0]=(double)myGPS.longitudeL;                        
00268         x[0][1]=(double)myGPS.latitudeL;   
00269         } 
00270     }
00271     if(myGPS.longitudeH!=139 || myGPS.latitudeH!=37) continue;
00272 }
00273 
00274 /////////////////////////////////////////
00275 //
00276 //get_Kalman
00277 //
00278 /////////////////////////////////////////
00279 void get_Kalman(){
00280     //Kalman Filter
00281     Kalman(myGPS.longitudeL,myGPS.latitudeL);
00282     //kousinn
00283     for(int i = 0;i < 2;i++){
00284         for(int j = 0;j < 2;j++){
00285             K[0][i][j]=K[1][i][j];
00286             x[0][i]=x[1][i];
00287             sigma[0][i][j]=sigma[1][i][j];
00288         }
00289     }
00290                 
00291     LatLong_afterFiltering(myGPS.longitudeH, myGPS.latitudeH, x[1][0], x[1][1]);
00292     }
00293 /////////////////////////////////////////
00294 //
00295 //Main Processing
00296 //
00297 /////////////////////////////////////////
00298 int main() {
00299     setting();
00300     
00301     //GPS setting
00302     GPS_Setting(2000, 200);
00303     int collect_flag = 0;
00304     char collect_state = 'a';
00305     int id;
00306     bool flag = true;
00307     
00308     //GPS Send Command
00309     GPS_SendCommand();
00310     
00311     wait(2);
00312        
00313     //interrupt start
00314     interrupt_start(agz_motorShield, refreshTime);
00315     
00316     while (true) {
00317         
00318         //Check Xbee Buffer Available
00319         xbee.readPacket();
00320             if (xbee.getResponse().isAvailable()) {
00321                 send_GPSdata();
00322             }
00323            
00324         myGPS.read();
00325         //recive gps module
00326         //check if we recieved a new message from GPS, if so, attempt to parse it,
00327         if ( myGPS.newNMEAreceived() ) {
00328             if ( !myGPS.parse(myGPS.lastNMEA()) ) {
00329                 continue;   
00330             }       
00331         }
00332         if (refresh_Timer.read_ms() >= refresh_Time) {
00333             refresh_Timer.reset();
00334             if (myGPS.fix) {
00335                 agz.nowStatus = GPS_AVAIL;
00336                 
00337                 get_GPSdata();
00338                 
00339                 get_Kalman();
00340                 
00341                 agz.reNewRobotPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
00342                 agz.reNewRobotPointKalman(myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
00343                 //printf("state = %d\n",agz.nowMode);
00344                 //printf("my %ld, %ld, %ld, %ld\n", myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
00345             }
00346             else agz.nowStatus = GPS_UNAVAIL;
00347             
00348         } 
00349         
00350     
00351     } 
00352 }