[아두이노] 라인트레이서 제작

지금까지 다룬 센서들을 이용해 라인트레이서를 만들어 보겠습니다.

아래의 준비물이 필요합니다.

1. IR 센서 2개

2. 초음파센서 1개

3. 서보모터 1개

4. DC모터 2개

5. 바퀴 2개

6. 회전바퀴 1개

7. 나무판 (혹은 아무거나 몸체가 될만한 것)


8. 18650배터리 2개

9. 배터리 케이스 2구

라인트레이서가 작동하는 환경은 다음과 같습니다.

• 하얀색 바탕, 검은색 차선 사이를 움직임
• 차선 사이에 종이컵 장애물이 있음
• 코스의 마지막에 벽이 있음

라인트레이서는 검은 차선 사이를 움직이며 종이컵을 회피해야 하고

마지막에 벽을 만나면 동작을 멈추어야합니다.

라인트레이서의 동작을 제어하기 위해 필요한 함수를 정의합니다.

<구동계>

go_forward() : "직진"; 왼쪽과 오른쪽의 DC모터 속도를 정하고 앞으로 가도록 합니다.

turn_left() : "제자리 좌회전"; 왼쪽 모터는 뒤로, 오른쪽 모터는 앞으로 가도록 합니다.

turn_right() : "제자리 우회전"; 왼쪽 모터는 앞으로, 오른쪽 모터는 뒤로 가도록 합니다.

brake() : "정지"; 왼쪽과 오른쪽의 DC모터를 정지시킵니다.

<센서류>

detect_left() : 좌측 검은선의 감지 함수입니다. 센서가 검은색 물체를 보게되면 값이 변합니다.

이 때의 변화지점을 기준으로 0과 1을 지정해 감지 여부를 정합니다.

IR센서는 주변 환경에 영향을 받으므로 측정되는 값은 신호와 노이즈가 섞인 형태입니다.

(신호+노이즈) - (노이즈) 구조로 작동하도록 해 신호만을 계산합니다.

detect_right() : 우측 검은선의 감지 함수입니다. detect_left()와 동일합니다.

make_dist() : 초음파로 측정되는 값을 cm로 변환하는 함수입니다.

detect_sonic() : 서보모터가 움직이는 함수입니다.

20도에서 160도 사이를 반복적으로 움직이도록 하고 make_dist() 함수를 사용하도록 합니다.

<동작 경우의 수>

response() : 센서의 값들을 모아서 각 경우에 맞게 분류해줍니다.

차선 사이를 지나면서 장애물을 피하고 벽을 보면 멈추도록 경우를 나누어 줍니다.


아래는 제가 작성한 코드입니다.


#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"
#include <Servo.h>

#define SPEED_GO 50   // 직진 속도
#define SPEED_TN 75   // 회전 속도
#define M1  1         // 좌측모터
#define M2  2         // 우측모터
#define SensorLeft  5 // 디지털 3: 좌측센서
#define SensorRight 6 // 디지털 5: 우측센서
#define TRIG  11      // 디지털 11
#define ECHO  12      // 디지털 12

int SL = 0;           // 좌측센서 상태
int SR = 0;           // 우측센서 상태

int deg = 160;        // 초기 각도
int a   = 15;         // 서보모터 속도
int DIS = 0;          // 거리

Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *myMotor1 = AFMS.getMotor(M1);
Adafruit_DCMotor *myMotor2 = AFMS.getMotor(M2);
Servo servo;

void setup() {
  Serial.begin(9600);
  Serial.println("::: Let's Go Line Tracer!! :::");

  pinMode(SensorLeft, OUTPUT);
  pinMode(SensorRight, OUTPUT);
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);

  AFMS.begin();
  servo.attach(10);
}

void go_forward(){ // 직진
  myMotor1->setSpeed(SPEED_GO);
  myMotor2->setSpeed(SPEED_GO);
  myMotor1->run(FORWARD);
  myMotor2->run(FORWARD);
  Serial.println("go forward");
}

void turn_left(){ // 제자리 좌회전
  myMotor1->setSpeed(SPEED_TN);
  myMotor2->setSpeed(SPEED_TN);
  myMotor1->run(BACKWARD);
  myMotor2->run(FORWARD);
  //delay(100);
  Serial.println("turn left"); 
}

void turn_right(){ // 제자리 우회전
  myMotor1->setSpeed(SPEED_TN);
  myMotor2->setSpeed(SPEED_TN);                                                                                     
  myMotor1->run(FORWARD);
  myMotor2->run(BACKWARD);
  //delay(100);
  Serial.println("turn right"); 
}

void brake(){ // 정지
  myMotor1->run(RELEASE);
  myMotor2->run(RELEASE);
  Serial.println("brake");
}

void detect_left(){ // 좌측센서 감지여부
  int a, b, c;
 
  digitalWrite(SensorLeft, HIGH); // 신호+노이즈
  delayMicroseconds(400);
  a = analogRead(A3);
 
  digitalWrite(SensorLeft, LOW); // 노이즈
  delayMicroseconds(400);
  b = analogRead(A3);

  c = a - b;

  if(c <= 140)
    SL = 1;
  else
    SL = 0;
}

void detect_right(){ // 우측센서 감지여부
  int a, b, c;
 
  digitalWrite(SensorRight, HIGH); // 신호+노이즈
  delayMicroseconds(400);
  a = analogRead(A5);
 
  digitalWrite(SensorRight, LOW); // 노이즈
  delayMicroseconds(400);
  b = analogRead(A5);

  c = a - b;

  if(c <= 140)
    SR = 1;
  else
    SR = 0;
}

void detect_sonic(){ // 초음파센서 감지여부
  servo.write(deg);
  deg = deg - a;
  if(deg <= 20 || deg >= 160){
    a*=-1;
  }
  delay(25);
  make_dist();
}

unsigned long make_dist(){
  unsigned long distance;
  digitalWrite(TRIG,HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG,LOW);
  distance = pulseIn(ECHO,HIGH)/58.2;
  DIS = distance;
  return distance;
}

void loop() {
  detect_left();
  detect_right();
  detect_sonic();
  response();
}

void response(){
  if (SL == 0 && SR == 0){ // 센서 X
    if (DIS <= 20){
      if (deg < 85) // 왼쪽 장애물 (센서 X, 레이더 O)
        turn_left();
      else if (deg > 95) // 오른쪽 장애물 (센서 X, 레이더 O)
        turn_right();
      else if (deg >= 85 && deg <= 95)
        brake();
    }
    else
      go_forward();
  }

  else if (SL == 1 && SR == 0){ // 왼쪽 센서 O
    if (DIS <= 20){
      if (deg < 85) // 왼쪽 장애물 (센서 O, 레이더 O)
        turn_left();
      else if (deg > 95) // 오른쪽 장애물 (센서 O, 레이더 O)
        turn_right();
      else if (deg >= 85 && deg <= 95)
        brake();
    }
    else
      turn_right();
  }
 
  else if (SL == 0 && SR == 1){ // 오른쪽 센서 O
    if (DIS <= 20){
      if (deg < 85) // 왼쪽 장애물 (센서 O, 레이더 O)
        turn_left();
      else if (deg > 95) // 오른쪽 장애물 (센서 O, 레이더 O)
        turn_right();
      else if (deg >= 85 && deg <= 95)
        brake();       
    }
    else
      turn_left();
  }

  else
    brake(); 
}


실행 결과... 차선을 잘 따라가고 장애물은 잘 피하나, 벽을 만났을 때 멈추지를 못하네요;;

다음 시간에 이 부분을 조금 더 손봐야할 듯 하네요...