Rui Kato
/
linetrace_test
ライントレース(テスト)
main.cpp@0:3a6873f917b6, 2017-11-13 (annotated)
- Committer:
- kato8
- Date:
- Mon Nov 13 05:38:57 2017 +0000
- Revision:
- 0:3a6873f917b6
????????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kato8 | 0:3a6873f917b6 | 1 | #include "mbed.h" |
kato8 | 0:3a6873f917b6 | 2 | |
kato8 | 0:3a6873f917b6 | 3 | DigitalOut myled(LED3); // 確認用LED |
kato8 | 0:3a6873f917b6 | 4 | |
kato8 | 0:3a6873f917b6 | 5 | AnalogIn photoref_L(A1); // フォトリフレクタ左 |
kato8 | 0:3a6873f917b6 | 6 | AnalogIn photoref_M(A2); // フォトリフレクタ中央 |
kato8 | 0:3a6873f917b6 | 7 | AnalogIn photoref_R(A3); // フォトリフレクタ右 |
kato8 | 0:3a6873f917b6 | 8 | |
kato8 | 0:3a6873f917b6 | 9 | PwmOut mypwm4(D4); |
kato8 | 0:3a6873f917b6 | 10 | PwmOut mypwm5(D6); |
kato8 | 0:3a6873f917b6 | 11 | PwmOut mypwm2(D2); |
kato8 | 0:3a6873f917b6 | 12 | PwmOut mypwm3(D3); |
kato8 | 0:3a6873f917b6 | 13 | |
kato8 | 0:3a6873f917b6 | 14 | #define STATE_A (0) |
kato8 | 0:3a6873f917b6 | 15 | #define STATE_B (1) |
kato8 | 0:3a6873f917b6 | 16 | #define STATE_C (2) |
kato8 | 0:3a6873f917b6 | 17 | #define STATE_D (3) |
kato8 | 0:3a6873f917b6 | 18 | #define STATE_E (4) |
kato8 | 0:3a6873f917b6 | 19 | |
kato8 | 0:3a6873f917b6 | 20 | void Motor(char); |
kato8 | 0:3a6873f917b6 | 21 | |
kato8 | 0:3a6873f917b6 | 22 | int main() { |
kato8 | 0:3a6873f917b6 | 23 | char mode = STATE_A; |
kato8 | 0:3a6873f917b6 | 24 | while(1) { |
kato8 | 0:3a6873f917b6 | 25 | if(photoref_M > 0.5) { // ライン中央:白 |
kato8 | 0:3a6873f917b6 | 26 | myled = 1; |
kato8 | 0:3a6873f917b6 | 27 | mode = STATE_E; |
kato8 | 0:3a6873f917b6 | 28 | Motor(mode); |
kato8 | 0:3a6873f917b6 | 29 | } |
kato8 | 0:3a6873f917b6 | 30 | else { // ライン中央:黒 |
kato8 | 0:3a6873f917b6 | 31 | myled = 0; |
kato8 | 0:3a6873f917b6 | 32 | mode = STATE_A; |
kato8 | 0:3a6873f917b6 | 33 | Motor(mode); |
kato8 | 0:3a6873f917b6 | 34 | } |
kato8 | 0:3a6873f917b6 | 35 | } |
kato8 | 0:3a6873f917b6 | 36 | } |
kato8 | 0:3a6873f917b6 | 37 | |
kato8 | 0:3a6873f917b6 | 38 | void Motor(char mode) { |
kato8 | 0:3a6873f917b6 | 39 | switch(mode) { |
kato8 | 0:3a6873f917b6 | 40 | // STATE_A : 前進(左:正転 右:正転) |
kato8 | 0:3a6873f917b6 | 41 | case STATE_A: |
kato8 | 0:3a6873f917b6 | 42 | // 左モータの制御 |
kato8 | 0:3a6873f917b6 | 43 | mypwm4.pulsewidth_ms(8); |
kato8 | 0:3a6873f917b6 | 44 | mypwm5.pulsewidth_ms(0); |
kato8 | 0:3a6873f917b6 | 45 | // 右モータの制御 |
kato8 | 0:3a6873f917b6 | 46 | mypwm2.pulsewidth_ms(8); |
kato8 | 0:3a6873f917b6 | 47 | mypwm3.pulsewidth_ms(0); |
kato8 | 0:3a6873f917b6 | 48 | break; |
kato8 | 0:3a6873f917b6 | 49 | |
kato8 | 0:3a6873f917b6 | 50 | // STATE_B : 右旋回(左:正転 右:逆転) |
kato8 | 0:3a6873f917b6 | 51 | case STATE_B: |
kato8 | 0:3a6873f917b6 | 52 | // 左モータの制御 |
kato8 | 0:3a6873f917b6 | 53 | mypwm4.pulsewidth_ms(5); |
kato8 | 0:3a6873f917b6 | 54 | mypwm5.pulsewidth_ms(0); |
kato8 | 0:3a6873f917b6 | 55 | // 右モータの制御 |
kato8 | 0:3a6873f917b6 | 56 | mypwm2.pulsewidth_ms(0); |
kato8 | 0:3a6873f917b6 | 57 | mypwm3.pulsewidth_ms(5); |
kato8 | 0:3a6873f917b6 | 58 | break; |
kato8 | 0:3a6873f917b6 | 59 | |
kato8 | 0:3a6873f917b6 | 60 | // STATE_C : 左旋回(左:逆転 右:正転) |
kato8 | 0:3a6873f917b6 | 61 | case STATE_C: |
kato8 | 0:3a6873f917b6 | 62 | // 左モータの制御 |
kato8 | 0:3a6873f917b6 | 63 | mypwm4.pulsewidth_ms(0); |
kato8 | 0:3a6873f917b6 | 64 | mypwm5.pulsewidth_ms(5); |
kato8 | 0:3a6873f917b6 | 65 | // 右モータの制御 |
kato8 | 0:3a6873f917b6 | 66 | mypwm2.pulsewidth_ms(5); |
kato8 | 0:3a6873f917b6 | 67 | mypwm3.pulsewidth_ms(0); |
kato8 | 0:3a6873f917b6 | 68 | break; |
kato8 | 0:3a6873f917b6 | 69 | |
kato8 | 0:3a6873f917b6 | 70 | // STATE_D : 後退(左:逆転 右:逆転) |
kato8 | 0:3a6873f917b6 | 71 | case STATE_D: |
kato8 | 0:3a6873f917b6 | 72 | // 左モータの制御 |
kato8 | 0:3a6873f917b6 | 73 | mypwm4.pulsewidth_ms(0); |
kato8 | 0:3a6873f917b6 | 74 | mypwm5.pulsewidth_ms(5); |
kato8 | 0:3a6873f917b6 | 75 | // 右モータの制御 |
kato8 | 0:3a6873f917b6 | 76 | mypwm2.pulsewidth_ms(0); |
kato8 | 0:3a6873f917b6 | 77 | mypwm3.pulsewidth_ms(5); |
kato8 | 0:3a6873f917b6 | 78 | break; |
kato8 | 0:3a6873f917b6 | 79 | |
kato8 | 0:3a6873f917b6 | 80 | // STATE_E : ブレーキ(左:ブレーキ 右:ブレーキ) |
kato8 | 0:3a6873f917b6 | 81 | case STATE_E: |
kato8 | 0:3a6873f917b6 | 82 | // 左モータの制御 |
kato8 | 0:3a6873f917b6 | 83 | mypwm4.pulsewidth_ms(5); |
kato8 | 0:3a6873f917b6 | 84 | mypwm5.pulsewidth_ms(5); |
kato8 | 0:3a6873f917b6 | 85 | // 右モータの制御 |
kato8 | 0:3a6873f917b6 | 86 | mypwm2.pulsewidth_ms(5); |
kato8 | 0:3a6873f917b6 | 87 | mypwm3.pulsewidth_ms(5); |
kato8 | 0:3a6873f917b6 | 88 | break; |
kato8 | 0:3a6873f917b6 | 89 | } |
kato8 | 0:3a6873f917b6 | 90 | } |