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