File content as of revision 3:b48cac06f913:

#include "mbed.h"
#include "math.h"

#include "PQLPS22HB.h"
#include "mpu9250_i2c.h"
#include "GPS_interrupt.h"
#include "QQQCAM.h"

#define ACC_RANGE _16G
#define GYRO_RANGE _2000DPS
#define SEA_LEVEL_PRESS 1029.3//投下前に設定
#define CURRENT_LOCATION_PRESS 1020.624268//投下前に設定
#define CURRENT_LOCATION_TEMP 10.650000//投下前に設定
#define LAT_GOAL 33.595085//投下前に計測
#define LON_GOAL 130.217850//投下前に計測
#define PI 3.14159265358979323846
#define R 6378137

Serial pc(USBTX,USBRX, 115200);
Serial xbee(p28, p27, 115200);
Serial openlog(p13, p14, 115200);
Serial gps_serial(p13, p14, 115200);
Serial xbee_raspi(p28, p27, 115200);
I2C i2c(p9, p10);
DigitalIn flightpin(p8);
DigitalOut pin(p22);

//切削基板・発注基板共用
PwmOut motor_left26(p26);//左IN2
PwmOut motor_left25(p25);//左IN1
PwmOut motor_right24(p24);//右IN1
PwmOut motor_right23(p23);//右IN2


LPS22HB lps22hb(i2c, LPS22HB::SA0_LOW);
mpu9250 mpu9250(i2c,AD0_HIGH);
GPS_interrupt gps(&gps_serial);

QQQCAM cam(xbee_raspi);

Timer timer;
Ticker tick_data;
Ticker tick_data_status;

void Phase1();
void Phase2();
void Phase3();
void Phase4();
void Phase5();
void rasp_data_received();
void on_data_received();
void GetData();
void GetStatus();
void SetSensor();
void SetMpu9250();
void GetMpu9250();
void SetLps22hb();
void GetLps22hb();
void SetGPS();
void GetGPS();
void CalculateAltitude();
float CalcAltitude_02();
void FlightPin();
void CutNichrome(float nich_wait);
void NichromeOn();
void NichromeOff();
void CalcDirection(float lat, float lon, float lat_f, float lon_f);
void CalcDistance(float lat, float lon);
void MotorFront(float f_left,float f_right,float f_wait);
void MotorBack(float f_left,float f_right,float f_wait);
void MotorStop();

int cansat_phase = 1;
int driving_mode = 0;//1:running, 2:stack
int direction_mode = 0;//-1:right, 1:left, 2:straight
int color_mode = 0;//0:>0.001, 1:>0.1, 2:0.1> 
int cansat_goal = 0;
float phase5_time = 0;
bool mpu9250_test;
bool mag_mpu9250_test;
float imu[6], mag[3];
float press, temp;
float altitude_ground, altitude, altitude_02;
float lat, lon, height;
bool nich_status = false;
float cansat_direction;
float cansat_distance;
float lat_0, lon_0;
int count_loop = 1;
int count_data = 1;
float percent;

int main()
{
    flightpin.mode(PullUp);
    cansat_phase = 1;
    Phase1();
    Phase2();
    //Phase3();
    cansat_phase = 4;
    Phase4();
    //rasp_data_received();
    
}

void Phase1()
{
    //xbee_raspi.attach(on_data_received, Serial::RxIrq);
    pc.printf("Phase_%d\r\n", cansat_phase);
    //xbee.printf("Phase_%d\r\n", cansat_phase);
    SetSensor();
    tick_data.attach(&GetData, 1.0);
    tick_data_status.attach(&GetStatus, 1.0);
    cansat_phase = 2;
    pc.printf("SetSensor finished!\r\n");
    //xbee.printf("SetSensor finished!\r\n");
}

void Phase2() //フライトピンの検知
{
    pc.printf("Phese_%d\r\n", cansat_phase);
    //xbee.printf("Phese_%d\r\n", cansat_phase);
    timer.start();
    while(cansat_phase == 2) {
        if(flightpin == 1) {
            break;
        }
    }
    cansat_phase = 3;
    pc.printf("FlightPin finished!\r\n");
    //xbee.printf("FlightPin finished!\r\n");
}

void Phase3(){//気圧による
    pc.printf("Phase_%d\r\n", cansat_phase);
    //xbee.printf("Phase_%d\r\n", cansat_phase);
    int count_al = 0;
    while(cansat_phase == 3) {
        if(CalcAltitude_02() >= 5){
            //xbee.printf("%d\r\n", count_al);
            //count_al++;
        }else if(CalcAltitude_02() < 5){
            CutNichrome(2.0f);
            break;
        }
    }
    //MotorFront(0.95, 1.0, 5.0);//パラシュートから離れる
    //MotorStop();
    cansat_phase = 4;
    pc.printf("CutNichrome finished!\r\n");
    //xbee.printf("CutNichrome finished!\r\n");
}

/*
void Phase3() //ニクロム線による分離
{
    pc.printf("Phase_%d\r\n", cansat_phase);
    //xbee.printf("Phase_%d\r\n", cansat_phase);
    while(cansat_phase == 3) {
        CutNichrome(2.0f);
        break;
    }
    //MotorFront(0.95, 1.0, 5.0);//パラシュートから離れる
    //MotorStop();
    cansat_phase = 4;
    pc.printf("CutNichrome finished!\r\n");
    //xbee.printf("CutNichrome finished!\r\n");
}
*/

void Phase4(){ //走行中
    pc.printf("Phase_%d\r\n", cansat_phase);
    //xbee.printf("Phase_%d\r\n", cansat_phase);
    //wait(3.0f);
    
    while(1) {
        GetGPS();
        if(lat < 5) {
            pc.printf("GPS Data NG...\r\n");
            pc.printf("lat = %f, lon = %f\r\n",lat, lon);
            wait(3.0f);
        } else {
            break;
        }
    }
    
    GetGPS();
    lat_0 = lat;
    lon_0 = lon;

    MotorFront(1.0, 1.0, 3.0);
    while(cansat_phase == 4) {
        //wait(4.0);
        GetGPS();

        //xbee.printf("lat : %f\tlon : %f\tlat_0 : %f\tlon_0 : %f\t\r\n", lat, lon, lat_0, lon_0);

        CalcDirection(lat, lon, lat_0, lon_0);
        float direc_n = cansat_direction;
        
        if(lat == lat_0){
            driving_mode = 2;
            pc.printf("lat_0 = %f, lon_0 = %f\r\n",lat_0, lon_0);
            pc.printf("lat   = %f, lon   = %f\r\n",lat, lon);
            pc.printf("Stack!!\n");
            MotorBack(1.0, 1.0, 1.0);
            MotorBack(1.0, 0, 1.0);
            MotorBack(0, 1.0, 1.0);
        }else{
            driving_mode = 1;
            //xbee.printf("%f\t\r\n", direc_n);
    
            if(-90 < direc_n  && direc_n <= 0) { //右へ
                direction_mode = -1;
                pc.printf("Go to the Right\r\n");
                pc.printf("lat = %f, lon = %f\r\n",lat, lon);
                //xbee.printf("Go to the Right-90\r\n");
                MotorFront(1.0, 0, 0.25);
    
            } else if(-180 < direc_n && direc_n<= -90) { //右へ
                direction_mode = -1;
                pc.printf("Go to the Right\r\n");
                pc.printf("lat = %f, lon = %f\r\n",lat, lon);
                //xbee.printf("Go to the Right-180\r\n");
                MotorFront(1.0, 0, 0.5);
    
            } else if(-270 < direc_n && direc_n <= -180) { //左へ
                direction_mode = 1;
                pc.printf("Go to the Left\r\n");
                pc.printf("lat = %f, lon = %f\r\n",lat, lon);
                //xbee.printf("Go to the Left-270\r\n");
                MotorFront(0, 1.0, 0.5);
    
            } else if(-360 <= direc_n && direc_n <= -270) { //左へ
                direction_mode = 1;
                pc.printf("Go to the Left\r\n");
                pc.printf("lat = %f, lon = %f\r\n",lat, lon);
                //xbee.printf("Go to the Left-360\r\n");
                MotorFront(0, 1.0, 0.25);
    
            } else if(0 < direc_n && direc_n<= 90) { //左へ
                direction_mode = 1;
                pc.printf("Go to the Left\r\n");
                pc.printf("lat = %f, lon = %f\r\n",lat, lon);
                //xbee.printf("Go to the Left90\r\n");
                MotorFront(0, 1.0, 0.25);
    
            } else if(90 < direc_n && direc_n <= 180) { //左へ
                direction_mode = 1;
                pc.printf("Go to the Left\r\n");
                pc.printf("lat = %f, lon = %f\r\n",lat, lon);
                //xbee.printf("Go to the Left180\r\n");
                MotorFront(0, 1.0, 0.5);
    
            } else if(180 < direc_n && direc_n<= 270) { //右へ
                direction_mode = -1;
                pc.printf("Go to the Right\r\n");
                pc.printf("lat = %f, lon = %f\r\n",lat, lon);
                //xbee.printf("Go to the Right270\r\n");
                MotorFront(1.0, 0, 0.5);
    
            } else if(270 < direc_n && direc_n <=360) { //右へ
                direction_mode = -1;
                pc.printf("Go to the Right\r\n");
                pc.printf("lat = %f, lon = %f\r\n",lat, lon);
                //xbee.printf("Go to the Right360\r\n");
                MotorFront(1.0, 0, 0.25);
            }
        }
        lat_0 = lat;
        lon_0 = lon;

        CalcDistance(lat, lon);

        if(cansat_distance <= 3) {
            driving_mode = 0;
            //xbee.printf("CanSat Goal!");
            MotorStop();
            cansat_phase = 5;
            break;
        } else {
            MotorFront(1.0, 1.0, 3.0);
            MotorStop();

            count_loop = count_loop + 1;
            //xbee.printf("lat : %f\tlon : %f\tlat_0 : %f\tlon_0 : %f\t\r\n", lat, lon, lat_0, lon_0);
        }
    }
}

void Phase5(){
    while(cansat_phase == 5){
        
    }
}

void rasp_data_received(){
    int Goal = 0;
    phase5_time = timer.read();
    
    //int flag = 0;
    /*
    while(flag == 0){
        if(cam.get_rate() == 0){
            //xbee.printf("Rasp Waiting\n");
            
        }else{
            flag = 1;
        }
    }
    */
      
    while(Goal == 0){
        //pc.printf("rate:%.3f\r\n", cam.get_rate());
        //xbee.printf("rate:%.3f\r\n", cam.get_rate());
        
        if(120 <= timer.read() - phase5_time){
            cansat_phase = 6;
            cansat_goal = 1;
            Goal = 1;
            break;
            }
        
        percent = cam.get_rate();
        
        if(percent >= 0.1){
             color_mode = 2;
             //pc.printf("Goal!!!");
             //xbee.printf("Goal!!!");
             //tick_data_status.detach();
             //tick_data.detach();
             cansat_phase = 6;
             cansat_goal = 1;
             Goal = 1;
        }else if(percent >=0.001){
            color_mode = 1;
            direction_mode = 2;
            //pc.printf("Red!!!");
            //xbee.printf("Red!!!");
            
            MotorFront(1.0, 1.0, 1.0);
            //MotorFront(0, 1.0, 0.3);
            //MotorFront(0, 0, 1.0);
        }else{
            color_mode = 0;
            direction_mode = -1;
            
            //pc.printf("No Red!!!");
            //xbee.printf("No Red!!!");
            MotorFront(1.0, 0.5, 1.0);
            //MotorFront(0, 0, 1.0);
        }  
        MotorFront(0.1, 0.1, 2.0);
        //MotorFront(0, 0, 2.0);    
    }
    MotorStop(); 
    wait(3.0);
    tick_data.detach();
    pc.printf("Bye!!!\n");

        
}

/*
void rasp_data_received(){
    int Goal = 0;
    phase5_time = timer.read();
    
    //int flag = 0;
    /*
    while(flag == 0){
        if(cam.get_rate() == 0){
            //xbee.printf("Rasp Waiting\n");
            
        }else{
            flag = 1;
        }
    }
    */
/*      
    while(Goal == 0){
        //pc.printf("rate:%.3f\r\n", cam.get_rate());
        //xbee.printf("rate:%.3f\r\n", cam.get_rate());
        
        if(120 <= timer.read() - phase5_time){
            cansat_phase = 6;
            cansat_goal = 1;
            Goal = 1;
            }
        
        percent = cam.get_rate();
        
        if(percent >= 0.1){
             color_mode = 2;
             //pc.printf("Goal!!!");
             //xbee.printf("Goal!!!");
             //tick_data_status.detach();
             //tick_data.detach();
             cansat_phase = 6;
             cansat_goal = 1;
             Goal = 1;
        }else if(percent >=0.001){
            color_mode = 1;
            direction_mode = 2;
            //pc.printf("Red!!!");
            //xbee.printf("Red!!!");
            
            MotorFront(1.0, 1.0, 1.0);
            //MotorFront(0, 1.0, 0.3);
            //MotorFront(0, 0, 1.0);
        }else{
            color_mode = 0;
            direction_mode = -1;
            
            //pc.printf("No Red!!!");
            //xbee.printf("No Red!!!");
            MotorFront(1.0, 0.5, 1.0);
            //MotorFront(0, 0, 1.0);
        }  
        MotorFront(0.1, 0.1, 2.0);
        //MotorFront(0, 0, 2.0);    
    }
    MotorStop(); 
    pc.printf("Bye!!!\n");   
}


/*
void on_data_received()
{
    char data = xbee_raspi.getc();
    xbee_raspi.printf("%c", data);
    switch(data) {
        case 0x81://ゴール判定
            //xbee_raspi.printf("red rate > 0.1\r\n");
            break;
        case 0x82://ゴール発見
            //xbee_raspi.printf("red rate > 0.01\r\n");
            //xbee.printf("Go Straight\r\n");
            MotorFront(1.0, 1.0, 1.0);
            MotorStop();
            break;
        case 0x83://ゴールが見えない
            //xbee_raspi.printf("cannot find red\r\n");
            //xbee.printf("Go to the Right360\r\n");
            MotorFront(1.0, 0, 1.0);
            MotorStop();
            break;
    }
}
*/
/***************************************************
センサのセットアップ
***************************************************/
void SetSensor()
{
    SetMpu9250();
    SetLps22hb();
    SetGPS();
}

void SetMpu9250() //MPU9250のセットアップ
{
    mpu9250_test = mpu9250.sensorTest();
    mag_mpu9250_test = mpu9250.mag_sensorTest();
    if(mpu9250_test == true) {
        pc.printf("MPU9250 OK!\r\n");
        //xbee.printf("MPU9250 OK!\r\n");
    } else {
        pc.printf("MPU9250 NG...\r\n");
        //xbee.printf("MPU9250 NG...\r\n");
    }
    if(mag_mpu9250_test == true) {
        pc.printf("MPU9250 MAG OK!\r\n");
        //xbee.printf("MPU9250 MAG OK!\r\n");
    } else {
        pc.printf("MPU9250 MAG NG...\r\n");
        //xbee.printf("MPU9250 MAG NG...\r\n");
    }
    mpu9250.setAcc(ACC_RANGE);
    mpu9250.setGyro(GYRO_RANGE);
    mpu9250.setOffset(0.528892327, -0.660206211, 0.757105891, -0.011691362, 0.025688783, 1.087885322, -159.750004, 121.425005, -392.700012);
}

void SetLps22hb() //LPS22HBのセットアップ
{
    lps22hb.begin();
    if(lps22hb.test() == true) {
        pc.printf("LPS22HB OK!\r\n");
        //xbee.printf("LPS22HB OK!\r\n");
    } else {
        pc.printf("LPS22HB NG...\r\n");
        //xbee.printf("LPS22HB NG...\r\n");
    }
}

void SetGPS() //GPSのセットアップ
{
//    gps.changeGPSFreq(10); 入れるとエラーが出る
    pc.printf("GPS OK!\r\n");
    //xbee.printf("GPS OK!\r\n");
}

/***************************************************
データの取得
***************************************************/
void GetData()
{
    GetMpu9250();
    GetLps22hb();
    GetGPS();
    //pc.printf("%7.2f,%d,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%7.2f,%4.1f,%f,%11.7f,%12.7f,%6.2f,%8.2f,%6.2f\r\n", timer.read(), cansat_phase, imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2], press, temp, altitude_02, lat, lon, height, cansat_direction, cansat_distance);
    openlog.printf("%7.2f,%d,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%7.2f,%4.1f,%f,%11.7f,%12.7f,%6.2f,%8.2f,%6.2f\r\n", timer.read(), cansat_phase, imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2], press, temp, altitude_02, lat, lon, height, cansat_direction, cansat_distance);
}

void GetStatus()
{
    xbee.printf("**************************************\r\n");
    xbee.printf("Phase        : %d\r\n", cansat_phase);
    xbee.printf("Altitude_02  : %.3f\r\n", CalcAltitude_02());
    xbee.printf("Driving Mode : %d\r\n", driving_mode);
    xbee.printf("Direction    : %d\r\n", direction_mode);
    xbee.printf("Distance     : %.2f\r\n", cansat_distance);
    xbee.printf("Color Percent: %f\r\n", percent);
    xbee.printf("Color        : %d\r\n", color_mode);
    xbee.printf("Phase5 Time  : %f\r\n", timer.read() - phase5_time);
    xbee.printf("Goal         : %d\r\n", cansat_goal);
    xbee.printf("Counter      : %d\r\n", count_data);
    xbee.printf("**************************************\r\n");
    count_data++;
}

void GetMpu9250()
{
    mpu9250.getAll(imu, mag);
    //pc.printf("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t\r\n",imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2]);
    //xbee.printf("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t\r\n",imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2]);
    //openlog.printf("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t\r\n",imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2]);
}

void GetLps22hb()
{
    lps22hb.read_press(&press);
    lps22hb.read_temp(&temp);
    //pc.printf("%f\t%f\t",press, temp);
}

void GetGPS()
{
    lat = gps.Latitude();
    lon = gps.Longitude();
    height = gps.Height();
    //pc.printf("%f\t%f\t%f\t", lat, lon, height);
    //xbee.printf("%f\t%f\t%f\t", lat, lon, height);
    //openlog.printf("%f\t%f\t%f\t", lat, lon, height);
}

/***************************************************
制御計算
***************************************************/
void CalculateAltitude()
{
    lps22hb.read_press(&press);
    lps22hb.read_temp(&temp);
    altitude_ground = (powf(SEA_LEVEL_PRESS / CURRENT_LOCATION_PRESS, 1/5.257) - 1) * (CURRENT_LOCATION_TEMP + 273.15) / 0.0065;
    altitude = (powf(SEA_LEVEL_PRESS / press, 1/5.257) - 1) * (temp + 273.15) / 0.0065 - altitude_ground;
    pc.printf("%f\t\r\n",altitude);
}

float CalcAltitude_02(){
    lps22hb.read_press(&press);
    lps22hb.read_temp(&temp);
    altitude_02 =  (CURRENT_LOCATION_TEMP + 273.15)/0.0065*(1 - powf(press/CURRENT_LOCATION_PRESS, 287 * 0.0065/9.80665));
    //pc.printf("%f\t%f\t\r\n",press, altitude_02);
    return altitude_02;
}

void FlightPin()
{
    //pc.printf("Hello FlightPin!\r\n");
    while(cansat_phase == 2) {
        if(flightpin == 1) {
            break;
        }
    }
    //pc.printf("Goodbye FlightPin!\r\n");
}

void CutNichrome(float nich_wait)
{
    NichromeOn();
    wait(nich_wait);
    NichromeOff();
}

void NichromeOn()
{
    pin = 1;
    nich_status = true;
}

void NichromeOff()
{
    pin = 0;
    nich_status = false;
}

void CalcDirection(float lat, float lon, float lat_f, float lon_f)  //ヒュベニの公式を使いたい
{
    float dlat_g = (LAT_GOAL - lat) * (PI / 180);
    float dlon_g = (LON_GOAL - lon) * (PI / 180);
    float direc_g = atan2(dlat_g, dlon_g) * (180 / PI);

    float dlat = (lat - lat_f) * (PI / 180);
    float dlon = (lon - lon_f) * (PI / 180);
    float direc = atan2(dlat, dlon) * (180 / PI);

    cansat_direction = direc_g - direc;
}

void CalcDistance(float lat, float lon)
{
    float dlat_distance = R * (LAT_GOAL - lat) * (PI / 180);
    float dlon_distance = R * (LON_GOAL - lon) * (PI / 180) * cos(lat * (PI / 180));
    float distance_squared = dlat_distance * dlat_distance + dlon_distance * dlon_distance;
    cansat_distance = sqrt(distance_squared);
}

/***************************************************
モーター制御
***************************************************/
void MotorFront(float f_left,float f_right,float f_wait)
{
    motor_left26 = 0;//正転
    motor_left25 = f_left;
    motor_right24 = f_right;
    motor_right23 = 0;
    wait(f_wait);
}

void MotorBack(float b_left,float b_right,float b_wait)
{
    motor_left26 = b_left;//逆転
    motor_left25 = 0;
    motor_right24 = 0;
    motor_right23 = b_right;
    wait(b_wait);
}

void MotorStop()
{
    motor_left26 = 0;//ストップ
    motor_left25 = 0;
    motor_right24 = 0;
    motor_right23 = 0;
}