File content as of revision 0:e7b7def631c2:

#include "mbed.h"
#include "BMP180.h"
#include "getGPS.h"
#include "Servo.h"
#include "calculate.h"
#include "MPU9250.h"
#define PIN_SDA D4
#define PIN_SCL D5
#define STOPTIME 1
#define GOTIME 5
#define ROTATETIME 1


Serial xbee(PA_9, PA_10);
GPS gps(PA_2,PA_3);
DigitalOut Nichrome(A6);
BMP180 bmp180(PIN_SDA,PIN_SCL);
MPU9250 mpu9250(D4, D5);
Servo myservo1(D7);    
Servo myservo2(A3);    
Servo myservo3(A1); 
Servo myservo4(D12);
Servo myservo5(D10);
Servo myservo6(A5); 
double bias_la=0,bias_lo=0;

int landing_judgement(){
    bmp180.Initialize(27,0);
    int i,n1,n2,landing_judgement;
    float dt,dp,h,dp0,a,b,dt_ave,dp_ave;
    for(i=0;i<15;i++){
        if(bmp180.ReadData(&dt,&dp)){
            a = a + dp;
            b = b + dt;
            n1 = n1 + 1;
            n2 = n2 + 1;
            wait(1);
            }
        }
    
    dp_ave = a / n1;
    dt_ave = b / n2;    
    dp0 = calculate_dp0(dp_ave,dt_ave);
    int x,y;
    while(x<10){
            if(bmp180.ReadData(&dt,&dp)){
                h = calculate_h(dp0,dp,dt);
                if(h >= 30){
                x = x + 1;
                    }
                wait(1);
            }
    }
    //10秒以上高度30mにいた場合離陸判定
    
    wait(10);
      
    while(y<10){
            if(bmp180.ReadData(&dt,&dp)){
                h = calculate_h(dp0,dp,dt); 
                if(h <= 10){
                y = y + 1;
                    }
                wait(1);
            }
    }
    
    wait(5);
    
    landing_judgement = landing_judgement + 1;
    return landing_judgement;
    
    //離陸判定後、10秒以上高度10m以下にいた場合着地判定
    
}


//止まる
void stop()
    {
        myservo1 = 0.5;
        myservo2 = 0.5;
        myservo3 = 0.5;
        myservo4 = 0.5;
        myservo5 = 0.5;
        myservo6 = 0.5;
        wait(STOPTIME);
    }

//前進
void move_forward()
{
    myservo1 = 0;
    myservo2 = 0;
    myservo3 = 0;
    myservo4 = 0;
    myservo5 = 0;
    myservo6 = 0;
    wait(GOTIME);
}

//後退
void move_backward()
{
    myservo1 = 1;
    myservo2 = 1;
    myservo3 = 1;
    myservo4 = 1;
    myservo5 = 1;
    myservo6 = 1;
    wait(GOTIME);
}

//右に曲がる
void turn_right()
{
    myservo1 = 1;
    myservo2 = 1;
    myservo3 = 1;
    myservo4 = 0;
    myservo5 = 0;
    myservo6 = 0;
    wait(ROTATETIME);
}

//左に曲がる
void turn_left()
{
    myservo1 = 0;
    myservo2 = 0;
    myservo3 = 0;
    myservo4 = 1;
    myservo5 = 1;
    myservo6 = 1;
    wait(ROTATETIME);
}

//倒れているときの処理
void wakeup(int time)
{
    int i;
    for(i=1;i<=time;i++)
    {
    move_forward()
    move_backward()
    turn_right()
    turn_left()
    }
}



float calculation_sita(float x_0,float y_0,float x_1,float y_1,float x,float y){
    //x,yは地磁気センサの値で北の方角の角度を90度、東の方角を0度とする。
    float sita_0 = atan((y_0 - y_1)/(x_0 - x_1));  //目的地の角度
    
    if(y_0 - y_1 > 0 && x_0 - x_1 < 0){
        sita_0 = sita_0 - 180;
    }
    if(y_0 - y_1 < 0 && x_0 - x_1 < 0){
        sita_0 = sita_0 + 180;
    }
    
    float Omag_x=0,Omag_y=0; //地磁気センサのxy平面が描く円の中点
    float sita_1 = atan((y - Omag_y)/(x- Omag_x));　//CanSatの角度 
    
    if(y - Omag_y > 0 && x- Omag_x < 0){
        sita_1 = sita_1 - 180;
    }
    if(y - Omag_y < 0 && x - Omag_x < 0){
        sita_1 = sita_1 + 180;
    }
    float sita
    sita= sita_0 - sita_1;  //CanSatから見た目的地の角度
    return sita
}    
      
    
    
int parachute_separation()
{
        Nichrome=1;
        wait(10);
        Nichrome=0;
    return 0;
    }

int end_flag=0;
double bias_la=0,bias_lo=0;



int main(void)
{   
int land_judgement_1=0,sep_judge=1;
//着地判定
while(1)
{
    land_judgement=landing_judgement()
    if(land_judgement_1==1)
    {
xbee.printf("landing\r\n");  
        break;
    }  
}
wait(30);
//パラシュート切り離し
while(1)
{
    sep_judge==1;
    sep_judge=parachute_separation();
    if(sep_judge==0)
    {
        xbee.printf("did separation\r\n");
        xbee.printf("separation complleted?\ty/n")
        int received_data = xbee.getc();
        if(received_data==89||received_data==121)
        {   
            xbee.printf("separation complieted\n\r");
            break;
        }
        if(received_data==78||received_data==110)
        {   
            xbee.printf("try again now\n\r");
        }
    }
}
//中間座標の取得
double midpoint_latitude,midpoint_longtitude;
xbee.printf("\r\n-----------------------\r\n");
xbee.printf("midpoint latitude:");
xbee.scanf("%lf",&midpoint_latitude);
xbee.printf("\r\nmidpoint longtitude:");
xbee.scanf("%lf",&midpoint_longtitude);
xbee.printf("\r\n-----------------------\r\n");
//倒れているかの判定
int i;
gps
for(i=1;i<=2;i++)
{
    move_forward();
}


move_forward();

//倒れているときの処理
wakeup(2)


#
}