File content as of revision 16:b5a60a976bf7:

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

US015 hs(D2, D3);
DigitalOut Ultra(D2);
GPS gps(D1, D0);
HMC6352 compass(D4, D5);
TB6612 motor_a(D10,D6,D7);  //モータA制御用(pwma,ain1,ain2)
TB6612 motor_b(D11,D8,D9);  //モータB制御用(pwmb,bin1,bin2)
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;
}


void Move(char input_data, float motor_speed) {
    switch (input_data) {
        case '1':  // 停止
            motor_a = 0;
            motor_b = 0;
            break;
        case '2':  // 前進
            motor_a = motor_speed;
            motor_b = motor_speed;
            break;
        case '3':  // 後退
            motor_a = -motor_speed;
            motor_b = -motor_speed;
            break;
        case '4':  // 時計回りに回転
            motor_a = motor_speed;
            motor_b = -motor_speed;
            break;
        case '5':  // 反時計回りに回転
            motor_a = -motor_speed;
            motor_b = motor_speed;
            break;
        case '6':  // Aのみ正転
            motor_a = motor_speed;
            break;
        case '7':  // Bのみ正転
            motor_b = motor_speed;
            break;
        case '8':  // Aのみ逆転
            motor_a = -motor_speed;
            break;
        case '9':  // Bのみ逆転
            motor_b = -motor_speed;
            break;
    }
    pc.printf("input_data=%d\r\n", input_data);
}


void Rotate(double rot){
   // double time = angle; // 試験で調整
    int time = 3;  // 試験用
    if(rot > 0) {
        Move('5', 1);
        wait(time);
        Move('1', 0);  
    }
    else {
        Move('6', 1);
        wait(time);
        Move('1', 0);
    }
}

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

    double theta;
    double delta;

    pc.printf("%f, %f\r\n", gps.latitude, gps.longitude);
    theta = atan2(next_CP_x - gps.latitude , next_CP_y - gps.longitude) * 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); 
    wait(2);
    return delta;
}