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