File content as of revision 6:d264f7f2b781:

#include "mbed.h"
#include "hcsr04.h"

AnalogIn photoref_L(A1); // フォトリフレクタ左
AnalogIn photoref_M(A2); // フォトリフレクタ中央
AnalogIn photoref_R(A3); // フォトリフレクタ右

PwmOut mypwm4(D4); // モータ
PwmOut mypwm6(D6); // モータ
PwmOut mypwm2(D2); // モータ
PwmOut mypwm3(D3); // モータ

HCSR04  usensor1(D9,D10);  // 超音波後；Trigger(DO), Echo(PWMIN)
HCSR04  usensor2(D11,D12); // 超音波横；Trigger(DO), Echo(PWMIN)

Timer t;
Ticker timer_flag;


DigitalOut myled(LED1);


unsigned int dist1,dist2;


#define STATE_A (0) // 前進
#define STATE_B (1)
#define STATE_C (2)
#define STATE_D (3)
#define STATE_E (4)
#define STATE_A2 (5)
#define STATE_A3 (6)

char mode = STATE_E;
int fase;
int wait_flag;
int timer;
void LineTrace(void);
float val_L, val_R, val_M;



void tflg() {
    timer = t.read(); 
}
/*void flg(){
        usensor.start();
        dist=usensor.get_dist_cm();
        printf("dist:%u\n", dist);
        }*/

int main() {
    t.start();
    wait(3);
    wait_flag = 0;
    fase = 0;
    
    timer_flag.attach(&tflg, 1.0); // タイマー割り込み設定
    
    while(1) {
        val_M = photoref_M.read();
        val_L = photoref_L.read();
        val_R = photoref_R.read();
        printf("M: %f\n", val_M);
        printf("L: %f\n", val_L);
        printf("R: %f\n", val_R);
    
        if(val_L > 0.5 && val_R > 0.5) { // 左：白，右：白
          LineTrace(); // 直進
        } else if(val_L < 0.5 && val_R > 0.5) { // 左：白，右：黒
          mode = STATE_A2; // 右前進
        } else if(val_L > 0.5 && val_R < 0.5) { // 左：黒，右：白
          mode = STATE_A3; // 左前進
        } else { // 左：黒，右：黒
          mode = STATE_E; // ブレーキ
        }
        wait_ms(100);
        /*
        // printf("timer: %d\n", timer);
        
        // 超音波取得
        usensor1.start();
        usensor2.start();
        wait_ms(500); 
        dist1 = usensor1.get_dist_cm(); 
        dist2 = usensor2.get_dist_cm();
        printf("dist1: %ld [cm]  ",dist1);
        printf("dist2: %ld [cm]\n",dist2); 
                
        switch(fase) {
            case 0:
                mode = STATE_A; // 前進
                //if(timer > 5) {
                    /*if(dist2 > 8){
                        mode = STATE_A3; //hidariに
                        }else if(dist2 < 10){
                            mode = STATE_A2;  //migiに
                            }else{mode = STATE_A;}
                    }
                if(dist1 > 41 && dist1 < 250) { // 後ろの壁から45cmの地点
                    wait_flag = 1;
                    fase = 1;
                    t.stop();
                    t.reset();
                    t.start();
                }
                break;
            case 1:
                mode = STATE_B; // 右旋回
                if(timer > 2) {
                    wait_flag = 1;
                    fase = 2;
                    t.reset();
                    t.stop();
                    t.start();
                }
                break;
            case 2:
                mode = STATE_A; // 前進
                if(timer > 4.0) {
                    wait_flag = 1;
                    fase = 3;
                }
                break;
            case 3:
                mode = STATE_D;
                if(timer > 13.0) {
                    wait_flag = 1;
                    fase = 4;
                }
                break;
            case 4:
                 mode = STATE_A; // 前進
                 if(timer > 21.0) {
                    wait_flag = 1;
                    fase = 5;
                }
                break;
            case 5:
                 mode = STATE_D; // 後退
                 if(timer > 29.0) {
                    wait_flag = 1;
                    fase = 6;
                }
                break;
            case 6:
                mode = STATE_A; // 前進
                if(timer > 37.0) {
                    wait_flag = 1;
                    fase = 7;
                }
                break;
            case 7:
                mode = STATE_D; // 後退
                if(timer > 37) {
                    mode = STATE_E; // 停止
                }
                break;
            default:
                break;
        }
        
        // 状態が切り替わるときは一時停止
        if(wait_flag == 1) {
            // フラグの初期化
            wait_flag = 0;
            myled = 0; // LED消灯
            // 左モータの制御
            mypwm4.pulsewidth_ms(50);
            mypwm6.pulsewidth_ms(50);
            // 右モータの制御
            mypwm2.pulsewidth_ms(50);
            mypwm3.pulsewidth_ms(50);
            // 500ms待機する
            mode = STATE_E;
            wait(0.5);
        }
        */
        switch(mode) {
            // STATE_A : 前進（左：正転　右：正転）
            case STATE_A:
                printf("STATE_A\n");
                myled = 1; // LED点灯
                // 左モータの制御
                mypwm4.pulsewidth_ms(15);
                mypwm6.pulsewidth_ms(0);
                // 右モータの制御
                mypwm2.pulsewidth_ms(15);
                mypwm3.pulsewidth_ms(0);
                break;
            // STATE_A2 : 右に曲がる（左：正転　右：正転）
            case STATE_A2:
                printf("STATE_A2\n");
                myled = 1; // LED点灯
                // 左モータの制御
                mypwm4.pulsewidth_ms(10);
                mypwm6.pulsewidth_ms(0);
                // 右モータの制御
                mypwm2.pulsewidth_ms(6);
                mypwm3.pulsewidth_ms(0);
                break;
            // STATE_A3 : hidariに曲がる（左：正転　右：正転）
            case STATE_A3:
                printf("STATE_A3\n");
                myled = 1; // LED点灯
                // 左モータの制御
                mypwm4.pulsewidth_ms(6);
                mypwm6.pulsewidth_ms(0);
                // 右モータの制御
                mypwm2.pulsewidth_ms(10);
                mypwm3.pulsewidth_ms(0);
                break;
            // STATE_B : 右旋回（左：正転　右：逆転）
            case STATE_B:
                printf("STATE_B\n");
                myled = 1; // LED点灯
                // 左モータの制御
                mypwm4.pulsewidth_ms(15);
                mypwm6.pulsewidth_ms(0);
                // 右モータの制御
                mypwm2.pulsewidth_ms(0);
                mypwm3.pulsewidth_ms(15);
                break;
            // STATE_C : 左旋回（左：逆転　右：正転）
            case STATE_C:
                printf("STATE_C\n");
                myled = 1; // LED点灯
                // 左モータの制御
                mypwm4.pulsewidth_ms(0);
                mypwm6.pulsewidth_ms(15);
                // 右モータの制御
                mypwm2.pulsewidth_ms(15);
                mypwm3.pulsewidth_ms(0);
                break;
            // STATE_D : 後退（左：逆転　右：逆転）
            case STATE_D:
                printf("STATE_D\n");
                myled = 1; // LED点灯
                // 左モータの制御
                mypwm4.pulsewidth_ms(0);
                mypwm6.pulsewidth_ms(15);
                // 右モータの制御
                mypwm2.pulsewidth_ms(0);
                mypwm3.pulsewidth_ms(15);
                break;
            // STATE_E : ブレーキ（左：ブレーキ　右：ブレーキ）
            case STATE_E:
                printf("STATE_E\n");
                myled = 0; // LED消灯
                // 左モータの制御
                mypwm4.pulsewidth_ms(30);
                mypwm6.pulsewidth_ms(30);
                // 右モータの制御
                mypwm2.pulsewidth_ms(30);
                mypwm3.pulsewidth_ms(30);
                break;
            default:
                break;
        }
    }
}


void LineTrace(void) {   // ラインの右側でライントレース
  if(val_M > 0.5) { // 中央：白
    mode = STATE_A2;      // 左前進
  } else {               // 中央：黒
    mode = STATE_A3;      // 右前進
  }
  wait_ms(50);
}