main.cpp

00001 /**********************************************/
00002 //  
00003 //    
00004 //
00005 //  Program name: Aigamozu ROBOT
00006 //  Author: Mineta Kizuku
00007 //  
00008 // 
00009 //
00010 /**********************************************/
00011 
00012 /**********************************************/
00013 //更新情報
00014 //2015/05/11
00015 //ロボットプログラムの作成
00016 //
00017 //2015/05/13
00018 //カルマンフィルタの共分散の値を0.0001以下にならないようにした
00019 //共分散の値を10進数に変換するようにした
00020 //
00021 //2015/05/13 Yokokawa
00022 //何回目のGPSでとられたGPSか確認するようにしました。
00023 //
00024 //2015/05/15
00025 //プログラムcreateReceiveStatusCommand()にて
00026 //Aigamozu_collect_dataにinかoutかを送るためにstate関連をいじったので必要に応じて直してください。
00027 //
00028 //2015/05//17
00029 //Send_Status関数内を変更：基地局への送信データのみ現在のモードを入れるパケットの部分に内外判定の結果を入れるようにした
00030 //基地局以外には現在のモードを送信するようにしてある
00031 //要確認！！！！
00032 //
00033 //2015/05/17
00034 //Get_GPS()の中身longitudeの範囲138〜140に変更
00035 //
00036 //2015/05/19
00037 //autoモードのとき、三十秒前進・三秒右に転回に変更した。
00038 //
00039 //2015/05/24
00040 //Kalmanフィルターを十進数で計算するようにした。
00041 //Kalmanフィルターの計算式を変更した。
00042 //set_kalmanを追加した。
00043 //
00044 //2015/05/29
00045 //auto_Move関数の実装とAigamozuControlPackets内にcontrol_Mortor関数の実装
00046 //
00047 //2015/05/30
00048 //新カルマンフィルタの実装
00049 /**********************************************/
00050 
00051 #include "mbed.h"
00052 #include "XBee.h"
00053 #include "MBed_Adafruit_GPS.h"
00054 #include "AigamozuControlPackets.h"
00055 #include "agzIDLIST.h"
00056 #include "aigamozuSetting.h"
00057 #include "Kalman.h"
00058 #include "math.h"
00059 
00060 #define SIGMA_MIN 0.0001
00061 
00062 //************ID Number*****************
00063 //Robot   ID: 'A' ~ 'Z'
00064 //Base    ID: 'a' ~ 'a'
00065 //manager ID: '0'(数字のゼロ)
00066 //
00067 const char MyID = 'A';
00068 //************ID Number*****************
00069 
00070 /////////////////////////////////////////
00071 //
00072 //Pin Setting
00073 //
00074 /////////////////////////////////////////
00075 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
00076 
00077 
00078 /////////////////////////////////////////
00079 //
00080 //Connection Setting
00081 //
00082 /////////////////////////////////////////
00083 
00084 //Serial Connect Setting: PC <--> mbed
00085 Serial pc(USBTX, USBRX);    
00086 
00087 //Serial Connect Setting: GPS <--> mbed
00088 Serial * gps_Serial;
00089 
00090 //Serial Connect Setting: XBEE <--> mbed
00091 XBee xbee(p13,p14);
00092 ZBRxResponse zbRx = ZBRxResponse();
00093 
00094 //set up GPS module
00095 
00096 //set up AigamozuControlPackets library
00097 AigamozuControlPackets agz(agz_motorShield);
00098 
00099 
00100 /////////////////////////////////////////
00101 //
00102 //For Kalman data
00103 //
00104 /////////////////////////////////////////
00105 #define FIRST_S2_1 1.0e-8
00106 #define FIRST_S2_2 1.0e-6
00107 #define COUNTER_MAX 10000
00108 #define ERROR_RANGE 0.001
00109 
00110 double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
00111 double s2x_cur=FIRST_S2_1,s2x_prev=FIRST_S2_1,s2y_cur=FIRST_S2_1,s2y_prev=FIRST_S2_1;//緯度経度のの時刻tと時刻t-1での共分散
00112 double s2_R=FIRST_S2_2;//GPSセンサの分散
00113 double s2_Q=FIRST_S2_2;
00114 double Kx=0,Ky=0;//カルマンゲイン
00115 double zx,zy;//観測値
00116 void Kalman(double Latitude,double Longitude);
00117 int change = 0;
00118 
00119 
00120 LocalFileSystem local("local");  // マウントポイントを定義（ディレクトリパスになる）
00121 
00122 /////////////////////////////////////////
00123 //
00124 //Plus Speed
00125 //
00126 //MNUAL_MODEの時にスピードを変える
00127 /////////////////////////////////////////
00128 void Plus_Speed(uint8_t *packetdata){
00129        
00130     if(agz.nowMode == MANUAL_MODE){ 
00131         agz.changeSpeed(packetdata);
00132     }
00133     
00134 }
00135 
00136 /////////////////////////////////////////
00137 //
00138 //Send Status
00139 //
00140 //リクエストがきたとき、自分の位置情報などを返信する
00141 /////////////////////////////////////////
00142 void Send_Status(char SenderIDc){
00143     XBeeAddress64 send_Address;
00144     if(SenderIDc == '0'){
00145         send_Address = manager_Address;
00146         agz.createReceiveStatusCommand(MyID,SenderIDc, agz.nowMode,
00147                                         agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00148                                         agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00149                                         agz.get_agzCov_lati(),agz.get_agzCov_longi());             
00150     }
00151     if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
00152         send_Address = robot_Address[SenderIDc - 'A'];
00153         //Create GPS Infomation Packet
00154         agz.createReceiveStatusCommand(MyID,SenderIDc, agz.nowMode,
00155                                         agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00156                                         agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00157                                         agz.get_agzCov_lati(),agz.get_agzCov_longi());
00158     }
00159     if(SenderIDc >= 'a' && SenderIDc <= 'z'){
00160         send_Address = base_Address[SenderIDc - 'a'];
00161 
00162         agz.createReceiveStatusCommand(MyID,SenderIDc, (int)agz.gpsAuto(),
00163                                         agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00164                                         agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00165                                         agz.get_agzCov_lati(),agz.get_agzCov_longi());    
00166     }
00167     //send normal data
00168 
00169  /*   
00170     //debug***************************************************
00171     printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
00172             agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00173             agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00174             agz.get_agzCov_lati(),agz.get_agzCov_longi()
00175             );
00176     for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
00177         printf("\n");
00178     //debug end***************************************************
00179  */   
00180     //Select Destination
00181     ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
00182     //Send -> Base
00183     xbee.send(tx64request);
00184     
00185 }
00186 
00187 /////////////////////////////////////////
00188 //
00189 //Get GPS function
00190 //
00191 /////////////////////////////////////////
00192 
00193 void Get_GPS(Adafruit_GPS *myGPS){
00194     static int flag = 0;
00195     
00196     if (myGPS->fix) {
00197         
00198         agz.nowStatus = GPS_AVAIL;
00199         agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
00200         
00201         if(flag < COUNTER_MAX){
00202             flag++; 
00203         }
00204         if(flag == 5){
00205             x_prev = agz.get_agzPoint_lati();
00206             y_prev = agz.get_agzPoint_longi();
00207         }
00208             
00209         if(flag >= 6){
00210             if(abs(x_prev - agz.get_agzPoint_lati()) < ERROR_RANGE && abs(y_prev - agz.get_agzPoint_longi()) < ERROR_RANGE){
00211                 Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi());
00212                 change = 1;
00213             }
00214             else{
00215                 change = 0;
00216             }
00217         /*    fp = fopen(filename, "w");
00218             fprintf(fp, "%d %.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
00219                 change, agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00220                 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00221                 agz.get_agzCov_lati(),agz.get_agzCov_longi());
00222                 */
00223             
00224         } 
00225             
00226                 printf("%.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
00227                 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
00228                 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
00229                 agz.get_agzCov_lati(),agz.get_agzCov_longi());
00230     }
00231     else agz.nowStatus = GPS_UNAVAIL;
00232     
00233 } 
00234 
00235 /////////////////////////////////////////
00236 //
00237 //New Mode
00238 //
00239 /////////////////////////////////////////
00240 
00241 void New_Mode(uint8_t *packetdata){
00242   
00243   //bool result;
00244    agz.changeMode(packetdata); 
00245   
00246 }
00247   
00248 /////////////////////////////////////////
00249 //
00250 //Get Status
00251 //
00252 /////////////////////////////////////////
00253 void Get_Status(char SenderIDc,uint8_t *packetdata){
00254     
00255     //マネージャからデータが来たとき
00256     if(SenderIDc == '0'){
00257         printf("get manager Status\n");
00258     }
00259     //他のロボットからデータが来たとき
00260     if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
00261         printf("get other robots Status\n");
00262     }
00263     //基地局からデータが来たとき
00264     if(SenderIDc >= 'a' && SenderIDc <= 'z'){
00265         printf("Get Base data\n");
00266         int id = SenderIDc - 'a';
00267         agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]);
00268         agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
00269         
00270         //debug
00271         for(int i = 0;i < 4;i++){
00272         printf("BASE:%d\n",i);
00273         printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\n",
00274             agz.get_basePoint_lati(i),agz.get_basePoint_longi(i),
00275             agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i)
00276             );
00277         }
00278     }
00279 }
00280 
00281 void Get_Status_Kalman(char SenderIDc,uint8_t *packetdata){
00282     
00283     //マネージャからデータが来たとき
00284     if(SenderIDc == '0'){
00285         printf("get manager Status Kalman\n");
00286     }
00287     //他のロボットからデータが来たとき
00288     if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
00289         printf("get other robots Status Kalman\n");
00290     }
00291     //基地局からデータが来たとき
00292     if(SenderIDc >= 'a' && SenderIDc <= 'z'){
00293         printf("Get Base data Kalman\n");
00294         int id = SenderIDc - 'a';
00295         agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
00296         
00297         //debug
00298         for(int i = 0;i < 4;i++){
00299         printf("BASE:%d\n",i);
00300         printf("latitudeK:%f,longitudeK:%f\n",
00301             agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i));
00302         }
00303     }
00304 }
00305 
00306 /////////////////////////////////////////
00307 //
00308 //Send_Request_to_base
00309 //
00310 /////////////////////////////////////////
00311 void Send_Request_Base(int basenumber){
00312     printf("send\n");
00313     agz.createRequestCommand(MyID, basenumber);
00314     //Select Destination
00315     ZBTxRequest tx64request(base_Address[basenumber],agz.packetData,agz.getPacketLength());
00316     //Send -> Base
00317     xbee.send(tx64request);
00318 }
00319 
00320 /////////////////////////////////////////
00321 //
00322 //auto_Move
00323 //
00324 //InAreaかOutAreaの判定
00325 //Kalmanを通した値を出力(Baseと自分)
00326 //2015/05/29
00327 //外側判定と処理の実装
00328 //内側判定：シーケンス動作
00329 //外側判定：10秒間バック、3秒間旋回を行い、その後シーケンス動作へ
00330 /////////////////////////////////////////
00331 
00332 void auto_Move(){
00333  
00334  bool result; // 毎回の内外判定の結果を格納
00335  bool out_flag = false; // 外側処理の実行フラグ
00336  const int sequenceTime[4] = {30000, 31000, 34000, 34200};
00337  const int outSequenceTime[4] = {10000, 11000, 14000, 14200};
00338  
00339  result = agz.gpsAuto();
00340  //agz.set_agzAutoGPS();
00341  //agz.set_agzKalmanGPS();
00342  
00343  if(out_flag == false && result == true){
00344     out_flag = true;
00345     agz.Out_Timer.reset();
00346     }
00347  
00348  if(out_flag == false){
00349     if(agz.Move_Timer.read_ms() < sequenceTime[0]){
00350           agz.control_Motor(0); //straight
00351         }
00352     if(agz.Move_Timer.read_ms() > sequenceTime[0] && agz.Move_Timer.read_ms() < sequenceTime[1]){
00353           agz.control_Motor(1);
00354         }
00355     if(agz.Move_Timer.read_ms() > sequenceTime[1] && agz.Move_Timer.read_ms() < sequenceTime[2]){
00356           agz.control_Motor(2); //Turn Right
00357         }   
00358     if(agz.Move_Timer.read_ms() > sequenceTime[2] && agz.Move_Timer.read_ms() < sequenceTime[3]){
00359           agz.control_Motor(1);
00360         }
00361     if(agz.Move_Timer.read_ms() > sequenceTime[3]){
00362           agz.Move_Timer.reset();
00363         }
00364     } 
00365  if(out_flag == true){
00366     if(agz.Out_Timer.read_ms() < outSequenceTime[0]){
00367            agz.control_Motor(3); //back
00368         }
00369     if(agz.Out_Timer.read_ms() > outSequenceTime[0] && agz.Out_Timer.read_ms() < outSequenceTime[1]){
00370            agz.control_Motor(1);
00371         }
00372     if(agz.Out_Timer.read_ms() > outSequenceTime[1] && agz.Out_Timer.read_ms() < outSequenceTime[2]){
00373            agz.control_Motor(2); //Turn Right
00374         }   
00375     if(agz.Out_Timer.read_ms() > outSequenceTime[2] && agz.Out_Timer.read_ms() < outSequenceTime[3]){
00376            agz.control_Motor(1);
00377         }
00378     if(agz.Out_Timer.read_ms() > outSequenceTime[3]){
00379            agz.Out_Timer.reset();
00380            agz.Move_Timer.reset();
00381            out_flag = false;
00382         }
00383   }
00384 }
00385 
00386 void print_gps(int count){
00387     
00388     printf("%d times:\n", count);
00389     printf("%f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
00390     
00391 }
00392 
00393 
00394 /////////////////////////////////////////
00395 //
00396 //Kalman Processing
00397 //
00398 /////////////////////////////////////////
00399 void calc_Kalman(){
00400   //calc Kalman gain
00401   Kx = (s2x_prev+s2_Q)/(s2x_prev+s2_R+s2_Q);
00402   Ky = (s2y_prev+s2_Q)/(s2y_prev+s2_R+s2_Q);
00403   //estimate
00404   x_cur = x_prev + Kx*(zx-x_prev);
00405   y_cur = y_prev + Ky*(zy-y_prev);
00406   //calc sigma
00407   s2x_cur = (1-Kx)*(s2x_prev+s2_Q);
00408   s2y_cur = (1-Ky)*(s2y_prev+s2_Q);
00409 
00410 }
00411 
00412 void Kalman(double Latitude,double Longitude){
00413 
00414     zx = Latitude;
00415     zy = Longitude;
00416 
00417     calc_Kalman();
00418     
00419     //更新
00420     x_prev = x_cur;
00421     y_prev = y_cur;
00422     s2x_prev = s2x_cur;
00423     s2y_prev = s2y_cur;
00424     
00425     //agzPontKalmanとagzCovに格納する
00426     agz.set_agzPointKalman_lati(x_cur);
00427     agz.set_agzPointKalman_longi(y_cur);
00428     agz.set_agzCov(s2x_cur,s2y_cur);
00429     
00430 }
00431 
00432 /////////////////////////////////////////
00433 //
00434 //Main Processing
00435 //
00436 /////////////////////////////////////////
00437 int main() {
00438     //start up time
00439     wait(3);
00440     //set pc frequency to 57600bps 
00441     pc.baud(PC_BAUD_RATE); 
00442     //set xbee frequency to 57600bps
00443     xbee.begin(XBEE_BAUD_RATE);    
00444         
00445 
00446     //GPS setting
00447     gps_Serial = new Serial(p28,p27);
00448     Adafruit_GPS myGPS(gps_Serial); 
00449     Timer refresh_Timer;
00450     const int refresh_Time = 1000; //refresh time in ms
00451     Timer auto_Timer;
00452     const int auto_Time = 100; //refresh time in ms
00453     int count = 0;
00454     
00455     
00456     
00457     myGPS.begin(GPS_BAUD_RATE); 
00458     
00459     Timer collect_Timer;
00460     const int collect_Time = 2000; //when we collect 4 GPS point, we use 
00461     int collect_flag = 0;
00462     
00463     char SenderIDc;
00464     //GPS Send Command
00465     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
00466     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
00467     myGPS.sendCommand(PGCMD_ANTENNA);
00468     
00469     wait_ms(2000);
00470        
00471     //interrupt start
00472     refresh_Timer.start();
00473     auto_Timer.start();
00474     agz.Move_Timer.start();
00475     agz.Out_Timer.start();
00476     collect_Timer.start();
00477     printf("start\n");
00478     
00479     
00480     while (true) {
00481         
00482         //Check Xbee Buffer Available
00483         xbee.readPacket();
00484             
00485         if (xbee.getResponse().isAvailable()) {
00486             xbee.getResponse().getZBRxResponse(zbRx);
00487             uint8_t *buf = zbRx.getFrameData();
00488                 
00489             if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
00490                 xbee.getResponse().getZBRxResponse(zbRx);
00491                 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
00492                 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
00493                 SenderIDc = buf1[5];//送信元のIDを取得する
00494                 char Command_type = agz.checkCommnadType(buf);//コマンドタイプを取得する
00495                  
00496                 //Check Command Type 
00497                 switch(Command_type){
00498                     //Get Request command
00499                     case MANUAL:{
00500                         Plus_Speed(buf);
00501                         break;
00502                     }
00503                     case STATUS_REQUEST:{
00504                         Send_Status(SenderIDc); 
00505                         printf("%c\n", SenderIDc);                  
00506                         break;          
00507                     }
00508                     case CHANGE_MODE:{
00509                         New_Mode(buf);
00510                         break;
00511                     }
00512                     case RECEIVE_STATUS:{
00513                         Get_Status(SenderIDc,buf1);
00514                         break;
00515                     }
00516                     default:{
00517                         break;
00518                     }
00519                 }//endswitch
00520             }//endifZB_RX_RESPONSE
00521         }//endifisAvailable
00522                        
00523         myGPS.read();
00524         //recive gps module
00525         //check if we recieved a new message from GPS, if so, attempt to parse it,
00526         if ( myGPS.newNMEAreceived() ) {
00527             if ( !myGPS.parse(myGPS.lastNMEA()) ) {
00528                 continue;   
00529             } 
00530             else{
00531                 count++;
00532             }    
00533         }
00534         //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
00535         if (refresh_Timer.read_ms() >= refresh_Time) {
00536             refresh_Timer.reset();
00537             //print_gps(count);
00538             Get_GPS(&myGPS);
00539 
00540         }
00541     
00542                  //get base GPS
00543         if( collect_Timer.read_ms() >= collect_Time){
00544             collect_Timer.reset();   
00545                      
00546             Send_Request_Base(collect_flag);
00547             
00548             collect_flag++;
00549 
00550             if(collect_flag == 4){
00551                 collect_flag = 0;
00552             }    
00553          } 
00554             
00555         if(agz.nowMode == AUTO_GPS_MODE && auto_Timer.read_ms() >= auto_Time){
00556            auto_Timer.reset();
00557            auto_Move(); 
00558         }
00559         
00560     }
00561     
00562 }