File content as of revision 6:574d9a6253c7:

#include "mbed.h"
#include "gps.h"
#include "ultrasonic.h"
#include "motordriver.h"
#include "HMC5883L.h"

Serial pc(USBTX, USBRX);            //地磁気センサー
HMC5883L compass(p28, p27);

DigitalOut FET(p21);                //FET

InterruptIn button1(p15);           //フォトインタラプタ
InterruptIn button2(p18);
DigitalIn test(p15);                //ここでピン15からの電圧の値、つまりフォトインタラプタが何か遮るものを検知すればハイの1を返して、
                                    //何もないつまりスリットの部分ではローの0を返す。それを変数testに代入している
DigitalIn test2(p18);

Serial pc(USBTX, USBRX);            //GPS
GPS gps(p28, p27); 

Motor motor1(p22, p16, p17, 1); // pwm, fwd, rev, can brake     モーター
Motor motor2(p22, p19, p20, 1); // pwm, fwd, rev, can brake

void dist(int distance)
{
    //put code here to happen when the distance is changed
    printf("Distance changed to %dmm\r\n", distance);
}

ultrasonic mu(p11, p12, .1, 1, &dist);    //Set the trigger pin to D8 and the echo pin to D9              超音波センサー
                                          //have updates every .1 seconds and a timeout after 1
                                          //second, and call dist when the distance changes
 

 
LocalFileSystem local("local");               // Create the local filesystem under the name "local"　　 データ保存
 
int main() {
    
    
      /*  FET = 0;                       //FET、ニクロム線切断
        wait(60);
        FET = 1;
        wait(30);
        FET = 0;   */
    
    motor1.stop(0);
    motor2.stop(0);
    
   
   FILE *fp = fopen("/local/gps.txt", "w");  // Open "gps.txt" on the local file system for writing
    fprintf(fp, "GPS Start\r\n");
    int n;
    for(n=0;n<45;n++)  //GPSの取得を45回行う(これで大体1分半)
    {
        
        if(gps.getgps()) //現在地取得
            fprintf(fp,"(%lf, %lf)\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
        
        else
            fprintf(fp,"No data\r\n");//データ取得に失敗した場合
        
        wait(1);
        
        printf("%d\r\n",n);              //今何回目かを出力（本番ではいらない）

    }    
    fprintf(fp,"GPS finish\r\n");
    fclose(fp);                      //GPSの測定終了  
    
    compass.init();                  //地磁気センサー動作
  
        mu.startUpdates();//start mesuring the distance(超音波センサー)
        int distance; 
        
        int flag,flag2;                   //変数flagを整数で型づけする。これがスイッチで、1の間は瞬間は何もしないけど、
                                    //スリットの間隔であるπ/4とタイヤの半径70mmつまり一つのスリットを通過するごとに52.5mm加算していく必要があるから
                                    //0になった瞬間はこれを総距離に加えるというスイッチの役割をする。
        float rightrun;                  //変数runをフロートで型づけする
        float leftrun2;
        rightrun=0.0;
        leftrun2=0.0;

        while(1)
    {       
        distance=mu.getCurrentDistance();   
        printf("%d\r\n",distance); 
    
    
        printf("%d\r\n", test.read());
        printf("%d\r\n", test2.read());
        if (test.read() == 1 and flag == 0){        //もしtestが1つまり何か障害物があって、かつflagが0つまりスイッチが切れているときは
            flag = 1;                               //この時はスイッチを1に切る。ただ障害物があるかつスイッチが1で切れているときはそのまま
            printf("test.read if\r\n");
        }
        else if (test.read() == 0 and flag == 1){   //そうじゃなくて今度はとうとうtestが0でスリットの部分になった瞬間なのにスイッチが1で切れているときは
            flag = 0;                               //まずこれでスイッチを0にして入れる。
                                                    //こうすることで同じスリットの中でtestが複数回0を返した時に何回も52.5mmを加算しつづけるということがなくなる
            rightrun += 52.5;                            //総距離runに52.5を加算する
            printf("test.read else\r\n");
        }
         if (test2.read() == 1 and flag2 == 0){
            flag2 = 1;
            printf("test2.read if\r\n");
        }
        else if (test2.read() == 0 and flag2 == 1){
            flag2 = 0;
            leftrun2 += 52.5;
            printf("test2.read else\r\n");
        }
        printf("%f", rightrun);
        printf("\t%f\r\n", leftrun2);
        if (rightrun > 250){                             //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
            break;                                  //つまりゴールについたらこのループからぬける
        }
        
        motor1.speed(0.5);   //通常走行
        motor2.speed(0.5);
                               //Do something else here
                               // mu.checkDistance(); //call checkDistance() as much as possible, as this is where
                               //the class checks if dist needs to be called.

        wait(0.2);
        
        if(100 < distance && distance < 500)        //障害物発見❕
         {
            
            printf("if success!\r\n");
            
            float ms1,ms2,msj1,msj2;
            ms1=1.0;            //直進の時モーターをどれだけ回せばいいかわからないのでとりあえず1.0にしておく⇒waitの秒数を変えた方が良い感じ
            ms2=1.0;
    
            msj1=1.0;           //回転の時
            msj2=1.0;

            motor1.stop(0);
            motor2.stop(0);
            wait(2);
            printf("mortor stop\r\n");

            motor1.speed(msj1);       //機体を時計回りに９０度回転
            motor2.speed(-msj2);
            wait(2);
            printf("mortor rotation\r\n");
            
            motor1.stop(0);
            motor2.stop(0);
            wait(2);
            printf("mortor stop\r\n");

            motor1.speed(ms1);       //直進
            motor2.speed(ms2);
            wait(2);
            printf("mortor straight\r\n");
            
            motor1.stop(0);
            motor2.stop(0);
            wait(2);
            printf("mortor stop\r\n");

            motor1.speed(-msj1);      //機体を反時計回りに９０度回転
            motor2.speed(msj2);
            wait(2);
            printf("mortor rotation\r\n");
            
            motor1.stop(0);
            motor2.stop(0);
            wait(2);
            printf("mortor stop\r\n");

            motor1.speed(ms1);       //直進
            motor2.speed(ms2);
            
           /* printf("%d\r\n", test.read());                   ここの部分は単純に５mを足すことにした方がいいかな？
            if (test.read() == 1 and flag == 0)
            { 
                flag = 1;
            }
            else if (test.read() == 0 and flag == 1)
            {
                flag=0;
                rightrun += 52.5;
            }
             if (test2.read() == 1 and flag2 == 0){
            flag2 = 1;
        }
        else if (test2.read() == 0 and flag2 == 1){
            flag2 = 0;
            leftrun2 += 52.5;
        }
        printf("%f", rightrun);
        printf("\t%f\r\n", leftrun2);*/
            wait(2);
            printf("mortor straight\r\n");   
            
            motor1.stop(0);
            motor2.stop(0);
            wait(2);
            printf("mortor stop\r\n");         

            motor1.speed(-msj1);      //機体を反時計回りに９０度回転
            motor2.speed(msj2);
            wait(2);
            printf("mortor rotation\r\n");
            
            motor1.stop(0);
            motor2.stop(0);
            wait(2);
            printf("mortor stop\r\n");

            motor1.speed(ms1);       //直進
            motor2.speed(ms2);
            wait(2);
            printf("mortor straight\r\n");
            
            motor1.stop(0);
            motor2.stop(0);
            wait(2);
            printf("mortor stop\r\n");

            motor1.speed(msj1);       //機体を時計回りに９０度回転
            motor2.speed(-msj2);
            wait(2);   
            printf("mortor rotation\r\n");
            
            motor1.stop(0);
            motor2.stop(0);
            wait(2);
            printf("mortor stop\r\n");
        }
   }
   
            motor1.stop(0);
            motor2.stop(0);
   
}



            float culculate_distance_3(float a,float b)        //距離推定プログラム、加速度の計算が送られてきたら,mainの中に入れる
            {
               float c; 
               c=0.5*a+0.5*b;//今は平均。計測をもとに修正を加える
               return c;
            }