main.cpp

00001 /**********************************************/
00002 //  
00003 //    
00004 //
00005 //  Program name: Aigamozu BASE
00006 //  Author: Mineta Kizuku
00007 //  
00008 //
00009 /**********************************************/
00010 
00011 /**********************************************/
00012 //更新情報
00013 //2015/05/11
00014 //ベースプログラムの作成
00015 //
00016 //2015/05/13
00017 //カルマンフィルタの共分散の値を0.0001以下にならないようにした
00018 //共分散の値を10進数に変換するようにした
00019 //
00020 //2015/05/17
00021 //Get_GPS()の中身longitudeの範囲138〜140に変更
00022 //
00023 //2015/05/24
00024 //Kalmanフィルターを十進数で計算するようにした。
00025 //Kalmanフィルターの計算式を変更した。
00026 //set_kalmanを追加した。
00027 /**********************************************/
00028 
00029 #include "mbed.h"
00030 #include "XBee.h"
00031 #include "MBed_Adafruit_GPS.h"
00032 #include "AigamozuControlPackets.h"
00033 #include "agzIDLIST.h"
00034 #include "aigamozuSetting.h"
00035 #include "agz_common.h"
00036 #include "Kalman.h"
00037 
00038 
00039 #define SIGMA_MIN 0.0001
00040 
00041 //************ID Number*****************
00042 const char MyID = 'd';
00043 //************ID Number*****************
00044 
00045 /////////////////////////////////////////
00046 //
00047 //Pin Setting
00048 //
00049 /////////////////////////////////////////
00050 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
00051 
00052 
00053 /////////////////////////////////////////
00054 //
00055 //Connection Setting
00056 //
00057 /////////////////////////////////////////
00058 
00059 //Serial Connect Setting: PC <--> mbed
00060 Serial pc(USBTX, USBRX);    
00061 
00062 //Serial Connect Setting: GPS <--> mbed
00063 Serial * gps_Serial;
00064 
00065 //Serial Connect Setting: XBEE <--> mbed
00066 XBee xbee(p13,p14);
00067 ZBRxResponse zbRx = ZBRxResponse();
00068 
00069 //set up GPS module
00070 
00071 //set up AigamozuControlPackets library
00072 AigamozuControlPackets agz(agz_motorShield);
00073 
00074 
00075 /////////////////////////////////////////
00076 //
00077 //For Kalman data
00078 //
00079 /////////////////////////////////////////
00080 #define FIRST_S2 0.000001
00081 #define COUNTER_MAX 10000
00082 #define ERROR_RANGE 0.001
00083 double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
00084 double s2x_cur=FIRST_S2,s2x_prev=FIRST_S2,s2y_cur=FIRST_S2,s2y_prev=FIRST_S2;//緯度経度のの時刻tと時刻t-1での共分散
00085 double s2_R=FIRST_S2;//GPSセンサの分散
00086 double Kx=0,Ky=0;//カルマンゲイン
00087 double zx,zy;//観測値
00088 void Kalman(double Latitude,double Longitude);
00089 int change = 0;
00090 
00091 /*
00092 LocalFileSystem local("local");  // マウントポイントを定義（ディレクトリパスになる）
00093 FILE *fp;
00094 char filename[16] = "/local/out0.txt";
00095 */
00096 /////////////////////////////////////////
00097 //
00098 //Send_Status
00099 //
00100 //リクエストがきたとき、自分の位置情報などを返信する
00101 /////////////////////////////////////////
00102 void Send_Status(char SenderIDc){
00103     XBeeAddress64 send_Address;
00104     if(SenderIDc == '0'){
00105         send_Address = manager_Address;
00106     }
00107     if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
00108         send_Address = robot_Address[SenderIDc - 'A'];
00109     }
00110     if(SenderIDc >= 'a' && SenderIDc <= 'z'){
00111         send_Address = base_Address[SenderIDc - 'a'];
00112     }
00113     //send normal data
00114     //Create GPS Infomation Packet
00115     agz.createReceiveStatusCommand(MyID,SenderIDc,
00116                                     agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00117                                     agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00118                                     agz.get_agzCov_lati(),agz.get_agzCov_longi());
00119     
00120 /*    //debug***************************************************
00121     printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
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     for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
00127         printf("\n");
00128     //debug end***************************************************
00129  */   
00130     //Select Destination
00131     ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
00132     //Send -> Base
00133     xbee.send(tx64request);
00134 }
00135 
00136 /////////////////////////////////////////
00137 //
00138 //Get GPS function
00139 //
00140 /////////////////////////////////////////
00141 
00142 void Get_GPS(Adafruit_GPS *myGPS){
00143     static int flag = 0;  
00144 //    static int save_counter = 0;
00145     
00146     if (myGPS->fix) {
00147         agz.nowStatus = GPS_AVAIL;
00148         agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
00149         
00150         if(flag < COUNTER_MAX){
00151             flag++; 
00152         }
00153         if(flag == 15){
00154             x_prev = agz.get_agzPoint_lati();
00155             y_prev = agz.get_agzPoint_longi();
00156         }
00157             
00158         if(flag >= 16){
00159           if(abs(x_prev - agz.get_agzPoint_lati()) < ERROR_RANGE && abs(y_prev - agz.get_agzPoint_longi()) < ERROR_RANGE){
00160                 Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi());
00161                 change = 1;
00162             }
00163 /*            else{
00164                 change = 0;
00165             }
00166             
00167             if(save_counter < 10){
00168                 fp = fopen(filename, "a");
00169                 fprintf(fp, "%d %.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
00170                     change, agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00171                     agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00172                     agz.get_agzCov_lati(),agz.get_agzCov_longi());
00173                 fclose(fp);   
00174                 
00175                 if((flag - 16) % 500 == 0){
00176                     filename[10]++;
00177                     save_counter++;
00178                 }
00179             }
00180 */
00181         } 
00182         
00183                 printf("%.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
00184                 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00185                 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00186                 agz.get_agzCov_lati(),agz.get_agzCov_longi());
00187     }
00188     else agz.nowStatus = GPS_UNAVAIL;
00189     
00190 } 
00191 
00192 
00193 /////////////////////////////////////////
00194 //
00195 //Kalman Processing
00196 //
00197 /////////////////////////////////////////
00198 void calc_Kalman(){
00199   //calc Kalman gain
00200   Kx = s2x_prev/(s2x_prev+s2_R);
00201   Ky = s2y_prev/(s2y_prev+s2_R);
00202   //estimate
00203   x_cur = x_prev + Kx*(zx-x_prev);
00204   y_cur = y_prev + Ky*(zy-y_prev);
00205   //calc sigma
00206   s2x_cur = s2x_prev-Kx*s2x_prev;
00207   s2y_cur = s2y_prev-Ky*s2y_prev;
00208 
00209 }
00210 
00211 void Kalman(double Latitude,double Longitude){
00212 
00213     zx = Latitude;
00214     zy = Longitude;
00215 
00216     calc_Kalman();
00217     
00218     //更新
00219     x_prev = x_cur;
00220     y_prev = y_cur;
00221     s2x_prev = s2x_cur;
00222     s2y_prev = s2y_cur;
00223     
00224     //agzPontKalmanとagzCovに格納する
00225     agz.set_agzPointKalman_lati(x_cur);
00226     agz.set_agzPointKalman_longi(y_cur);
00227     agz.set_agzCov(s2x_cur,s2y_cur);
00228     
00229 }
00230 
00231 
00232 /////////////////////////////////////////
00233 //
00234 //Main Processing
00235 //
00236 /////////////////////////////////////////
00237 int main() {
00238     //start up time
00239     wait(3);
00240     //set pc frequency to 57600bps 
00241     pc.baud(PC_BAUD_RATE); 
00242     //set xbee frequency to 57600bps
00243     xbee.begin(XBEE_BAUD_RATE);    
00244         
00245 
00246     //GPS setting
00247     gps_Serial = new Serial(p28,p27);
00248     Adafruit_GPS myGPS(gps_Serial); 
00249     Timer refresh_Timer;
00250     const int refresh_Time = 1000; //refresh time in ms
00251     myGPS.begin(GPS_BAUD_RATE); 
00252     
00253     char SenderIDc;
00254     //GPS Send Command
00255     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
00256     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
00257     myGPS.sendCommand(PGCMD_ANTENNA);
00258     
00259     wait(2);
00260        
00261     //interrupt start
00262     refresh_Timer.start();
00263 
00264     printf("start\n");
00265     
00266     while (true) {
00267         
00268         //Check Xbee Buffer Available
00269         xbee.readPacket();
00270             
00271         if (xbee.getResponse().isAvailable()) {
00272             xbee.getResponse().getZBRxResponse(zbRx);
00273             uint8_t *buf = zbRx.getFrameData();
00274                 
00275             if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
00276                 xbee.getResponse().getZBRxResponse(zbRx);
00277                 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
00278                 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
00279                 SenderIDc = buf1[5];//送信元のIDを取得する
00280                 char Command_type =agz.checkCommnadType(buf);//コマンドタイプを取得する
00281                  
00282                 //Check Command Type 
00283                 switch(Command_type){
00284                     //Get Request command
00285                     case STATUS_REQUEST:{
00286                         Send_Status(SenderIDc);                     
00287                         break;          
00288                     }
00289                     default:{
00290                         break;
00291                     }
00292                 }//endswitch
00293             }//endifZB_RX_RESPONSE
00294         }//endifisAvailable
00295                        
00296         myGPS.read();
00297         //recive gps module
00298         //check if we recieved a new message from GPS, if so, attempt to parse it,
00299         if ( myGPS.newNMEAreceived() ) {
00300             if ( !myGPS.parse(myGPS.lastNMEA()) ) {
00301                 continue;   
00302             }       
00303         }
00304         
00305         
00306         if (refresh_Timer.read_ms() >= refresh_Time) {
00307             refresh_Timer.reset();
00308             Get_GPS(&myGPS);
00309 
00310         }
00311     }
00312 }