File content as of revision 5:b6daf80a7237:

#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);

Ticker flag;
Ticker timerflag;
Timer t;

DigitalOut myled(LED1);

HCSR04  usensor1(D9,D10); // Trigger(DO), Echo(PWMIN)
HCSR04  usensor2(D11,D12); // Trigger(DO), Echo(PWMIN)
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)
#define STATE_A4 (7)
#define STATE_D2 (8) 

char mode,fase,linetrace;
int wait_flag;
double timer,measL,measM,measR;


void tflg(){
        timer = t.read(); //タイマー値読み込み
        }
/*void flg(){
        usensor.start();
        dist=usensor.get_dist_cm();
        printf("dist:%u\n", dist);
        }*/

int main() {
    
t.start();
mode = STATE_E;
wait_flag = 0;

fase = 0;

mypwm4.period_ms(100);
mypwm6.period_ms(100);
mypwm2.period_ms(100);
mypwm3.period_ms(100);
    
timerflag.attach(&tflg, 0.1);
/*flag.attach(&flg, 1.0);*/

      
while(1){
    
usensor1.start();
usensor2.start(); 
        dist1=usensor1.get_dist_cm(); 
        dist2=usensor2.get_dist_cm();
        measL = photoref_L.read(); 
        measM = photoref_M.read();
        measR = photoref_R.read();
         wait(0.1); 
        printf("dist1: %ld\n",dist1);      
        printf("dist2: %ld\n",dist2); 
          printf("fase: %ld\n",fase); 
                    printf("timer: %lf\n",timer); 

          
      

switch (fase)
        {   case 0:
                if(timer > 5.0){
                    mode = STATE_A2;
                    if(dist1 > 45 && dist1 < 100){
                        fase = 1;
                        t.stop();
                        t.reset();
                    }
                }
                        break;
 /*  switch(linetrace){
                    case 0:
                    if(timer > 5 && measL>0.2 && measM>0.2 && measR>0.2){
                    mode = STATE_A4;
                    }if((timer > 5 && measL<0.2 && measM<0.2) || (timer > 5 && measM<0.2 && measR<0.2)){
                        linetrace = 1;
                        t.stop();
                        t.reset();
                    }
                    break;
    
                    case 1:
                    t.start();
                    if(measL<0.2 && measM>0.2){
                        mode = STATE_A3;
                        }else if(measM<0.2 && measL>0.2){
                        mode = STATE_A2;
                        }else if(measL>0.2 && measM>0.2){
                            if(dist2 < 10){
                                mode = STATE_A2;
                                }else if(dist2 > 10){
                                    mode = STATE_A3;
                                    }
                        }else if((timer > 1 && measL<0.2 && measM<0.2) || (timer > 1 && measM<0.2 && measR<0.2) ){
                        linetrace = 2;
                        t.stop();
                        t.reset();
                        }
                        break;
                        
                    case 2:
                    t.start();
                    mode = STATE_B;
                    if((timer > 2 && measL<0.2 && measM<0.2) || (timer > 1 && measM<0.2 && measR<0.2)){
                    fase = 2;
                    t.stop();
                    t.reset();
                        } //if閉じ
                    } //switch閉じ
                break;*/
                           
            case 1:
            if(timer == 0){    
            wait_flag = 1;
            mode = STATE_B;
            t.start();
                 }else if(timer > 2.0 && dist1 > 60 && dist1 < 250){
                    wait_flag = 1;
                    mode = STATE_A;
                    t.stop();
                    t.reset();
                    fase = 2;
                    }
                break;
            case 2:
            t.start(); 
                if(timer > 10.0 && timer < 11.0){
                    wait_flag = 1;
                    mode = STATE_D;
                    fase = 3;
                    t.stop();
                    t.reset();
                    }/*else if (timer > 10.0 && timer < 11.0 && dist2 < 8 ){
                        wait_flag = 1;
                        mode = STATE_D2;
                        fase = 3;
                        t.stop();
                        t.reset();
                        }*/
                        break;
            case 3:
            t.start(); 
               /* if(timer > 15.0 && timer < 16.0 && dist2 > 8 && dist2 < 100){
                    wait_flag = 1;
                    mode = STATE_A4;
                    fase = 4;
                    t.stop();
                    t.reset();
                    }else */
                    if (timer > 15.0 && timer < 16.0){
                        wait_flag = 1;
                        mode = STATE_A3;
                        fase = 4;
                        t.stop();
                        t.reset();
                        }
                        break;
                    
            case 4:
            /*if(measL <0.2 || measM <0.2 || measR <0.2){
                    wait_flag = 1;
                    mode = STATE_C;
                    fase = 5;
                    }*/
                    if(dist1 > 45 && dist1 < 100){
                        fase = 5;}
                    
                    break;
            case 5:        
                                wait_flag = 1;
                    mode = STATE_C;
                    t.start();
                    if(timer > 2.0 && dist1 > 60 && dist1 < 250){
                    t.stop();
                    t.reset();
                    fase = 6;
                    }
                    break;
                
            case 6:
               t.start();
               if(timer <2.0){
               wait_flag = 1;
               mode = STATE_A;
               }else{
                   wait_flag = 1;
                   mode = STATE_A4;}
               break;
                    
                    
                    
                    
                    
                    
                    
                    
                    
                  /*  else if(timer > 20.0 && timer < 21.0){
                        wait_flag = 1;
                        mode = STATE_A;
                        }else if(timer > 30.0 && timer < 31.0){
                            wait_flag = 1;
                            mode = STATE_D;
                            }else if(timer > 40.0 && timer < 41.0){
                                wait_flag = 1;
                                mode = STATE_A;
                                }else if(timer > 50.0 && timer < 51.0){
                                    wait_flag = 1;
                                    mode = STATE_D;}
                                    break; */
        }
// 状態が切り替わるときは一時停止
        if(wait_flag==1)
        {
            // フラグの初期化
            wait_flag = 0;
            myled = 0; // LED消灯
            // 左モータの制御
            mypwm4.pulsewidth_ms(80);
            mypwm6.pulsewidth_ms(80);
            // 右モータの制御
            mypwm2.pulsewidth_ms(80);
            mypwm3.pulsewidth_ms(80);
            // 500ms待機する
            wait(0.5);
        }
        
switch (mode)
        {
            // STATE_A : 前進（左：正転　右：正転）
            case STATE_A:
                myled = 1; // LED点灯
                // 右モータの制御
                mypwm4.pulsewidth_ms(40);
                mypwm6.pulsewidth_ms(0);
                // 左モータの制御
                mypwm2.pulsewidth_ms(38);
                mypwm3.pulsewidth_ms(0);
                break;
                // STATE_A2 : 右に曲がる（左：正転　右：正転）
            case STATE_A2:
                myled = 1; // LED点灯
                // 右モータの制御
                mypwm4.pulsewidth_ms(55);
                mypwm6.pulsewidth_ms(0);
                // 左モータの制御
                mypwm2.pulsewidth_ms(38);
                mypwm3.pulsewidth_ms(0);
                break;
                // STATE_A3 : hidariに曲がる（左：正転　右：正転）
            case STATE_A3:
                myled = 1; // LED点灯
                // 右モータの制御
                mypwm4.pulsewidth_ms(40);
                mypwm6.pulsewidth_ms(0);
                // 左モータの制御
                mypwm2.pulsewidth_ms(55);
                mypwm3.pulsewidth_ms(0);
                break;
                case STATE_A4:
                myled = 1; // LED点灯
                // 右モータの制御
                mypwm4.pulsewidth_ms(50);
                mypwm6.pulsewidth_ms(0);
                // 左モータの制御
                mypwm2.pulsewidth_ms(40);
                mypwm3.pulsewidth_ms(0);
                break;
            // STATE_B : 左旋回（左：正転　右：逆転）
            case STATE_B:
                myled = 0; // LED点灯
                // 右モータの制御
                mypwm4.pulsewidth_ms(30);
                mypwm6.pulsewidth_ms(0);
                // 左モータの制御
                mypwm2.pulsewidth_ms(0);
                mypwm3.pulsewidth_ms(30);
                break;
                
            // STATE_C : 右旋回（左：逆転　右：正転）
            case STATE_C:
                myled = 0; // LED点灯
                // 右モータの制御
                mypwm4.pulsewidth_ms(0);
                mypwm6.pulsewidth_ms(40);
                // 左モータの制御
                mypwm2.pulsewidth_ms(40);
                mypwm3.pulsewidth_ms(0);
                break;
                
            // STATE_D : 後退（左：逆転　右：逆転）
            case STATE_D:
                myled = 1; // LED点灯
                // 右モータの制御
                mypwm4.pulsewidth_ms(0);
                mypwm6.pulsewidth_ms(50);
                // 左モータの制御
                mypwm2.pulsewidth_ms(0);
                mypwm3.pulsewidth_ms(48);
                break;
                
            // STATE_D : 後退（左：逆転　右：逆転）
            case STATE_D2:
                myled = 1; // LED点灯
                // 右モータの制御
                mypwm4.pulsewidth_ms(0);
                mypwm6.pulsewidth_ms(47);
                // 左モータの制御
                mypwm2.pulsewidth_ms(0);
                mypwm3.pulsewidth_ms(48);
                break;
            // STATE_E : ブレーキ（左：ブレーキ　右：ブレーキ）
            case STATE_E:
                myled = 0; // LED消灯
                // 右モータの制御
                mypwm4.pulsewidth_ms(60);
                mypwm6.pulsewidth_ms(60);
                // 左モータの制御
                mypwm2.pulsewidth_ms(60);
                mypwm3.pulsewidth_ms(60);
                break;
        }
    }
    }