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 = 'b';
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 double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
00083 double s2x_cur=FIRST_S2,s2x_prev=FIRST_S2,s2y_cur=FIRST_S2,s2y_prev=FIRST_S2;//緯度経度のの時刻tと時刻t-1での共分散
00084 double s2_R=FIRST_S2;//GPSセンサの分散
00085 double Kx=0,Ky=0;//カルマンゲイン
00086 double zx,zy;//観測値
00087 void Kalman(double Latitude,double Longitude);
00088 
00089 
00090 /////////////////////////////////////////
00091 //
00092 //Send_Status
00093 //
00094 //リクエストがきたとき、自分の位置情報などを返信する
00095 /////////////////////////////////////////
00096 void Send_Status(char SenderIDc){
00097     XBeeAddress64 send_Address;
00098     if(SenderIDc == '0'){
00099         send_Address = manager_Address;
00100     }
00101     if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
00102         send_Address = robot_Address[SenderIDc - 'A'];
00103     }
00104     if(SenderIDc >= 'a' && SenderIDc <= 'z'){
00105         send_Address = base_Address[SenderIDc - 'a'];
00106     }
00107     //send normal data
00108     //Create GPS Infomation Packet
00109     agz.createReceiveStatusCommand(MyID,SenderIDc,
00110                                     agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00111                                     agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00112                                     agz.get_agzCov_lati(),agz.get_agzCov_longi());
00113     
00114 /*    //debug***************************************************
00115     printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
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     for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
00121         printf("\n");
00122     //debug end***************************************************
00123  */   
00124     //Select Destination
00125     ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
00126     //Send -> Base
00127     xbee.send(tx64request);
00128 }
00129 
00130 /////////////////////////////////////////
00131 //
00132 //Get GPS function
00133 //
00134 /////////////////////////////////////////
00135 
00136 void Get_GPS(Adafruit_GPS *myGPS){
00137     static int flag = 0;
00138     
00139     if (myGPS->fix) {
00140         agz.nowStatus = GPS_AVAIL;
00141         agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
00142         
00143         if(flag < COUNTER_MAX){
00144             flag++; 
00145         }
00146         if(flag == 15){
00147             x_prev = agz.get_agzPoint_lati();
00148             y_prev = agz.get_agzPoint_longi();
00149         }
00150             
00151         if(flag >= 16){
00152             Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi());
00153         } 
00154         
00155                 printf("%.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
00156                 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00157                 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00158                 agz.get_agzCov_lati(),agz.get_agzCov_longi());
00159     }
00160     else agz.nowStatus = GPS_UNAVAIL;
00161     
00162 } 
00163 
00164 
00165 /////////////////////////////////////////
00166 //
00167 //Kalman Processing
00168 //
00169 /////////////////////////////////////////
00170 void calc_Kalman(){
00171   //calc Kalman gain
00172   Kx = s2x_prev/(s2x_prev+s2_R);
00173   Ky = s2y_prev/(s2y_prev+s2_R);
00174   //estimate
00175   x_cur = x_prev + Kx*(zx-x_prev);
00176   y_cur = y_prev + Ky*(zy-y_prev);
00177   //calc sigma
00178   s2x_cur = s2x_prev-Kx*s2x_prev;
00179   s2y_cur = s2y_prev-Ky*s2y_prev;
00180 
00181 }
00182 
00183 void Kalman(double Latitude,double Longitude){
00184 
00185     zx = Latitude;
00186     zy = Longitude;
00187 
00188     calc_Kalman();
00189     
00190     //更新
00191     x_prev = x_cur;
00192     y_prev = y_cur;
00193     s2x_prev = s2x_cur;
00194     s2y_prev = s2y_cur;
00195     
00196     //agzPontKalmanとagzCovに格納する
00197     agz.set_agzPointKalman_lati(x_cur);
00198     agz.set_agzPointKalman_longi(y_cur);
00199     agz.set_agzCov(s2x_cur,s2y_cur);
00200     
00201 }
00202 
00203 
00204 /////////////////////////////////////////
00205 //
00206 //Main Processing
00207 //
00208 /////////////////////////////////////////
00209 int main() {
00210     //start up time
00211     wait(3);
00212     //set pc frequency to 57600bps 
00213     pc.baud(PC_BAUD_RATE); 
00214     //set xbee frequency to 57600bps
00215     xbee.begin(XBEE_BAUD_RATE);    
00216         
00217 
00218     //GPS setting
00219     gps_Serial = new Serial(p28,p27);
00220     Adafruit_GPS myGPS(gps_Serial); 
00221     Timer refresh_Timer;
00222     const int refresh_Time = 1000; //refresh time in ms
00223     myGPS.begin(GPS_BAUD_RATE); 
00224     
00225     char SenderIDc;
00226     //GPS Send Command
00227     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
00228     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
00229     myGPS.sendCommand(PGCMD_ANTENNA);
00230     
00231     wait(2);
00232        
00233     //interrupt start
00234     refresh_Timer.start();
00235 
00236     printf("start\n");
00237     
00238     while (true) {
00239         
00240         //Check Xbee Buffer Available
00241         xbee.readPacket();
00242             
00243         if (xbee.getResponse().isAvailable()) {
00244             xbee.getResponse().getZBRxResponse(zbRx);
00245             uint8_t *buf = zbRx.getFrameData();
00246                 
00247             if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
00248                 xbee.getResponse().getZBRxResponse(zbRx);
00249                 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
00250                 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
00251                 SenderIDc = buf1[5];//送信元のIDを取得する
00252                 char Command_type =agz.checkCommnadType(buf);//コマンドタイプを取得する
00253                  
00254                 //Check Command Type 
00255                 switch(Command_type){
00256                     //Get Request command
00257                     case STATUS_REQUEST:{
00258                         Send_Status(SenderIDc);                     
00259                         break;          
00260                     }
00261                     default:{
00262                         break;
00263                     }
00264                 }//endswitch
00265             }//endifZB_RX_RESPONSE
00266         }//endifisAvailable
00267                        
00268         myGPS.read();
00269         //recive gps module
00270         //check if we recieved a new message from GPS, if so, attempt to parse it,
00271         if ( myGPS.newNMEAreceived() ) {
00272             if ( !myGPS.parse(myGPS.lastNMEA()) ) {
00273                 continue;   
00274             }       
00275         }
00276         
00277         
00278         if (refresh_Timer.read_ms() >= refresh_Time) {
00279             refresh_Timer.reset();
00280             Get_GPS(&myGPS);
00281 
00282         }
00283     }
00284 }