Rui Kato
/
Nucleo_SS2_No2
2号機
Fork of Nucleo_SS2 by
main.cpp
- Committer:
- Morimoto448
- Date:
- 2017-12-12
- Revision:
- 4:04ca6a211b85
- Parent:
- 3:dd3b7c3c327b
- Child:
- 5:ab74616c24b4
File content as of revision 4:04ca6a211b85:
#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 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) 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; fase = 0; mypwm4.period_ms(100); mypwm6.period_ms(100); mypwm2.period_ms(100); mypwm3.period_ms(100); timerflag.attach(&tflg, 1.0); /*flag.attach(&flg, 1.0);*/ while(1){ usensor1.start(); usensor2.start(); wait(0.2); dist1=usensor1.get_dist_cm(); dist2=usensor2.get_dist_cm(); /*printf("dist1: %ld\n",dist1); */ printf("dist2: %ld\n",dist2); /* 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: /*printf("timer: %d\n", timer);*/ if(timer > 5){ /*if(dist2 > 8){ mode = STATE_A3; //hidariに }else if(dist2 < 10){ mode = STATE_A2; //migiに }else{mode = STATE_A;} }*/if(dist1 > 45 && dist1<250){ fase = 1; t.stop(); t.reset(); t.start(); } break; case 1: if(timer == 0){ wait_flag = 1; mode = STATE_B; }else if(timer > 2 && dist1 > 60){ wait_flag = 1; mode = STATE_A; t.reset(); t.stop(); 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_A2 : 右に曲がる(左:正転 右:正転) case STATE_A2: myled = 1; // LED点灯 // 左モータの制御 mypwm4.pulsewidth_ms(50); mypwm6.pulsewidth_ms(0); // 右モータの制御 mypwm2.pulsewidth_ms(80); mypwm3.pulsewidth_ms(0); break; // STATE_A3 : hidariに曲がる(左:正転 右:正転) case STATE_A3: myled = 1; // LED点灯 // 左モータの制御 mypwm4.pulsewidth_ms(80); mypwm6.pulsewidth_ms(0); // 右モータの制御 mypwm2.pulsewidth_ms(10); 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; } } }