Diff: main.cpp

Revision:

38:5cd6d4964f65

Parent:

37:19a9a37a5658

--- a/main.cpp	Thu Oct 29 07:52:14 2015 +0000
+++ b/main.cpp	Mon Oct 24 08:15:16 2016 +0000
@@ -4,43 +4,16 @@
 //
 //  Program name: Aigamozu ROBOT
 //  Author: Mineta Kizuku
-//  
+//  Yokokawa
 // 
 //
 /**********************************************/
 
 /**********************************************/
 //更新情報
-//2015/05/11
-//ロボットプログラムの作成
-//
-//2015/05/13
-//カルマンフィルタの共分散の値を0.0001以下にならないようにした
-//共分散の値を10進数に変換するようにした
-//
-//2015/05/13 Yokokawa
-//何回目のGPSでとられたGPSか確認するようにしました。
-//
-//2015/05/15
-//プログラムcreateReceiveStatusCommand()にて
-//Aigamozu_collect_dataにinかoutかを送るためにstate関連をいじったので必要に応じて直してください。
-//
-//2015/05//17
-//Send_Status関数内を変更：基地局への送信データのみ現在のモードを入れるパケットの部分に内外判定の結果を入れるようにした
-//基地局以外には現在のモードを送信するようにしてある
-//要確認！！！！
-//
-//2015/05/17
-//Get_GPS()の中身longitudeの範囲138〜140に変更
-//
-//2015/05/19
-//autoモードのとき、三十秒前進・三秒右に転回に変更した。
-//
-//2015/05/24
-//Kalmanフィルターを十進数で計算するようにした。
-//Kalmanフィルターの計算式を変更した。
-//set_kalmanを追加した。
-//
+//展示会用プログラム
+//中身をすっきりさせました。
+//Automodeの際のプログラムはgpsAuto()の中身を変更すれば大丈夫です。
 /**********************************************/
 
 #include "mbed.h"
@@ -49,19 +22,8 @@
 #include "AigamozuControlPackets.h"
 #include "agzIDLIST.h"
 #include "aigamozuSetting.h"
-#include "Kalman.h"
 #include "math.h"
 
-#define SIGMA_MIN 0.0001
-
-//************ID Number*****************
-//Robot   ID: 'A' ~ 'Z'
-//Base    ID: 'a' ~ 'a'
-//manager ID: '0'(数字のゼロ)
-//
-const char MyID = 'D';
-//************ID Number*****************
-
 /////////////////////////////////////////
 //
 //Pin Setting
@@ -92,26 +54,9 @@
 AigamozuControlPackets agz(agz_motorShield);
 
 
-/////////////////////////////////////////
-//
-//For Kalman data
-//
-/////////////////////////////////////////
-#define FIRST_S2 0.000001
-#define COUNTER_MAX 10000
-double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
-double s2x_cur=FIRST_S2,s2x_prev=FIRST_S2,s2y_cur=FIRST_S2,s2y_prev=FIRST_S2;//緯度経度のの時刻tと時刻t-1での共分散
-double s2_R=FIRST_S2;//GPSセンサの分散
-double Kx=0,Ky=0;//カルマンゲイン
-double zx,zy;//観測値
-void Kalman(double Latitude,double Longitude);
+
 int change = 0;
 
-/*
-LocalFileSystem local("local");  // マウントポイントを定義（ディレクトリパスになる）
-FILE *fp;
-char filename[16] = "/local/out0.txt";
-*/
 /////////////////////////////////////////
 //
 //Plus Speed
@@ -126,112 +71,7 @@
     
 }
 
-/////////////////////////////////////////
-//
-//Send Status
-//
-//リクエストがきたとき、自分の位置情報などを返信する
-/////////////////////////////////////////
-void Send_Status(char SenderIDc){
-    XBeeAddress64 send_Address;
-    if(SenderIDc == '0'){
-        send_Address = manager_Address;
-        agz.createReceiveStatusCommand(MyID,SenderIDc, agz.nowMode,
-                                        agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
-                                        agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
-                                        agz.get_agzCov_lati(),agz.get_agzCov_longi());             
-    }
-    if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
-        send_Address = robot_Address[SenderIDc - 'A'];
-        //Create GPS Infomation Packet
-        agz.createReceiveStatusCommand(MyID,SenderIDc, agz.nowMode,
-                                        agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
-                                        agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
-                                        agz.get_agzCov_lati(),agz.get_agzCov_longi());
-    }
-    if(SenderIDc >= 'a' && SenderIDc <= 'z'){
-        send_Address = base_Address[SenderIDc - 'a'];
 
-        agz.createReceiveStatusCommand(MyID,SenderIDc, (int)agz.gpsAuto(),
-                                        agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
-                                        agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
-                                        agz.get_agzCov_lati(),agz.get_agzCov_longi());    
-    }
-    //send normal data
-
- /*   
-    //debug***************************************************
-    printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
-            agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
-            agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
-            agz.get_agzCov_lati(),agz.get_agzCov_longi()
-            );
-    for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
-        printf("\n");
-    //debug end***************************************************
- */   
-    //Select Destination
-    ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
-    //Send -> Base
-    xbee.send(tx64request);
-    
-}
-
-/////////////////////////////////////////
-//
-//Get GPS function
-//
-/////////////////////////////////////////
-
-void Get_GPS(Adafruit_GPS *myGPS){
-    static int flag = 0;
-//    static int save_counter = 0;
-    
-    if (myGPS->fix) {
-        
-        agz.nowStatus = GPS_AVAIL;
-        agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
-        
-        if(flag < COUNTER_MAX){
-            flag++; 
-        }
-        if(flag == 15){
-            x_prev = agz.get_agzPoint_lati();
-            y_prev = agz.get_agzPoint_longi();
-        }
-            
-        if(flag >= 16){
-            if(abs(x_prev - agz.get_agzPoint_lati()) < 0.001 && abs(y_prev - agz.get_agzPoint_longi()) < 0.001){
-                Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi());
-                change = 1;
-            }
-            else{
-                change = 0;
-            }
-/*            if(save_counter < 10){
-                fp = fopen(filename, "a");
-                fprintf(fp, "%d %.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
-                    change, agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
-                    agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
-                    agz.get_agzCov_lati(),agz.get_agzCov_longi());
-                fclose(fp);   
-                
-                if((flag - 16) % 500 == 0){
-                    filename[10]++;
-                    save_counter++;
-                }
-            }
-*/
-        } 
-            
-                printf("%.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
-                agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
-                agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
-                agz.get_agzCov_lati(),agz.get_agzCov_longi());
-    }
-    else agz.nowStatus = GPS_UNAVAIL;
-    
-} 
 
 /////////////////////////////////////////
 //
@@ -245,154 +85,7 @@
    agz.changeMode(packetdata); 
   
 }
-  
-/////////////////////////////////////////
-//
-//Get Status
-//
-/////////////////////////////////////////
-void Get_Status(char SenderIDc,uint8_t *packetdata){
-    
-    //マネージャからデータが来たとき
-    if(SenderIDc == '0'){
-        printf("get manager Status\n");
-    }
-    //他のロボットからデータが来たとき
-    if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
-        printf("get other robots Status\n");
-    }
-    //基地局からデータが来たとき
-    if(SenderIDc >= 'a' && SenderIDc <= 'z'){
-        printf("Get Base data\n");
-        int id = SenderIDc - 'a';
-        agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]);
-        agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
-        
-        //debug
-        for(int i = 0;i < 4;i++){
-        printf("BASE:%d\n",i);
-        printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\n",
-            agz.get_basePoint_lati(i),agz.get_basePoint_longi(i),
-            agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i)
-            );
-        }
-    }
-}
 
-void Get_Status_Kalman(char SenderIDc,uint8_t *packetdata){
-    
-    //マネージャからデータが来たとき
-    if(SenderIDc == '0'){
-        printf("get manager Status Kalman\n");
-    }
-    //他のロボットからデータが来たとき
-    if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
-        printf("get other robots Status Kalman\n");
-    }
-    //基地局からデータが来たとき
-    if(SenderIDc >= 'a' && SenderIDc <= 'z'){
-        printf("Get Base data Kalman\n");
-        int id = SenderIDc - 'a';
-        agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
-        
-        //debug
-        for(int i = 0;i < 4;i++){
-        printf("BASE:%d\n",i);
-        printf("latitudeK:%f,longitudeK:%f\n",
-            agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i));
-        }
-    }
-}
-
-/////////////////////////////////////////
-//
-//Send_Request_to_base
-//
-/////////////////////////////////////////
-void Send_Request_Base(int basenumber){
-    printf("send\n");
-    agz.createRequestCommand(MyID, basenumber);
-    //Select Destination
-    ZBTxRequest tx64request(base_Address[basenumber],agz.packetData,agz.getPacketLength());
-    //Send -> Base
-    xbee.send(tx64request);
-}
-
-/////////////////////////////////////////
-//
-//auto_Move
-//
-//InAreaかOutAreaの判定
-//Kalmanを通した値を出力(Baseと自分)
-/////////////////////////////////////////
-
-void auto_Move(){
- 
- bool result;
- int i;
- 
- result = agz.gpsAuto();
- //agz.set_agzAutoGPS();
- //agz.set_agzKalmanGPS();
- 
- if(result == true){
-        printf("Out Area\n");
- }
-else if(result == false){
-        printf("In Area\n");
-}  
-for(i = 0; i < 4; i++){
-    printf("%d: %f, %f\n", i, agz.get_basePointKalman_lati(i), agz.get_basePointKalman_longi(i));
- }
- printf("robot: %f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
- 
-}
-
-void print_gps(int count){
-    
-    printf("%d times:\n", count);
-    printf("%f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
-    
-}
-
-
-/////////////////////////////////////////
-//
-//Kalman Processing
-//
-/////////////////////////////////////////
-void calc_Kalman(){
-  //calc Kalman gain
-  Kx = s2x_prev/(s2x_prev+s2_R);
-  Ky = s2y_prev/(s2y_prev+s2_R);
-  //estimate
-  x_cur = x_prev + Kx*(zx-x_prev);
-  y_cur = y_prev + Ky*(zy-y_prev);
-  //calc sigma
-  s2x_cur = s2x_prev-Kx*s2x_prev;
-  s2y_cur = s2y_prev-Ky*s2y_prev;
-
-}
-
-void Kalman(double Latitude,double Longitude){
-
-    zx = Latitude;
-    zy = Longitude;
-
-    calc_Kalman();
-    
-    //更新
-    x_prev = x_cur;
-    y_prev = y_cur;
-    s2x_prev = s2x_cur;
-    s2y_prev = s2y_cur;
-    
-    //agzPontKalmanとagzCovに格納する
-    agz.set_agzPointKalman_lati(x_cur);
-    agz.set_agzPointKalman_longi(y_cur);
-    agz.set_agzCov(s2x_cur,s2y_cur);
-    
-}
 
 /////////////////////////////////////////
 //
@@ -411,33 +104,15 @@
     //GPS setting
     gps_Serial = new Serial(p28,p27);
     Adafruit_GPS myGPS(gps_Serial); 
-    Timer refresh_Timer;
-    const int refresh_Time = 1000; //refresh time in ms
+    
     Timer auto_Timer;
     const int auto_Time = 2000; //refresh time in ms
-    int count = 0;
-    
-    const int straight = 30000, turning = 34000, wait = 31000;
-    
-    myGPS.begin(GPS_BAUD_RATE); 
-    
-    Timer collect_Timer;
-    const int collect_Time = 2000; //when we collect 4 GPS point, we use 
-    int collect_flag = 0;
-    
-    char SenderIDc;
-    //GPS Send Command
-    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
-    myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
-    myGPS.sendCommand(PGCMD_ANTENNA);
     
     wait_ms(2000);
        
     //interrupt start
-    refresh_Timer.start();
     auto_Timer.start();
     agz.Move_Timer.start();
-    collect_Timer.start();
     printf("start\n");
     
     
@@ -453,8 +128,6 @@
             if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
                 xbee.getResponse().getZBRxResponse(zbRx);
                 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
-                uint8_t *buf1 = &buf[11];//データの部分のみを格納する
-                SenderIDc = buf1[5];//送信元のIDを取得する
                 char Command_type = agz.checkCommnadType(buf);//コマンドタイプを取得する
                  
                 //Check Command Type 
@@ -464,9 +137,7 @@
                         Plus_Speed(buf);
                         break;
                     }
-                    case STATUS_REQUEST:{
-                        Send_Status(SenderIDc); 
-                        printf("%c\n", SenderIDc);                  
+                    case STATUS_REQUEST:{                 
                         break;          
                     }
                     case CHANGE_MODE:{
@@ -474,7 +145,6 @@
                         break;
                     }
                     case RECEIVE_STATUS:{
-                        Get_Status(SenderIDc,buf1);
                         break;
                     }
                     default:{
@@ -483,60 +153,12 @@
                 }//endswitch
             }//endifZB_RX_RESPONSE
         }//endifisAvailable
-                       
-        myGPS.read();
-        //recive gps module
-        //check if we recieved a new message from GPS, if so, attempt to parse it,
-        if ( myGPS.newNMEAreceived() ) {
-            if ( !myGPS.parse(myGPS.lastNMEA()) ) {
-                continue;   
-            } 
-            else{
-                count++;
-            }    
-        }
-        //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
-        if (refresh_Timer.read_ms() >= refresh_Time) {
-            refresh_Timer.reset();
-            //print_gps(count);
-            Get_GPS(&myGPS);
-
-        }
-    
-                 //get base GPS
-        if( collect_Timer.read_ms() >= collect_Time){
-            collect_Timer.reset();   
-                     
-            Send_Request_Base(collect_flag);
-            
-            collect_flag++;
-
-            if(collect_flag == 4){
-                collect_flag = 0;
-            }    
-         } 
-            
-        if(agz.nowMode == AUTO_GPS_MODE && auto_Timer.read_ms() >= auto_Time){
+  
+     if(agz.nowMode == AUTO_GPS_MODE && auto_Timer.read_ms() >= auto_Time){
            auto_Timer.reset();
-           auto_Move(); 
+           agz.gpsAuto();
         }
         
-     if(agz.nowMode == AUTO_GPS_MODE){
-        if(agz.Move_Timer.read_ms() < straight){
-           agz.test_Auto(0); //straight
-        }
-        if(agz.Move_Timer.read_ms() > straight && agz.Move_Timer.read_ms() < wait){
-            agz.test_Auto(1);
-        }
-        if(agz.Move_Timer.read_ms() > wait && agz.Move_Timer.read_ms() < turning){
-            agz.test_Auto(2); //Turn Right
-        }   
-        if(agz.Move_Timer.read_ms() > turning){
-            agz.test_Auto(3);
-            wait_ms(200);
-            agz.Move_Timer.reset();
-        }
-    }
     }
     
 }
\ No newline at end of file