File content as of revision 30:3bcf14e8dd63:

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

 
PwmOut motorSpeed(p26);
DigitalOut motorDir1(p19);
DigitalOut motorDir2(p20);


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

MPU6050 mpu(p9, p10);                //(SDA,SCL)のピンをアサインしてね☆   加速度センサー

DigitalIn flight(p21);      //フライトピン
DigitalOut FET(p22);                //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 motorForward(void);
void motorStop(void);
void motorReverse(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");
    
  // FET = 0; 
  /*   while(1) {
    if(flight==1) {
        wait(10);
        }
        
    else{
        FET = 0;                       //FET、ニクロム線切断
        wait(20);
        FET = 1;
        wait(12);
        FET = 0;
        wait(10);
        FET = 1;
        wait(12);
        FET = 0; 
    break;
            }
    }     */
    
    motor1.stop(0);
    motor2.stop(0);
    
    wait(20);     //確認用
    
 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の測定終了 
 
          motorSpeed.period_ms(2);              //モーター調節
          motorSpeed = 0.9;

     
compass.init();                  //地磁気センサー動作
      
int i;
for(i=0;i<20;i++)                 //地磁気測定
{
    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;
    //地磁気センサのキャリブレーション
    motor1.speed(mc1); //車体を時計回りに３秒回転
    motorReverse();
    wait(1.6);
    
    motor1.stop(0);
    motor2.stop(0);
    wait(1);
    
    motor1.speed(-mc1); //車体を反時計回りに３秒回
    motorForward();
    wait(1.6);
    
    motor1.stop(0);
    motor2.stop(0);
    wait(1);
    printf("compass carriblation\r\n"); //キャリブレーション終了
    
float mcn1,mcn2;    
double heading;
    mcn1=1.0;
    mcn2=1.0;
compass.init();
heading=compass.getHeadingXYDeg();
if(90<heading<267.5){
    motor1.speed(mcn1);//右回転
    motorReverse();
    wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間
    motor1.stop(0);
    motor2.stop(0);
    wait(1);
}else if(0<=heading<=90.0){
    motor1.speed(-mcn1);//左回転
    motorForward();
    wait((heading+90.0)*0.004448);
    motor1.stop(0);
    motor2.stop(0);
    wait(1);
}else if(272.5<heading<360){
    motor1.speed(-mcn1);//左回転
    motorForward();
    wait((heading-270)*0.004448);
    motor1.stop(0);
    motor2.stop(0);
    wait(1);
}else{
    wait(5);
}
printf("searchN\r\n"); //機体が北を向く
  
        mu.startUpdates();//start mesuring the distance(超音波センサー)
        int distance; 
        
        int flag=0,flag2=0;                   //変数flagを整数で型づけする。これがスイッチで、1の間は瞬間は何もしないけど、
                                    //スリットの間隔であるπ/4とタイヤの半径70mmつまり一つのスリットを通過するごとに52.5mm加算していく必要があるから
                                    //0になった瞬間はこれを総距離に加えるというスイッチの役割をする。
        float rightrun;                  //変数runをフロートで型づけする
        float leftrun2;
        rightrun=0.0;
        leftrun2=0.0;
        
        float filterCoefficient = 0.9; // ローパスフィルターの係数(これは環境によって要調整。1に近づけるほど平滑化の度合いが大きくなる。
        float lowpassValue = 0;
        float highpassValue = 0;
        float speed = 0;//加速度時から算出した速度
        float oldSpeed = 0;//ひとつ前の速度
        float oldAccel = 0;//ひとつ前の加速度
        float difference=0;//変位
        float timespan=0.01;//時間差
        int accel[3];//accelを3つの配列で定義。*/
        
        int tt=0;
        float run=0;

        while(1)
    {       
        distance=mu.getCurrentDistance();   
        printf("%d\r\n",distance); 
        
        mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
        int x = accel[0]-123;//x軸方向の加速度
        int y = accel[1]+60;//y軸方向の加速度
        int z = accel[2]+1110 ;//z軸方向の加速度
        float X = x*0.000597964111328125;
        float Y = y*0.000597964111328125;
        float Z = z*0.000597964111328125; 
        double a = X*X+Y*Y+Z*Z-95.982071137936;
        if (a>0){
        a = sqrt(a);
        }
        if (a<0) {
        a = abs(a);
        a = -sqrt(a);
        }
        //printf("%lf %f %f %f\r\n",a,X,Y,Z);
        // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 ＋ (1 - 係数) * センサの値)
        lowpassValue = lowpassValue * filterCoefficient + a * (1 - filterCoefficient);
        // ハイパスフィルター(センサの値 - ローパスフィルターの値)//
        highpassValue = a - lowpassValue;

        // 速度計算(加速度を台形積分する)
        speed = ((highpassValue + oldAccel) * timespan) / 2 + speed;
        oldAccel = highpassValue;

        // 変位計算(速度を台形積分する)
        difference = ((speed + oldSpeed) * timespan) / 2 + difference;
        oldSpeed = speed;

        //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示
        printf("speed %f diference %f\r\n",speed,difference);//速度（m）と変位を表示*/
    
        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 += 54.95;                            //総距離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 += 54.95;
            printf("test2.read else\r\n");
        }
        printf("%f", rightrun);
        printf("\t%f\r\n", leftrun2);
        run=culculate_distance_3(rightrun,leftrun2);
        if (2000<run<2100){ //半分くらい進んだら方位を計測し直す
            if(90<heading<267.5){
    motor1.speed(mcn1);//右回転
    motorReverse();
    wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間
    motor1.stop(0);
    motor2.stop(0);
    wait(1);
}else if(0<=heading<=90.0){
    motor1.speed(-mcn1);//左回転
    motorForward();
    wait((heading+90.0)*0.004448);
    motor1.stop(0);
    motor2.stop(0);
    wait(1);
}else if(272.5<heading<360){
    motor1.speed(-mcn1);//左回転
    motorForward();
    wait((heading-270)*0.004448);
    motor1.stop(0);
    motor2.stop(0);
    wait(1);
}else{
    wait(5);
}
printf("searchN\r\n"); //機体が北を向く
}
        else if (run >= 4396){                             //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
            break;                                  //つまりゴールについたらこのループからぬける
        }
        
    /*    if(difference >= 0.3)
         {
             break;
         }    */
        
        motor1.speed(0.5);   //通常走行
        motorForward();
                               //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);
        
        tt++;
        if(tt == 10)
        {
            fprintf(fp, "accel.rightrun.leftrun2\r\n");
            
            fprintf(fp,"(%lf, %lf,%lf)\r\n", difference, rightrun, leftrun2);//加速度とフォトインタラプタによる距離を出力
            tt=0;
        }    
        
        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);       //機体を時計回りに９０度回転
            motorReverse();
            wait(0.4);
            printf("mortor rotation\r\n");
            
            motor1.stop(0);
            motor2.stop(0);
            wait(2);
            printf("mortor stop\r\n");

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

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

            motor1.speed(ms1);       //直進
            motorForward();
            
            
        int t=0;
    for(t=0;t<50;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 += 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);
        
        
        
        mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
        int x = accel[0]-123;//x軸方向の加速度
        int y = accel[1]+60;//y軸方向の加速度
        int z = accel[2]+1110 ;//z軸方向の加速度
        float X = x*0.000597964111328125;
        float Y = y*0.000597964111328125;
        float Z = z*0.000597964111328125; 
        double a = X*X+Y*Y+Z*Z-95.982071137936;
        if (a>0){
        a = sqrt(a);
        }
        if (a<0) {
        a = abs(a);
        a = -sqrt(a);
        }
        //printf("%lf %f %f %f\r\n",a,X,Y,Z);
        // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 ＋ (1 - 係数) * センサの値)
        lowpassValue = lowpassValue * filterCoefficient + a * (1 - filterCoefficient);
        // ハイパスフィルター(センサの値 - ローパスフィルターの値)//
        highpassValue = a - lowpassValue;

        // 速度計算(加速度を台形積分する)
        speed = ((highpassValue + oldAccel) * timespan) / 2 + speed;
        oldAccel = highpassValue;

        // 変位計算(速度を台形積分する)
        difference = ((speed + oldSpeed) * timespan) / 2 + difference;
        oldSpeed = speed;

        //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示
        printf("speed %f diference %f\r\n",speed,difference);//速度（m）と変位を表示*/
        
        if (run >= 4396){                             //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
            break;                                  //つまりゴールについたらこのループからぬける
        }
        
        wait(0.01);
           
    }      
    
    
      
            printf("mortor straight\r\n");   
            
            motor1.stop(0);
            motor2.stop(0);
            wait(2);
            printf("mortor stop\r\n");         

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

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

            motor1.speed(msj1);       //機体を時計回りに９０度回転
            motorReverse();
            wait(0.57);   
            printf("mortor rotation\r\n");
            
            motor1.stop(0);
            motor2.stop(0);
            wait(2);
            printf("mortor stop\r\n");
            
if(90<heading<267.5){
    motor1.speed(mcn1);//右回転
    motorReverse();
    wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間
    motor1.stop(0);
    motor2.stop(0);
    wait(1);
}else if(0<=heading<=90.0){
    motor1.speed(-mcn1);//左回転
    motorForward();
    wait((heading+90.0)*0.004448);
    motor1.stop(0);
    motor2.stop(0);
    wait(1);
}else if(272.5<heading<360){
    motor1.speed(-mcn1);//左回転
    motorForward();
    wait((heading-270)*0.004448);
    motor1.stop(0);
    motor2.stop(0);
    wait(1);
}else{
    wait(5);
}
printf("searchN\r\n"); //機体が北を向く
            
        }
   }
            motor1.stop(0);
            motor2.stop(0);
            
            
            fprintf(fp, "last accel.photo\r\n");
            fprintf(fp,"(%lf, %lf)\r\n", difference, run);//最後の加速度とフォトインタラプタによる距離を出力
            fprintf(fp, "last right.left\r\n");
            fprintf(fp,"(%lf, %lf)\r\n", rightrun, leftrun2);
            fclose(fp);
    
}



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