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

Dependencies:   mbed

Fork of linetrace_test by Rui Kato

main.cpp

Committer:
kato8
Date:
2017-12-04
Revision:
2:7b84f6d2cf63
Parent:
1:3af3d6834515

File content as of revision 2:7b84f6d2cf63:

#include "mbed.h"

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) // 左前進
#define STATE_F (5) // 右前進
#define STATE_G (6) // ブレーキ

void Motor(char);
void LineTrace(void);

int main() {
  while(1) {
    LineTrace();
  }
}

void LineTrace(void) {   // ラインの右側でライントレース
  if(photoref_M > 0.5) { // センサ中央:白
    Motor(STATE_E);      // 左前進
  } else {               // センサ中央:黒
    Motor(STATE_F);      // 右前進
  }
  wait_ms(50);
}

void Motor(char mode) {
  switch(mode) {
    case STATE_A: // 前進
      // 左モータの制御
      mypwm4.pulsewidth_ms(8);
      mypwm5.pulsewidth_ms(0);
      // 右モータの制御
      mypwm2.pulsewidth_ms(8);
      mypwm3.pulsewidth_ms(0);
      break;
    case STATE_B: // 右旋回
      // 左モータの制御
      mypwm4.pulsewidth_ms(5);
      mypwm5.pulsewidth_ms(0);
      // 右モータの制御
      mypwm2.pulsewidth_ms(0);
      mypwm3.pulsewidth_ms(5);
      break;
    case STATE_C: // 左旋回
      // 左モータの制御
      mypwm4.pulsewidth_ms(0);
      mypwm5.pulsewidth_ms(5);
      // 右モータの制御
      mypwm2.pulsewidth_ms(5);
      mypwm3.pulsewidth_ms(0);
      break;
    case STATE_D: // 後退
      // 左モータの制御
      mypwm4.pulsewidth_ms(0);
      mypwm5.pulsewidth_ms(5);
      // 右モータの制御
      mypwm2.pulsewidth_ms(0);
      mypwm3.pulsewidth_ms(5);
      break;
    case STATE_E: // 左前進
      // 左モータの制御
      mypwm4.pulsewidth_ms(5);
      mypwm5.pulsewidth_ms(5);
      // 右モータの制御
      mypwm2.pulsewidth_ms(5);
      mypwm3.pulsewidth_ms(0);
      break;
    case STATE_F: // 右前進
      // 左モータの制御
      mypwm4.pulsewidth_ms(5);
      mypwm5.pulsewidth_ms(0);
      // 右モータの制御
      mypwm2.pulsewidth_ms(5);
      mypwm3.pulsewidth_ms(5);
      break;
    case STATE_G: // ブレーキ
      // 左モータの制御
      mypwm4.pulsewidth_ms(5);
      mypwm5.pulsewidth_ms(5);
      // 右モータの制御
      mypwm2.pulsewidth_ms(5);
      mypwm3.pulsewidth_ms(5);
      break;
  }
}