File content as of revision 5:9f9cf3912fda:

#include "mbed.h"
#include "getGPS.h"
#include "us015.h"
#include "HMC6352.h"
#include "Math.h"

US015 hs(D2, D3);
DigitalOut Ultra(D2);
GPS gps(D1, D0);
HMC6352 compass(D4, D5);
Serial pc(USBTX, USBRX);
double GPS_x, GPS_y;  // 現在地の座標
double next_CP_x, next_CP_y;
double CPs_x[100];  // = [];  //CPリスト(x座標)
double CPs_y[100];  // = [];  // CPリスト(y座標)
double *pGPS_x = &GPS_x, *pGPS_y = &GPS_y;
double theta;
double delta;

int FrontGet() 
{
        Ultra = 1;  //超音波on
        hs.TrigerOut();
        wait(1);
        int distance;
        distance = hs.GetDistance();
        pc.printf("distance=%d\r\n", distance);  //距離出力
        Ultra=0;//超音波off
        
        if(distance < 200) {
            return 1;
        }
        else {
            return 0;
        }
}

int catchGPS()
{
    pc.printf("GPS Start\r\n");
    /* 1秒ごとに現在地を取得してターミナル出力 */
    while(1) {
        if(gps.getgps()) {  //現在地取得
            pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
            *pGPS_x = gps.latitude;
            *pGPS_y = gps.longitude;
            break;
        }
        else {
            pc.printf("No data\r\n");//データ取得に失敗した場合
            wait(1);
        }
    }
    return 0;
}


double AngleGet() 
{
    double angle;
    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    angle = compass.sample() / 10;

    double next_x;
    double next_y;
    double theta;
    double delta;
    //if(gps.getgps()) {
    pc.printf("%f, %f\r\n", 0, 0);  //gps.latitude=0, gps.longitude=0
    next_x = 0;
    next_y = 100;
    theta = atan(next_x - 0 / next_y - 0) * 180 / M_PI;
    printf("theta=%f\r\n", theta);
    if(theta >= 0) {
        delta = angle - theta;
    }
    else {
        theta = theta + 360;
        delta = angle - theta;
    }
    printf("%f-%f=%f\r\n", angle, theta, delta); 
    //}
    /*
    else {
            pc.printf("No data\r\n");//データ取得に失敗した場合
            wait(1);
    }
    */
    wait(2);
    return delta;
}
/*
    double angle;
    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    angle = compass.sample() / 10;
            
    if(gps.getgps()) {
    pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
    next_CP_x = gps.latitude;
    next_CP_y = gps.longitude;
    theta = atan(next_CP_x - 0 / next_CP_y - 0) * 180 / M_PI;
    printf("%f", theta);
    delta = theta - angle;
    printf("%f-%f=%f\r\n", theta, angle, delta); 
    return delta;
    }
    else {
            pc.printf("No data\r\n");//データ取得に失敗した場合
            wait(1);
    }
}
*/