otamesi

Dependencies:   mbed

main.cpp

Committer:
seangshim
Date:
2018-10-15
Revision:
1:10af8aaa5b40
Parent:
0:a01fda36fde8
Child:
2:37d831f82840

File content as of revision 1:10af8aaa5b40:

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

DigitalOut FET(p21);                //FET

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

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の測定終了  
  
        mu.startUpdates();//start mesuring the distance(超音波センサー)
        int distance;
        
        while(1)
    {
        int flag;                   //変数flagを整数で型づけする。これがスイッチで、1の間は瞬間は何もしないけど、
                                    //スリットの間隔であるπ/4とタイヤの半径70mmつまり一つのスリットを通過するごとに52.5mm加算していく必要があるから
                                    //0になった瞬間はこれを総距離に加えるというスイッチの役割をする。
        float run;                  //変数runをフロートで型づけする

        printf("%d\r\n", test.read());
        if (test.read() == 1 and flag == 0){        //もしtestが1つまり何か障害物があって、かつflagが0つまりスイッチが切れているときは
            flag = 1;                               //この時はスイッチを1に切る。ただ障害物があるかつスイッチが1で切れているときはそのまま
        }
        else if (test.read() == 0 and flag == 1){   //そうじゃなくて今度はとうとうtestが0でスリットの部分になった瞬間なのにスイッチが1で切れているときは
            flag = 0;                               //まずこれでスイッチを0にして入れる。
                                                    //こうすることで同じスリットの中でtestが複数回0を返した時に何回も52.5mmを加算しつづけるということがなくなる
            run += 52.5;                            //総距離runに52.5を加算する
        }
       /* if (run > 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.

        distance=mu.getCurrentDistance();
        wait(0.1);
        
        printf("%d\r\n",distance);
        
        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);       //機体を時計回りに90度回転
            motor2.speed(-msj2);
            wait(2);
            printf("mortor rotation\r\n");

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

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

            motor1.speed(ms1);       //直進
            motor2.speed(ms2);
            
            printf("%d\r\n", test.read());
            if (test.read() == 1 and flag == 0)
            { 
                flag = 1;
            }
            else if (test.read() == 0 and flag == 1)
            {
                flag=0;
            }
            
            wait(2);
            printf("mortor straight\r\n");            

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

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

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



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