2号機

Dependencies:   HCSR04 mbed

Fork of Nucleo_SS2 by SS2

main.cpp

Committer:
ishi777
Date:
2017-12-04
Revision:
2:f568ec2e6955
Parent:
1:aabe6887447c
Child:
3:dd3b7c3c327b

File content as of revision 2:f568ec2e6955:

#include "mbed.h"

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

Ticker flag;
Timer t;

DigitalOut myled(LED1);


#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;
int wait_flag;
int timer;

void flg(){
        timer = t.read(); 
        }

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);
    
flag.attach(&flg, 1.0);


        
while(1){
    
    switch(timer)
{
    case 8.0:
        wait_flag = 1;
        mode = STATE_A;
        break;
    case 13.0:
        wait_flag = 1;
        mode = STATE_B;
        break;
    case 17.0:
        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; 
        
}

// 状態が切り替わるときは一時停止
        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(75);
                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(50);
                mypwm6.pulsewidth_ms(0);
                // 右モータの制御
                mypwm2.pulsewidth_ms(0);
                mypwm3.pulsewidth_ms(50);
                break;
                
            // STATE_C : 左旋回(左:逆転 右:正転)
            case STATE_C:
                myled = 1; // LED点灯
                // 左モータの制御
                mypwm4.pulsewidth_ms(0);
                mypwm6.pulsewidth_ms(50);
                // 右モータの制御
                mypwm2.pulsewidth_ms(50);
                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;
        }
    }
    
}