File content as of revision 1:6ff5fbdf32bb:

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

US015 hs(D2, D3);
DigitalOut Ultra(D2);
GPS gps(D1, D0);
HMC6352 compass(D4, D5);
ATP3011 talk(D4,D5); // I2C sda scl
TB6612 motor_a(D10,D6,D7);  //モータA制御用(pwma,ain1,ain2)
TB6612 motor_b(D11,D8,D9);  //モータB制御用(pwmb,bin1,bin2)
Serial xbee(A7, A2);

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 theta;
double delta;

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


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


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

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

    double theta;
    double delta;

    xbee.printf("gps.latitude=%f, gps.longitude=%f\r\n", gps.latitude, gps.longitude);
    theta = atan2(next_CP_y - gps.longitude , next_CP_x - gps.latitude) * 180 / 3.14159265359;
    printf("theta=%f\r\n", theta);
    if(theta >= 0) {
        delta = angle - theta;
    }
    else {
        theta = theta + 360;
        delta = angle - theta;
    }    
    if(delta<0){
        delta+=360;
        }
    printf("%f-%f=%f\r\n", angle, theta, delta); 
    wait(2);
    return delta;
}

void Calibration()
{
    xbee.printf("calibration start\r\n");
    compass.setCalibrationMode(0x43);
    Move('4', 0.15);
    xbee.printf("mortor mode:4 speed:0.15\n\r");
    wait(60);
    Move('1', 0);
    xbee.printf("mortor mode:1 speed:0\n\r");
    compass.setCalibrationMode(0x45);
    xbee.printf("calibration end\r\n");
    while(1) {
        if(gps.getgps()) {  //現在地取得
        xbee.printf("lati=%lf\nlong=%lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
        GPS_x = gps.latitude;
        GPS_y = gps.longitude;
        break;
        }
        else {
            xbee.printf("No data\r\n");//データ取得に失敗した場合
            wait(1);
        }
    }

    return;
}

 /*地上局から新情報を送るときはflag=がでてきたらスペースか.を入力
 3秒後ぐらいにmessage=が出てくるので、そしたら新情報を入力*/
void speak()
{
    int timeout_ms=500;
    char mess[100];            
        if(talk.IsActive(timeout_ms)==true){
            xbee.printf("Active\n\rflag=");
            wait(3);
            if(xbee.readable()){
                xbee.printf("\n\rmessage=");
                int i=0;
                    do{
                        mess[i++]= xbee.getc();
                    }
                    while(mess[i-1]!= 0x0d && i<99);
                talk.Synthe(mess);
            }
            else{
                xbee.printf("preset_message speak\r\n");
                talk.Synthe("purissetommese-ji,,konnichiwa.");
            }
        }
        else{
            xbee.printf("\r\nNot Active\n");
        }
}