준비물은 다음과 같습니다.
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(); // 동시에 감지되면 뒤로 물러나서 왼쪽으로 턴
}
}
사실, 구동이나 감지 부분은 이미 만들어 놓았기에 경우의 수를 조금 추가하면 됩니다.
어쨌든 최종적으로 저의 아두이노 로봇(?)은 이런 모양입니다.
무게 중심을 맞추기 위해서 배터리를 가운데에 놓고 보드를 뒤쪽으로,
센서와 포크는 앞쪽으로 놓았는데도 약간씩 헛돌더군요;;
이 부분은 조금 수정해야할 것 같습니다.
이외에도 수정할 부분이 많지만 일단은 여기까지 쓰겠습니다...
