File content as of revision 39:f6c7b746f0c4:

/**********************************************/
//  
//    
//
//  Program name: Aigamozu ROBOT
//  Author: Mineta Kizuku
//  
// 
//
/**********************************************/

/**********************************************/
//更新情報
//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を追加した。
//
//2015/05/29
//auto_Move関数の実装とAigamozuControlPackets内にcontrol_Mortor関数の実装
//
//2015/05/30
//新カルマンフィルタの実装
//
//2015/06/04
//内外判定の結果によって動きを変えるように変更
//ロボットA,B,C,D,Eは基地局a,b,c,dの内側を
//ロボットF,G,H,I,Jは基地局e,f,g,hの内側を走る
/**********************************************/

#include "mbed.h"
#include "XBee.h"
#include "MBed_Adafruit_GPS.h"
#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 = 'L';

const int collect_base_address[4]={5,6,7,8};//1,2,3,4,5,6,7,8
//************ID Number*****************

/////////////////////////////////////////
//
//Pin Setting
//
/////////////////////////////////////////
VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);


/////////////////////////////////////////
//
//Connection Setting
//
/////////////////////////////////////////

//Serial Connect Setting: PC <--> mbed
Serial pc(USBTX, USBRX);    

//Serial Connect Setting: GPS <--> mbed
Serial * gps_Serial;

//Serial Connect Setting: XBEE <--> mbed
XBee xbee(p13,p14);
ZBRxResponse zbRx = ZBRxResponse();

//set up GPS module

//set up AigamozuControlPackets library
AigamozuControlPackets agz(agz_motorShield);


/////////////////////////////////////////
//
//For Kalman data
//
/////////////////////////////////////////
#define FIRST_S2_1 1.0e-8
#define FIRST_S2_2 1.0e-6
#define COUNTER_MAX 10000
#define ERROR_RANGE 0.001

double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
double s2x_cur=FIRST_S2_1,s2x_prev=FIRST_S2_1,s2y_cur=FIRST_S2_1,s2y_prev=FIRST_S2_1;//緯度経度のの時刻tと時刻t-1での共分散
double s2_R=FIRST_S2_2;//GPSセンサの分散
double s2_Q=FIRST_S2_2;
double Kx=0,Ky=0;//カルマンゲイン
double zx,zy;//観測値
void Kalman(double Latitude,double Longitude);
int change = 0;


LocalFileSystem local("local");  // マウントポイントを定義（ディレクトリパスになる）

/////////////////////////////////////////
//
//Plus Speed
//
//MNUAL_MODEの時にスピードを変える
/////////////////////////////////////////
void Plus_Speed(uint8_t *packetdata){
       
    if(agz.nowMode == MANUAL_MODE){ 
        agz.changeSpeed(packetdata);
    }
    
}

/////////////////////////////////////////
//
//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(collect_base_address),
                                        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;
    
    if (myGPS->fix) {
        
        agz.nowStatus = GPS_AVAIL;
        agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
        
        if(flag < COUNTER_MAX){
            flag++; 
        }
        if(flag == 5){
            x_prev = agz.get_agzPoint_lati();
            y_prev = agz.get_agzPoint_longi();
        }
            
        if(flag >= 6){
            if(abs(x_prev - agz.get_agzPoint_lati()) < ERROR_RANGE && abs(y_prev - agz.get_agzPoint_longi()) < ERROR_RANGE){
                Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi());
                change = 1;
            }
            else{
                change = 0;
            }
        /*    fp = fopen(filename, "w");
            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());
                */
            
        } 
            
                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;
    
} 

/////////////////////////////////////////
//
//New Mode
//
/////////////////////////////////////////

void New_Mode(uint8_t *packetdata){
  
  //bool result;
   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と自分)
//2015/05/29
//外側判定と処理の実装
//内側判定：シーケンス動作
//外側判定：10秒間バック、3秒間旋回を行い、その後シーケンス動作へ
/////////////////////////////////////////

void auto_Move(){
 
 bool result=false; // 毎回の内外判定の結果を格納
 bool out_flag = false; // 外側処理の実行フラグ
 const int sequenceTime[4] = {30000, 31000, 34000, 34200};
 const int outSequenceTime[4] = {10000, 11000, 14000, 14200};
 
 
 //内外判定を行うtrueの時は外 falseの時は内側にいる
 result = agz.gpsAuto(collect_base_address);
 
 //agz.set_agzAutoGPS();
 //agz.set_agzKalmanGPS();
 
 if(out_flag == false && result == true){
    out_flag = true;
    agz.Out_Timer.reset();
    }
 
 if(out_flag == false){
    if(agz.Move_Timer.read_ms() < sequenceTime[0]){
          agz.control_Motor(0); //straight
        }
    if(agz.Move_Timer.read_ms() > sequenceTime[0] && agz.Move_Timer.read_ms() < sequenceTime[1]){
          agz.control_Motor(1);
        }
    if(agz.Move_Timer.read_ms() > sequenceTime[1] && agz.Move_Timer.read_ms() < sequenceTime[2]){
          agz.control_Motor(2); //Turn Right
        }   
    if(agz.Move_Timer.read_ms() > sequenceTime[2] && agz.Move_Timer.read_ms() < sequenceTime[3]){
          agz.control_Motor(1);
        }
    if(agz.Move_Timer.read_ms() > sequenceTime[3]){
          agz.Move_Timer.reset();
        }
    } 
 if(out_flag == true){
    if(agz.Out_Timer.read_ms() < outSequenceTime[0]){
           agz.control_Motor(3); //back
        }
    if(agz.Out_Timer.read_ms() > outSequenceTime[0] && agz.Out_Timer.read_ms() < outSequenceTime[1]){
           agz.control_Motor(1);
        }
    if(agz.Out_Timer.read_ms() > outSequenceTime[1] && agz.Out_Timer.read_ms() < outSequenceTime[2]){
           agz.control_Motor(2); //Turn Right
        }   
    if(agz.Out_Timer.read_ms() > outSequenceTime[2] && agz.Out_Timer.read_ms() < outSequenceTime[3]){
           agz.control_Motor(1);
        }
    if(agz.Out_Timer.read_ms() > outSequenceTime[3]){
           agz.Out_Timer.reset();
           agz.Move_Timer.reset();
           out_flag = false;
        }
  }
}

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+s2_Q)/(s2x_prev+s2_R+s2_Q);
  Ky = (s2y_prev+s2_Q)/(s2y_prev+s2_R+s2_Q);
  //estimate
  x_cur = x_prev + Kx*(zx-x_prev);
  y_cur = y_prev + Ky*(zy-y_prev);
  //calc sigma
  s2x_cur = (1-Kx)*(s2x_prev+s2_Q);
  s2y_cur = (1-Ky)*(s2y_prev+s2_Q);

}

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);
    
}

/////////////////////////////////////////
//
//Main Processing
//
/////////////////////////////////////////
int main() {
    //start up time
    wait(3);
    //set pc frequency to 57600bps 
    pc.baud(PC_BAUD_RATE); 
    //set xbee frequency to 57600bps
    xbee.begin(XBEE_BAUD_RATE);    
        

    //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 = 100; //refresh time in ms
    int count = 0;
    
    
    
    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();
    agz.Out_Timer.start();
    collect_Timer.start();
    printf("start\n");
    
    
    while (true) {
        
        //Check Xbee Buffer Available
        xbee.readPacket();
            
        if (xbee.getResponse().isAvailable()) {
            xbee.getResponse().getZBRxResponse(zbRx);
            uint8_t *buf = zbRx.getFrameData();
                
            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 
                switch(Command_type){
                    //Get Request command
                    case MANUAL:{
                        Plus_Speed(buf);
                        break;
                    }
                    case STATUS_REQUEST:{
                        Send_Status(SenderIDc); 
                        printf("%c\n", SenderIDc);                  
                        break;          
                    }
                    case CHANGE_MODE:{
                        New_Mode(buf);
                        break;
                    }
                    case RECEIVE_STATUS:{
                        Get_Status(SenderIDc,buf1);
                        break;
                    }
                    default:{
                        break;
                    }
                }//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_base_address[collect_flag]);
            
            collect_flag++;

            if(collect_flag == 4){
                collect_flag = 0;
            }    
         } 
            
        if(agz.nowMode == AUTO_GPS_MODE && auto_Timer.read_ms() >= auto_Time){
           auto_Timer.reset();
           auto_Move(); 
        }
        
    }
    
}