File content as of revision 24:d5fe0afc4565:

/**********************************************/
//更新情報

/**********************************************/

#include "mbed.h"
#include "XBee.h"
#include "MBed_Adafruit_GPS.h"
//#include "AigamozuControlPackets.h"
#include "agzIDLIST.h"
#include "aigamozuSetting.h"
#include "HMC5883L.h"
#include "VNH5019.h"
#include "cansat.h"
#include "math.h"
#include "BME280.h"

#define SIGMA_MIN 0.0001

#define STOP 0 //compass initial
#define CAL 1 //compass calibration
#define RUN 2 //compass run

/////////////////////////////////////////
//
//Pin Setting
//
/////////////////////////////////////////
VNH5019 agz_motorShield(p23,p22,p25,p21,p24,p26);
//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
Serial xbee(p13,p14);
ZBRxResponse zbRx = ZBRxResponse();

// compass
HMC5883L compass(p9, p10);

//set up GPS module

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

//set up for tempratures...
#if defined(TARGET_LPC1768)
BME280 sensor(p9, p10);
#else
BME280 sensor(I2C_SDA, I2C_SCL);
#endif

DigitalIn short_in(p29);
DigitalOut short_out(p30);
DigitalInOut nic(p5);

 int short_flag = 0;
 int running_flag = 0;

Timer pressure_Timer;
const int pressure_Time = 10000;
Timer compass_Timer;
const int compass_Time = 50;
Timer running_Timer;
const int running_Time = 50;
Timer parachute_Timer;
const int parachute_Time = 30000;
/////////////////////////////////////////
//
//For Compass data
//
/////////////////////////////////////////
Ticker compass_interrupt;
double heading0 = 0.0;    
double heading1 = 0.0;
double heading2 = 0.0;
double heading3 = 0.0;
double headingLPF = 0.0;
double initHeading;
double tgtHeading;
double preHeading = 0.0;
 
int maxX, minX, maxY, minY;
const int ofsX = -102;                   //calibration x
const int ofsY = -381;                   //calibration y

    int16_t raw[3];

/////////////////////////////////////////
//
//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;

int mode = -1; //ロボットのモード
double target_x = 139.987305,target_y = 40.142655;
double  goal_Pressure = 0.0, goal_Temperature,goal_Humidity; //地表地点の気圧、気温、湿度

Timer sep_Timer;
const int sep_Time = 3000; //seperate time in ms
int fall_flag = 0;

int log_number = 0;


/////////////////////////////////////////
//
//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){ //カルマンフィルターにGPSの値をかける

    zx = Latitude;
    zy = Longitude;

    calc_Kalman();
    
    //更新
    x_prev = x_cur;
    y_prev = y_cur;
    s2x_prev = s2x_cur;
    s2y_prev = s2y_cur;
    
    //robotK\x,robotK_yに格納する
    cansat.set_robotKalman_x(x_cur);
    cansat.set_robotKalman_y(y_cur);
}

/////////////////////////////////////////
//
//Get GPS function
//
/////////////////////////////////////////

void Get_GPS(Adafruit_GPS *myGPS){ //GPSが取得できている時0を返し、取得できていない時1を返す
    static int flag = 0;
    
    if (myGPS->fix) {
        
        cansat.nowStatus = GPS_AVAIL;
        cansat.set_robot_y((double)myGPS->latitudeH + (double)(myGPS->latitudeL / 10000.0 / 60.0));
        cansat.set_robot_x((double)myGPS->longitudeH +(double)(myGPS->longitudeL / 10000.0 / 60.0));
    
        
        if(flag < COUNTER_MAX){
            flag++; 
        }
        if(flag == 5){
            x_prev = cansat.get_robot_x();
            y_prev = cansat.get_robot_y();
        }
            
        if(flag >= 6){
            if(abs(x_prev - cansat.get_robot_x()) < ERROR_RANGE && abs(y_prev - cansat.get_robot_y()) < ERROR_RANGE){
                Kalman(cansat.get_robot_x(), cansat.get_robot_y());
                change = 1;
            }
            else{
                change = 0;
            }            
                //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 cansat.nowStatus = GPS_UNAVAIL;
    
} 

//ロボットの動き方
char robot_Action(int robot_angle, int target_angle) {
    
    int n, t, r;
    t = target_angle;
    r = robot_angle;
    n = r - t;
    if(n<0) n *= -1;
    
    
    if(t==r) {
        return 'f';     // target_angleとrobot_angleが等しいときは前進
    } else if(n == 4) {
        return 'r';     // target_angleとrobot_angleが正反対にあるときは右回転
    } else if(n > 0 && n < 4) {
        if(t > r)
            return 'r'; // robot_angleから見て右方向にtarget_angleがあるときに右回転
        else
            return 'l'; // robot_angleから見て左方向にtarget_angleがあるときに左回転
    } else if(n > 4 && n < 8){
        if(t > r)
            return 'l'; // robot_angleから見て左方向にtarget_angleがあるときに左回
        else
            return 'r'; // robot_angleから見て右方向にtarget_angleがあるときに右回転
    } else
        return 'b';
    
}

//対象とロボットの角度
double robot_compass(double robot_x, double robot_y) {
    double angle = 0;
    
    if(robot_x==0&&robot_y>0)
        return 0;
    else if(robot_x>0&&robot_y==0) //東
        return 90;
    else if(robot_x==0&&robot_y<0) //南
        return 180;
    else if(robot_x<0&&robot_y==0) //西
        return 270;
    else if(robot_x>=0&&robot_y>=0) { //北東
        if(robot_x<=robot_y)
            angle = atan2(robot_x, robot_y);
        else
            angle = (M_PI/2) - atan2(robot_y, robot_x);
        return angle * 180.0 / M_PI;
    } else if(robot_x>=0&&robot_y<0) { //南東
        if(robot_x>abs(robot_y)){
            angle = (M_PI/2) - atan2(abs(robot_y), robot_x);
        }
        else{
            angle = atan2(abs(robot_y), robot_x);
        }
        return angle * 180.0 / M_PI + 90;
    } else if(robot_x<0&&robot_y<0) { //南西
        if(abs(robot_x)<abs(robot_y)){
            angle = atan2(abs(robot_x), abs(robot_y));
        }
        else{
            angle = (M_PI/2) - atan2(abs(robot_y), abs(robot_x));
        }
        return angle * 180.0 / M_PI + 180;
    } else if(robot_x<0&&robot_y>=0) { //北西
        if(abs(robot_x)>robot_y){
            angle = (M_PI/2) - atan2(robot_y, abs(robot_x));
        }
        else{
            angle = atan2(abs(robot_x), robot_y);
        }
        return 360 - angle * 180.0 / M_PI;
    }
    
    return -1;
}

int calc_angle(double c){
    if(c > 337.5 ||(c >=0 && c < 22.5)) return 0;   // 北
    else if(c >= 22.5 && c < 67.5) return 1;        // 北東
    else if(c >= 67.5 && c < 112.5) return 2;       // 東
    else if(c >= 112.5 && c < 157.5) return 3;      // 南東
    else if(c >= 157.5 && c < 202.5) return 4;      // 南
    else if(c >= 202.5 && c < 247.5) return 5;      // 南西
    else if(c >= 247.5 && c < 292.5) return 6;      // 西
    else if(c >= 292.5 && c < 337.5) return 7;      // 北西
    else return 8;
}  

void Compass_intrpt(){    
   
    compass.getXYZ(raw);
    double heading = atan2(static_cast<double>(raw[2]-ofsY), static_cast<double>(raw[0]-ofsX)); //y=raw[2]
    if(heading < 0)heading += 2*M_PI;
    if(heading > 2*M_PI)heading -= 2*M_PI;
    heading3 = heading2;
    heading2 = heading1;
    heading1 = heading0;
    heading0 = heading;
    headingLPF = (heading0 + heading1 + heading2 + heading3)/4.0; //low pass filter

    headingLPF = headingLPF * 180.0 / M_PI;
    //  pc.printf("heading=%f\r\n",headingLPF);

    cansat.set_compass(raw[0], raw[2], raw[1], headingLPF);
   cansat.set_robot_angle(calc_angle(cansat.get_compass_angle()));
}

    
/******************************
スタンバイモード
******************************/
void standby(){
    
    cansat.control_Motor(1, cansat.get_speed()); //モーターに電流を流さない
    cansat.set_temperature(sensor.getTemperature()); //気温の設定
    cansat.set_pressure(sensor.getPressure()); //気圧の設定
    cansat.set_humidity(sensor.getHumidity()); //湿度の設定
    
    if(cansat.get_pressure() > goal_Pressure){ //最も高い気圧の値をgoal_Pressureに格納する
        goal_Pressure = cansat.get_pressure();
    }
    
    if(!short_in){ //ショートピンが抜けた時
        xbee.printf("change mode: falling\n");
        short_flag++;
    }
    else{
        if(running_Timer.read_ms() >= running_Time){ //50msecごとに動作
            running_Timer.reset();
            xbee.printf("stand by, %f, %f\n", goal_Pressure, cansat.get_pressure()); //ゴールと現在の気圧をプリントする
        }
    }
    if(short_flag >= 5){ //short_flagが5以上の場合
        mode = 1; //落下モードに変更
        goal_Pressure = goal_Pressure - 0.5; //気圧のゴール判定をgoal_Pressureよりも0.5低くする
        pressure_Timer.start();
        parachute_Timer.start();
    }
    
}

/******************************
落下モード
******************************/
void falling_print(){
    
    xbee.printf("%d: %04.2f hPa\n", log_number, cansat.get_pressure()); //現在の気圧の値をプリントする

    log_number++;
    
}

void falling(){
    
    cansat.set_temperature(sensor.getTemperature());    // 取得した温度を変数に格納
    cansat.set_pressure(sensor.getPressure());          // 取得した気圧を変数に格納
    cansat.set_humidity(sensor.getHumidity());          // 取得した湿度を変数に格納
    
    falling_print();
    
    if(cansat.get_pressure() >= goal_Pressure){         // 取得した気圧と閾値を比較
        if(fall_flag == 0){
            nic = 1;    // ニクロム線を切る
            fall_flag = 1;
            sep_Timer.reset();
        }
    }
    
    if(fall_flag == 1 && pressure_Timer.read_ms() >= pressure_Time){    // 取得した気圧が閾値より大きく、落下してから10秒以上経過しているとき
        mode = 2;   // 走行モードに移る
        xbee.printf("my pressure is high!\n");
        xbee.printf("remove the parachute\n");
        xbee.printf("change mode: running\n");
    }
    
   if(fall_flag == 0 && parachute_Timer.read_ms() >= parachute_Time){   // 取得した気圧が閾値より小さく、落下してから30秒以上経過しているとき
        mode = 2;   // 走行モードに移る
        nic = 1;    // ニクロム線に電流を流す
        xbee.printf("Time out!\n");
        xbee.printf("remove the parachute\n");
        wait_ms(3000); //3秒間停止
        nic = 0; //ニクロム線に電流を流し終わる
        xbee.printf("change mode: running\n");

    }    


}

/******************************
走行モード
******************************/

void running_print(){
    
    if(running_flag <= 20){ //走り初めの時
        xbee.printf("%d: %f, %f, %d\n", log_number, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed()); //生のGPSデータなどをログとしてプリントする
    }
    else{
        xbee.printf("%d: %f, %f, %d\n", log_number, cansat.get_robotKalman_x(), cansat.get_robotKalman_y(), cansat.get_speed()); //カルマンフィルターを通したGPSのデータなどをログとしてプリントする
    }       
    xbee.printf("%c, %f\n", cansat.motor_command, cansat.get_compass_angle()); //モーターのコマンド、コンパスから出た方向
    xbee.printf("%d, %d\n",cansat.get_robot_angle(), cansat.get_target_angle()); //ロボットとターゲットの角度
 //   double y1 = cansat.get_target_y();
  //  double y2 = cansat.get_robot_y();
  //  double x1 = cansat.get_target_x();
  //  double x2 = cansat.get_robot_x();
  //  xbee.printf("taret_angle:%f",robot_compass(x1-x2, y1-y2));

    log_number++;
    
}

void running(){
    
  //  double r = 6378.137;
    double y1 = cansat.get_target_y();
    double y2 = cansat.get_robot_y();
    double x1 = cansat.get_target_x();
    double x2 = cansat.get_robot_x();
    double x_k2 = cansat.get_robotKalman_x();
    double y_k2 = cansat.get_robotKalman_y();
    double dx = x2 - x1;
    double dy = y2 - y1;
    double dx_k = x_k2 - x1;
    double dy_k = y_k2 - y1;
    double y_sub;
    double x_sub;
  
    if(running_flag < 20){ //走り始めは生のGPSの値を用いて制御する
        y_sub = dy*111135.0;
        x_sub = dx*91191.0;
        cansat.set_target_distance(sqrt(y_sub*y_sub + x_sub*x_sub)); //ゴール地点との距離を決める
        cansat.set_target_angle(calc_angle(robot_compass(x1-x2, y1-y2))); //ロボットが行く方向を求める
        running_flag++;
    }
    else{ //カルマンフィルターの値を用いて制御する
        y_sub = dy_k*111135.0;
        x_sub = dx_k*91191.0;
        cansat.set_target_distance(sqrt(y_sub*y_sub + x_sub*x_sub));
        cansat.set_target_angle(calc_angle(robot_compass(x1-x_k2, y1-y_k2)));
    } 
    
    if(cansat.get_target_distance() <= 10) cansat.set_speed(64); //ゴール地点との距離が10m以内の時,pwm:64
    else if(cansat.get_target_distance() <= 20 && cansat.get_target_distance() > 10) cansat.set_speed(128); //ゴール地点との距離が10mより大きく20mm以内の時,pwm:128
    else cansat.set_speed(255); //ゴール地点との距離が20mより大きい時,pwm:255
    
    if(fall_flag == 1 && sep_Timer.read_ms() >= sep_Time){ //落下後30秒がたった時ニクロム線の電源を切る
        nic = 0;
    }
    if(compass_Timer.read_ms() >= compass_Time){ //50msecごとに動作する
        compass_Timer.reset();
        if(cansat.get_compass_z() < 0) { //ひっくり返っているとき
            cansat.control_Motor(0, cansat.get_speed()); //前進する
        } else {
            cansat.motor_command = robot_Action(cansat.get_robot_angle(), cansat.get_target_angle()); //移動するときの方向を決める
            switch(cansat.motor_command) {
                case 'f': //前進
                    cansat.control_Motor(0, cansat.get_speed());
                    break;
                case 'l': //左
                    cansat.control_Motor(2, cansat.get_speed());
                    break;
                case 'r': //右
                    cansat.control_Motor(3, cansat.get_speed());
                    break;
                case 'b': //後退
                    cansat.control_Motor(4, cansat.get_speed());
                    break;
            }
        }
    }
    
    running_print(); //ログをプリントする
    
    if(cansat.get_target_distance() <= 1){ //ゴール地点との距離が1m以下のとき
        mode = 100; //停止モードに移行
        xbee.printf("change mode: stopping\n");
        xbee.printf("it is goal point.\n");
    }
}

/******************************
停止モード
******************************/
void stopping(){
            
            cansat.control_Motor(1, cansat.get_speed()); //モーターの動きを停止する
            
}

/////////////////////////////////////////
//
//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);    
    xbee.baud(57600);    
    //Compass setting
    compass.init();

    //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
    int count = 0;
    Timer compass_refresh_Timer;
    const int compass_refresh_Time = 500;

    myGPS.begin(GPS_BAUD_RATE); 
    
    //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();
    compass_Timer.start();
    running_Timer.start(); 
    sep_Timer.start();
    compass_refresh_Timer.start();
    cansat.set_target(target_x, target_y);
   // wait_ms(10000);

    //compass_interrupt.attach(&Compass_intrpt, 0.5);


    printf("start\n");
    xbee.printf("target: %f, %f\n",cansat.get_target_x(), cansat.get_target_y());
    //int mode = -1;
    nic.output();
    nic = 0; //ニクロム線に電流を流さない
    
    short_out = 1; //ショートピンに電流を流す
    wait_ms(1000);
//    if(short_out){
//       xbee.printf("HIGH\n");
//    }
    
    while (true) {
        
        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);
            //log++;
 /*           xbee.printf("%d times, x:%f, y:%f, speed:%d\n", log, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed());
            xbee.printf("moter command: %c\n", cansat.motor_command);
            xbee.printf("robot_angle:%d, target_angle:%d, robot_compass:%f, %04.2f hPa\n",cansat.get_robot_angle(), cansat.get_target_angle(),  cansat.get_compass_angle(), cansat.get_pressure());
 */
        }
        if(compass_refresh_Timer.read_ms() >= compass_refresh_Time){ //コンパスの値を50msecごとに更新する
            Compass_intrpt();
            compass_refresh_Timer.reset();
        }

        switch(mode){
            //スタートモード：パラシュートが開くまではこのモードを実行
            case -1:
                standby();
            break;
            //落下モード：落下時はこのモード。気圧計または、時間でロボットとパラシュートを分離する
            case 1:
                falling();
            break;
            //走行モード：ターゲットにむかって走行を行う
            case 2:
                if (running_Timer.read_ms() >= running_Time) {
                    running_Timer.reset();
                    running();
                }
            break;
            //停止モード：ターゲット
            case 100:
                stopping();
            break;
        }
        
   /*     while(1){
            
            printf("compass x : %i, compass y : %i, compass z : %i\n", raw[0], raw[1], raw[2]);
            printf("set compass x : %i, set compass y : %i, set compass z : %i\n", cansat.get_compass_x(), cansat.get_compass_y(), cansat.get_compass_z());
            printf("compass angle : %f\n", headingLPF);
            printf("set compass angle : %f\n", cansat.get_compass_angle());
            printf("robot angle : %d\n", calc_angle(headingLPF));
            printf("set robot angle : %d\n", cansat.get_robot_angle());
        }
*/
            
    }
    
}