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