SS2
/
Nucleo_SS2_No2
2号機
Fork of Nucleo_SS2_No2 by
main.cpp
- Committer:
- kato8
- Date:
- 2017-12-19
- Revision:
- 6:d264f7f2b781
- Parent:
- 5:ab74616c24b4
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); }