ライントレース(テスト)

Dependencies:   mbed

main.cpp

Committer:
kato8
Date:
2017-11-13
Revision:
0:3a6873f917b6

File content as of revision 0:3a6873f917b6:

#include "mbed.h"

DigitalOut myled(LED3); // 確認用LED

AnalogIn photoref_L(A1); // フォトリフレクタ左
AnalogIn photoref_M(A2); // フォトリフレクタ中央
AnalogIn photoref_R(A3); // フォトリフレクタ右

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

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

void Motor(char);

int main() {
  char mode = STATE_A;
  while(1) {
    if(photoref_M > 0.5) { // ライン中央:白
      myled = 1;
      mode = STATE_E;
      Motor(mode);
    }
    else {                 // ライン中央:黒
      myled = 0;
      mode = STATE_A;
      Motor(mode);
    }
  }
}

void Motor(char mode) {
  switch(mode) {
    // STATE_A : 前進(左:正転 右:正転)
    case STATE_A:
      // 左モータの制御
      mypwm4.pulsewidth_ms(8);
      mypwm5.pulsewidth_ms(0);
      // 右モータの制御
      mypwm2.pulsewidth_ms(8);
      mypwm3.pulsewidth_ms(0);
      break;

      // STATE_B : 右旋回(左:正転 右:逆転)
      case STATE_B:
         // 左モータの制御
         mypwm4.pulsewidth_ms(5);
         mypwm5.pulsewidth_ms(0);
         // 右モータの制御
         mypwm2.pulsewidth_ms(0);
         mypwm3.pulsewidth_ms(5);
         break;

      // STATE_C : 左旋回(左:逆転 右:正転)
     case STATE_C:
       // 左モータの制御
       mypwm4.pulsewidth_ms(0);
       mypwm5.pulsewidth_ms(5);
       // 右モータの制御
       mypwm2.pulsewidth_ms(5);
       mypwm3.pulsewidth_ms(0);
       break;

     // STATE_D : 後退(左:逆転 右:逆転)
     case STATE_D:
       // 左モータの制御
       mypwm4.pulsewidth_ms(0);
       mypwm5.pulsewidth_ms(5);
       // 右モータの制御
       mypwm2.pulsewidth_ms(0);
       mypwm3.pulsewidth_ms(5);
       break;

     // STATE_E : ブレーキ(左:ブレーキ 右:ブレーキ)
     case STATE_E:
       // 左モータの制御
       mypwm4.pulsewidth_ms(5);
       mypwm5.pulsewidth_ms(5);
       // 右モータの制御
       mypwm2.pulsewidth_ms(5);
       mypwm3.pulsewidth_ms(5);
       break;
  }
}