저세상 메이킹: [아두이노] 종이컵 분류기

오늘은 지난 시간에 사용한 라인트레이서를 개조해서 종이컵 분류기를 만들어보겠습니다.

준비물은 다음과 같습니다.

1. 지난 시간에 사용한 라인트레이서

2. 초음파 센서 하나 더

종이컵 분류기가 작동하는 방법은 다음과 같습니다.

하얀색 바탕, 검은색 테두리의 직사각형 내에서 움직임
키가 큰 종이컵을 쳐내고, 키가 작은 종이컵을 회피함
혹은 키가 큰 종이컵을 회피하고, 키가 작은 종이컵을 쳐냄

약간의 개조와 코드 수정을 통해 종이컵 분류기를 구현할 수 있습니다.

먼저 초음파 센서를 키가 큰 종이컵 높이에 맞춰 달아줍니다.

그리고 코드를 수정합니다. (끝)

아래는 수정된 코드입니다.

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

#define SPEED_GO 50 // 직진 속도
#define SPEED_TN 160 // 회전 속도
#define M1 1 // 좌측모터
#define M2 2 // 우측모터
#define SensorLeft 5 // 디지털 3: 좌측센서
#define SensorRight 6 // 디지+털 5: 우측센서
#define TRIG_H 8 // 위쪽 초음파센서
#define ECHO_H 11
#define TRIG_L 12 // 아래쪽 초음파센서
#define ECHO_L 13

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

int deg = 140;
int a = 15;
int DIS = 0; // 하단
int DIS2 = 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_H, OUTPUT);
pinMode(ECHO_H, INPUT);
pinMode(TRIG_L, OUTPUT);
pinMode(ECHO_L, INPUT);

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

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

void go_backward(){ // 후진
myMotor1->setSpeed(100); //50
myMotor2->setSpeed(100);
myMotor1->run(BACKWARD);
myMotor2->run(BACKWARD);
Serial.println("go backward");
}

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

void turn_left2(){
myMotor1->setSpeed(30);
myMotor2->setSpeed(30);
myMotor1->run(BACKWARD);
myMotor2->run(FORWARD);
Serial.println("turn left2");
]

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

void turn_right2(){
myMotor1->setSpeed(30);// 75
myMotor2->setSpeed(30);
myMotor1->run(FORWARD);
myMotor2->run(BACKWARD);
Serial.println("turn right2");
]

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 >= -600)
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 >=0)
SR = 1;
else
SR = 0;
}

void detect_sonic_L(){ // 하단 초음파센서 감지여부
servo.write(deg);
deg = deg - a;
if(deg <= 40 || deg >= 140){
a*=-1;
}
delay(25);
make_dist();
detect_sonic_H(); // 상단에서 걸리는 것이 없냐? 토스
}

void detect_sonic_H(){ // 상단 초음파센서 감지여부
unsigned long distance;
digitalWrite(TRIG_H,HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_H,LOW);
distance = pulseIn(ECHO_H,HIGH)/58.2;
DIS2 = distance;
}

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

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

void response(){
if (SL == 0 && SR == 0){ // 사각형 내에서
if (DIS <= 20){ // 낮은 위치 감지
if (DIS2 <= 10){ // 높은 위치 감지
if (deg < 85){ // 오른쪽에 장애물 위치
go_backward();
delay(100);
turn_left2();
delay(100);
go_forward();
delay(100);
turn_left();
}
else if (deg > 95){ // 왼쪽에 장애물 위치
go_backward();
delay(100);
turn_right2();
delay(100);
go_forward();
delay(100);
turn_right();
}
else if (deg >= 85 && deg <= 95)
brake();
}
else{
go_forward();
}
}

else
go_forward();
}

else if (SL == 1 && SR == 0){ // 사각형에서 이탈
if (DIS <= 20){
if (deg < 85)
turn_left();
else if (deg > 95)
turn_right();
else if (deg >= 85 && deg <= 95)
brake();
}
else
turn_right();
}

else if (SL == 0 && SR == 1){ // 사각형에서 이탈
if (DIS <= 20){
if (deg < 85)
turn_left();
else if (deg > 95)
turn_right();
else if (deg >= 85 && deg <= 95)
brake();
}
else
turn_left();
}

else{
go_backward();
delay(1000);
turn_left2(); // 동시에 감지되면 뒤로 물러나서 왼쪽으로 턴
}
}

사실, 구동이나 감지 부분은 이미 만들어 놓았기에 경우의 수를 조금 추가하면 됩니다.

어쨌든 최종적으로 저의 아두이노 로봇(?)은 이런 모양입니다.