File content as of revision 41:7c537a922510:

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

 
PwmOut motorSpeedR(p26);
PwmOut motorSpeedL(p25);
DigitalOut motor1Dir1(p19);
DigitalOut motor1Dir2(p20);
DigitalOut motor2Dir1(p16);
DigitalOut motor2Dir2(p17);


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

MPU6050 mpu(p9, p10);                //(SDA,SCL)  加速度センサー*/

DigitalIn flight(p23);      //フライトピン
DigitalOut SW(p22);         //トリガー用
DigitalOut FET(p21);                //FET

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


//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
                                          
void motorForwardR(void);
void motorStopR(void);
void motorReverseR(void);                                         
 
void motorForwardL(void);
void motorStopL(void);
void motorReverseL(void);
 
LocalFileSystem local("local");               // Create the local filesystem under the name "local"　　 データ保存 
 
float culculate_distance_3(float a,float b);

int main() {
    
    printf("cansat start\r\n");
    flight==1;
    FET = 0;
    SW = 1;  //p23をhigh(3.3V)にする。 
     while(1) {
    if(flight==1) {
        wait(10);
        printf("mada\r\n");
        }
        
    else{
        if(flight==1) {
        wait(10);
        printf("madamada\r\n");
        }
        else{
          printf("yattar\r\n");
        FET = 0;                       //FET、ニクロム線切断
        wait(20);
        FET = 1;
        wait(12);
        FET = 0;
        wait(10);
        FET = 1;
        wait(12);
        FET = 0; 
        SW = 0;                         //p23をlow(0V)にする。 
    break;
            }
    }
    }
    
    motorStopR();
    motorStopL();
    
    wait(20);     //確認用//デフォ20秒
    
//    FILE *fp = fopen("/local/gps.txt", "w");  // Open "gps.txt" on the local file system for writing
    
   printf("GPS start\r\n");
   FILE *fp = fopen("/local/gps,foto.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分半)
    {
        printf("gps for\r\n");
        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の測定終了   */
 
          motorSpeedR.period_ms(4);              //モーター調節
          motorSpeedR = 0.655;
          motorSpeedL.period_ms(4);              //モーター調節
          motorSpeedL = 0.755;
     
compass.init();                  //地磁気センサー動作
      
int i;
for(i=0;i<20;i++)                 //地磁気測定//デフォ20
{
    pc.printf("raw=%f\r\n",compass.getHeadingXYDeg()); //度数法で表記
    fprintf(fp,"raw=\r\n");
    fprintf(fp,"%lf\r\n",compass.getHeadingXYDeg());
    wait(0.5);
}
float mc1,mc2;
    mc1=1.0;
    mc2=1.0;
    //地磁気センサのキャリブレーション
    motorForwardL(); //車体を時計回りに３秒回転
    motorReverseR();
    wait(1.6);
    
    motorStopR();
    motorStopL();
    wait(1);
    
    motorReverseL(); //車体を反時計回りに３秒回
    motorForwardR();
    wait(1.6);
    
    motorStopR();
    motorStopL();
    wait(1);
    printf("compass carriblation\r\n"); //キャリブレーション終了
    
//float mcn1,mcn2;    
double heading,wx,wy,wz;
 //   mcn1=1.0;
   // mcn2=1.0;

heading=compass.getHeadingXYDeg();//headingに地磁気の値を格納する
if(90<heading<267.5){
    printf("right\r\n");
    motorForwardL();//右回転
    motorReverseR();
    wx=(270-heading)*0.004448;
    wait(wx); //角度のずれ*1度回転するのにかかる時間
    motorStopR();
    motorStopL();
    wait(1);
}
else if(0<=heading<=90.0){
    printf("left\r\n");
    motorForwardL();//左回転
    motorReverseR();
    wy=(270-heading)*0.004448;
    wait(wy);
    motorStopR();
    motorStopL();
    wait(1);
}
else if(272.5<heading<360){
    printf("left\r\n");
    motorForwardL();//左回転
    motorReverseR();
    wz=(360-heading)*0.004448;
    wait(wz);
    motorStopR();
    motorStopL();
    wait(1);
}
else{
    wait(5);
}
printf("searchN\r\n"); //機体が北を向く
wait(2);
  
        mu.startUpdates();//start mesuring the distance(超音波センサー)
        int distance; 
        
        int flag=0,flag2=0;                   //変数flagを整数で型づけする。これがスイッチで、1の間は瞬間は何もしないけど、
                                    //スリットの間隔であるπ/4とタイヤの半径70mmつまり一つのスリットを通過するごとに52.5mm加算していく必要があるから
                                    //0になった瞬間はこれを総距離に加えるというスイッチの役割をする。
        float rightrun;                  //変数runをフロートで型づけする
        float leftrun;
        rightrun=0.0;
        leftrun=0.0;

        int accel[3];//accelを3つの配列で定義。*/
        int gyro[3];
        
        int tt=0;
        float run=0;
        fprintf(fp, "x,y,z,lefttrun2,rightrun\r\n");

        int k=0;//中間地点角度判定プログラムを１回しか通さないためのフラグ
        while(1)
    {       
        distance=mu.getCurrentDistance();   
        printf("%d\r\n",distance); 
        
        mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
        int x = accel[0];//x軸方向の加速度
        int y = accel[1];//y軸方向の加速度
        int z = accel[2] ;//z軸方向の加速度
        mpu.readGyroData(gyro);//加速度の値をaccel[3]に代入
        int gx = gyro[0];//x軸方向の加速度
        int gy = gyro[1];//y軸方向の加速度
        int gz = gyro[2];//z軸方向の加速度

        printf("x=%d,y=%d,z=%d\r\n",x,y,z); //加速度の表示
    
        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を返した時に何回も56.5mmを加算しつづけるということがなくなる
            rightrun += 113;                            //総距離runに113mmを加算する(タイヤの全周452mm)
            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;
            leftrun += 113;
            printf("test2.read else\r\n");
        }
        printf("%f %g rot ", leftrun,leftrun/452);
        printf("%f %g rot\r\n", rightrun,rightrun/452);
        run=culculate_distance_3(rightrun,leftrun);
    if(run >= 30000){//もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
        break;                                  //つまりゴールについたらこのループからぬける
        }
    else if(15000< run && run<15050 && k!=1){//中間地点で北を向きなおす
    motorStopR();
    motorStopL();
    wait(1);
    heading=compass.getHeadingXYDeg();//コンパスの値をもう一度headingに格納
    wait(2);
    k=1;
    
heading=compass.getHeadingXYDeg();//headingに地磁気の値を格納する
if(90<heading<267.5){
    printf("right\r\n");
    motorForwardL();//右回転
    motorReverseR();
    wx=(270-heading)*0.004448;
    wait(wx); //角度のずれ*1度回転するのにかかる時間
    motorStopR();
    motorStopL();
    wait(1);
}
else if(0<=heading<=90.0){
    printf("left\r\n");
    motorForwardL();//左回転
    motorReverseR();
    wy=(270-heading)*0.004448;
    wait(wy);
    motorStopR();
    motorStopL();
    wait(1);
}
else if(272.5<heading<360){
    printf("left\r\n");
    motorForwardL();//左回転
    motorReverseR();
    wz=(360-heading)*0.004448;
    wait(wz);
    motorStopR();
    motorStopL();
    wait(1);
}
else{
    wait(5);
}
printf("searchN\r\n"); //機体が北を向く
}

        
    /*    if(difference >= 0.3)
         {
             break;
         }    */
        
        motorForwardL();   //通常走行
        motorForwardR();
                               //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.01);
        
            
            fprintf(fp,"%5d, %5d, %5d, %5d, %5d, %5d, %8lf, %8lf\r\n", x, y, z,gx,gy,gz,  leftrun, rightrun);//加速度とフォトインタラプタによる距離を出力
        
        if(450 < distance && distance < 550)        //障害物発見
         {
            
            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;

            motorStopR();
            motorStopL();
            wait(2);
            printf("mortor stop\r\n");

            motorForwardL();       //機体を時計回りに９０度回転
            motorReverseR();
            wait(1);
            printf("mortor rotation\r\n");
            
            motorStopR();
            motorStopL();
            wait(2);
            printf("mortor stop\r\n");

            motorForwardL();       //直進
            motorForwardR();
            wait(3);
            printf("mortor straight\r\n");
            
            motorStopR();
            motorStopL();
            wait(2);
            printf("mortor stop\r\n");

            motorReverseL();      //機体を反時計回りに９０度回転
            motorForwardR();
            wait(1);
            printf("mortor rotation\r\n");
            
            motorStopR();
            motorStopL();
            wait(2);
            printf("mortor stop\r\n");

            motorForwardL();       //直進
            motorForwardR();
            
            
        int t=0;
    for(t=0;t<100;t++)
    {
            printf("%d\r\n", test.read());
            printf("%d\r\n", test2.read());                   
            if (test.read() == 1 and flag == 0)
            { 
                flag = 1;
            }
            else if (test.read() == 0 and flag == 1)
            {
                flag=0;
                rightrun += 113;
            }
             if (test2.read() == 1 and flag2 == 0){
            flag2 = 1;
        }
        else if (test2.read() == 0 and flag2 == 1){
            flag2 = 0;
            leftrun += 113;
        }
        printf("%f", leftrun);
        printf("\t%f\r\n", rightrun);
        
        
        
        mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
        mpu.readGyroData(gyro);
        int x = accel[0];
        int y = accel[1];
        int z = accel[2];
        int gx = gyro[0];
        int gy = gyro[1];
        int gz = gyro[2];
        fprintf(fp,"%5d, %5d, %5d, %8lf, %8lf, %5d, %5d, %5d\r\n", x, y, z, leftrun, rightrun,gx, gy ,gz);//加速度とフォトインタラプタによる距離を出力

       if (run >= 300000){                             //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
            break;                                  //つまりゴールについたらこのループからぬける
        }
        
        wait(0.01);
           
    }      
    
    
      
            printf("mortor straight\r\n");   
            
            motorStopR();
            motorStopL();
            wait(2);
            printf("mortor stop\r\n");         

            motorReverseL();      //機体を反時計回りに９０度回転
            motorForwardR();
            wait(1);
            printf("mortor rotation\r\n");
            
            motorStopR();
            motorStopL();
            wait(2);
            printf("mortor stop\r\n");

            motorForwardL();       //直進
            motorForwardR();
            wait(2);
            printf("mortor straight\r\n");
            
            motorStopR();
            motorStopL();
            wait(2);
            printf("mortor stop\r\n");

            motorForwardL();       //機体を時計回りに９０度回転
            motorReverseR();
            wait(1);   
            printf("mortor rotation\r\n");
            
            motorStopR();
            motorStopL();
            wait(2);
            printf("mortor stop\r\n");
            
heading=compass.getHeadingXYDeg();//headingに地磁気の値を格納する
if(90<heading<267.5){
    printf("right\r\n");
    motorForwardL();//右回転
    motorReverseR();
    wx=(270-heading)*0.004448;
    wait(wx); //角度のずれ*1度回転するのにかかる時間
    motorStopR();
    motorStopL();
    wait(1);
}
else if(0<=heading<=90.0){
    printf("left\r\n");
    motorForwardL();//左回転
    motorReverseR();
    wy=(270-heading)*0.004448;
    wait(wy);
    motorStopR();
    motorStopL();
    wait(1);
}
else if(272.5<heading<360){
    printf("left\r\n");
    motorForwardL();//左回転
    motorReverseR();
    wz=(360-heading)*0.004448;
    wait(wz);
    motorStopR();
    motorStopL();
    wait(1);
}
else{
    wait(5);
}
printf("searchN\r\n"); //機体が北を向く
            
        }
   }
            motorStopR();
            motorStopL();
            
            
            fprintf(fp, "last photo\r\n");
            fprintf(fp,"(%lf)\r\n", run);//最後の加速度とフォトインタラプタによる距離を出力
            fprintf(fp, "last left.right\r\n");
            fprintf(fp,"(%lf, %lf)\r\n", leftrun, rightrun);
            fclose(fp);

    
}



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

void motorForwardL() {
    motorStopR();
    motor2Dir1 = 1;
    motor2Dir2 = 0;
}
 
void motorReverseL() {
    motorStopR();
    motor2Dir1 = 0;
    motor2Dir2 = 1;
}
 
void motorStopL() {
    motor2Dir1 = 0;
    motor2Dir2 = 0;
}