File content as of revision 3:dd3b7c3c327b:

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

PwmOut mypwm4(D4);
PwmOut mypwm6(D6);
PwmOut mypwm2(D2);
PwmOut mypwm3(D3);

Ticker flag;
Ticker timerflag;
Timer t;

DigitalOut myled(LED1);

HCSR04  usensor(D9,D10); // Trigger(DO), Echo(PWMIN)
unsigned int dist;


#define STATE_A (0)
#define STATE_B (1)
#define STATE_C (2)
#define STATE_D (3)
#define STATE_E (4)
#define STATE_A2 (5)

char mode,fase;
int wait_flag;
int timer;

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;

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){
    
usensor.start();
        wait(0.2); 
        dist=usensor.get_dist_cm();
        /*printf("cm: %ld\n",dist); */


  /*  switch(timer)
{
    case 8.0:
        wait_flag = 1;
        mode = STATE_A;
        break;
    case 12.0:
        wait_flag = 1;
        mode = STATE_B;
        break;
    case 15.5:
        wait_flag = 1;
        mode = STATE_A2;
        break;
    case 22.0:
        wait_flag = 1;
        mode = STATE_D;
        break;
    case 30.0:
        wait_flag = 1;
        mode = STATE_A;
        break;
    case 37.0:
        wait_flag = 1;
        mode = STATE_C;
        break;
    case 42.0:
        wait_flag = 1;
        mode = STATE_A;
        break; 
        
}*/

switch (fase)
        {
            case 0:
                if(timer == 5){   
                    mode = STATE_A;
                    t.stop();
                    t.reset();
                    }
                if(dist > 45){
                    fase = 1;
                    t.start();
                }
                break;
            
            case 1:
                if(timer == 0){
                    wait_flag = 1;
                    mode = STATE_B;
                    }else if(timer == 1.0){
                    wait_flag = 1;
                    mode = STATE_A;
                    t.stop();
                    t.reset();
                    fase = 2;
                    t.start();
                    }
                break;
            case 2:
                if(timer == 6.0){
                    wait_flag = 1;
                    mode = STATE_D;
                    }else if(timer == 16.0){
                        wait_flag = 1;
                        mode = STATE_A;
                        }else if(timer == 26.0){
                            wait_flag = 1;
                            mode = STATE_D;
                            }else if(timer == 36.0){
                                wait_flag = 1;
                                mode = STATE_A;}else if(timer == 46.0){
                                    wait_flag = 1;
                                    mode = STATE_D;
        }

// 状態が切り替わるときは一時停止
        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(70);
                mypwm6.pulsewidth_ms(0);
                // 右モータの制御
                mypwm2.pulsewidth_ms(70);
                mypwm3.pulsewidth_ms(0);
                break;
                // STATE_A : 前進（左：正転　右：正転）
            case STATE_A2:
                myled = 1; // LED点灯
                // 左モータの制御
                mypwm4.pulsewidth_ms(98);
                mypwm6.pulsewidth_ms(0);
                // 右モータの制御
                mypwm2.pulsewidth_ms(100);
                mypwm3.pulsewidth_ms(0);
                break;
                
            // STATE_B : 右旋回（左：正転　右：逆転）
            case STATE_B:
                myled = 1; // LED点灯
                // 左モータの制御
                mypwm4.pulsewidth_ms(100);
                mypwm6.pulsewidth_ms(0);
                // 右モータの制御
                mypwm2.pulsewidth_ms(0);
                mypwm3.pulsewidth_ms(100);
                break;
                
            // STATE_C : 左旋回（左：逆転　右：正転）
            case STATE_C:
                myled = 1; // LED点灯
                // 左モータの制御
                mypwm4.pulsewidth_ms(0);
                mypwm6.pulsewidth_ms(100);
                // 右モータの制御
                mypwm2.pulsewidth_ms(100);
                mypwm3.pulsewidth_ms(0);
                break;
                
            // STATE_D : 後退（左：逆転　右：逆転）
            case STATE_D:
                myled = 1; // LED点灯
                // 左モータの制御
                mypwm4.pulsewidth_ms(0);
                mypwm6.pulsewidth_ms(80);
                // 右モータの制御
                mypwm2.pulsewidth_ms(0);
                mypwm3.pulsewidth_ms(83);
                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;
        }
    }
    
}
}