#8/12 아두이노 로봇 HowTo

자동 주행 알고리즘

전원을 넣고 나면 장애물을 피해서 배터리가 다 닳을 때까지 계속 이동하는 로봇이다. 우리가 가지고 있는 정보는 초음파 센서로 들어오는 정보에 의존을 한다. 그래서 loop 함수를 다음과 같이 작성을 했다.

​

초반에도 설명을 한 바와 같이 loop 함수는 무한 반복이 되는 함수다. 그 무한 반복되는 함수가 아래의 20여줄의 함수이며 이 함수에 의해서 로봇은 장애물을 계속 피해가며 움직이게 된다.

---

void loop() {

	Detect_Front(); // 정면 거리 측정

	if(distanceFront > STOP_DISTANCE) // 정의된 거리보다 크면 앞으로 전진하고 물체 확인
	{
		Robot_Go();
	} else { // 정면의 거리가 정의된 거리보다 작을 경우
		Detect_Object(); // 좌우의 거리를 구한다
		// 둘다 지정거리보다 작으면
		if (distanceRight < STOP_DISTANCE && distanceLeft < STOP_DISTANCE){
			Robot_180Turn(); // 180도 회전
		} else {
			if(distanceRight >= distanceLeft) // 오른쪽 거리가 더 멀면
				Robot_RightTurn(); // 우회전
			else // 왼쪽 거리가 더 멀면
				Robot_LeftTurn(); // 좌회전
		}
	}
}

---

​

 

Detect_Front();

어려운 알고리즘이 아니라 글로서 설명을 하고자 한다.

처음에 loop 함수가 시작이 되면 우선 정면에 장애물 까지의 거리를 Detect_Front 함수를 이용해 측정한다.

​

if(distanceFront > STOP_DISTANCE)

Detect_Front()는 거리를 측정해서 distanceFront라는 변수에 저장을 한다. 그래서 이 변수와 우리가 지정한 STOP_DISTANCE와 비교를 한다. Detect_Front 함수의 반환 값을 측정한 거리로 하고 그 반환값을 distanceFront가 받는 것이 정석이겠으나 다음에 나올 Detect_Object() 함수는 변수 두 개에 각각 측정값을 넣게 되어 있어 구조체 혹은 클래스 형태로 반환하도록 구현을 해야 하는데 초심자가 이해하기 어려울 듯 하여 전역 변수를 썼다.

측정한 거리가 STOP_DISTANCE 보다 크다면 다음의 {...}안에 있는 Robot_Go()를 수행한다.

그리고나서 else 문은 수행하지 않고 함수의 끝으로 이동을 한다. 만일 측정 거리가 STOP_DISTANCE 보다 작거나 같다면 else 구문을 수행하게 된다.

이 if문이 항상 참이라고 가정을 하면 측정 거리값은 줄어들면서 로봇은 계속 앞으로 전진을 한다. Robot_Go() 함수가 300미리 동안 전진하고 Robot_Stop()을 호출한다.

다음은 loop 맨 앞으로 이동을 해서 Detect_Front()를 호출해서 정면의 물체까지 거리를 측정한다. 물체를 측정할 때 서보 모터를 정면 90도로 회전시키고 250미리 delay를 줬고, 거리를 측정할 때 20미리를 사용하므로 로봇의 동작은 300미리 시간만큼 이동하고 270미리 만큼은 서 있게 된다.

​

---

Detect_Object(); // 좌우의 거리를 구한다

// 둘다 지정거리보다 작으면
if (distanceRight < STOP_DISTANCE && distanceLeft < STOP_DISTANCE){
	Robot_180Turn(); // 180도 회전
} else {
	if(distanceRight >= distanceLeft) // 오른쪽 거리가 더 멀면
		Robot_RightTurn(); // 우회전
	else // 왼쪽 거리가 더 멀면
		Robot_LeftTurn(); // 좌회전
}

---

 

​else 구문

다음은 측정한 값이 STOP_DISTANCE보다 작을 경우에 해당하는 else 구문이다. 이 구문을 만났다는 것은 로봇이 앞으로 전진할 공간이 없음을 뜻한다. 그래서 우회전, 좌회전 또는 유턴의 세 가지를 선택할 수 있다.

else 구문의 첫번째는 좌우의 거리를 측정하는 Detect_Object() 함수를 호출해서 distanceRight와 distanceLeft에 거리값을 얻어오는 것이다. 서보 모터가 좌우로 이동하면서 거리를 측정한다.

측정을 한 후에는 if ~ else ~ 구문이 있다.

​

if (distanceRight < STOP_DISTANCE && distanceLeft < STOP_DISTANCE)

좌우의 거리가 모두 지정된 거리보다 가까운데 물체가 있다는 것이므로 정면과 좌, 우측으로 이동을 할 공간이 없다. 따라서 유턴을 하는 180도 회전함수 Robot_180Turn()을 이용해 유턴을 하고 함수의 마지막으로 이동을 한다. 무한 반복 함수이므로 loop의 맨 앞으로 이동을 하면 다시 정면의 물체까지의 거리를 측정한다. Robot_180Turn() 이전에는 뒷쪽이었던 것이 유턴을 하고 나면 다시 정면이 되니 거리를 측정하는 것이다.

​

else 구문

좌우의 공간 중 최소한 하나 이상이 STOP_DISTANCE 보다 거리가 먼 경우에 해당을 한다. 둘 다 STOP_DISTANCE보다 멀 수도 있고 둘 중의 하나만 STOP_DISTANCE보다 거리가 멀 수도 있다. 어쨌든 둘 중의 하나는 STOP_DISTANCE보다 거리가 멀다는 얘기다.

여기서 distanceRight과 distanceLeft를 비교해서 distanceRight이 distanceLeft보다 크거나 같으면 우회전(Robot_RightTurn()) 작으면 좌회전(Robot_LeftTurn())을 수행하고 함수의 마지막으로 이동을 한다.

​

최종 결과물

지금까지 살펴본 코드는 다음과 같다. 로봇의 자율 주행 코드라고 하기에는 분량이 얼마 되지 않는다. 지금까지 해 본 로봇을 실행을 시켜보도 무엇이 부족한지 살펴보도록 하자.

 

---

​

/******************************************************
* AutoRobot
* made by Sohyemini
* 2020.10.04
******************************************************/

#include <Servo.h>

/*
* 두 개의 바퀴를 위한 디지털 Input/output D1에서 D4까지 선언
* D1, D2는 왼쪽 바퀴, D3, D4는 오른쪽 바퀴임
*/

int LeftWheel_D1 = 2;
int LeftWheel_D2 = 3;
int RightWheel_D3 = 4;
int RightWheel_D4 = 5;

// 초음파 센서를 움직이기 위한 서보 모터 핀 번호
int ServoPin = 6;
Servo servo;

// 정면, 좌, 우의 물체 거리를 저장할 변수
float distanceFront;
float distanceLeft;
float distanceRight;

// 초음파 센서 각각의 핀 번호, 거리 측정을 위함
int echoPin = 9;
int trigPin = 8;

// 장애물이 이 범위에 들어오면 방향을 틀어야 함
// 단위는 센티미터
#define STOP_DISTANCE 40

/*
* 로봇의 이동을 담당할 함수 선언 및 delay 선언
*/
void Robot_Stop();
void Robot_Go();
void Robot_RightTurn();
void Robot_LeftTurn();
void Robot_180Turn();

void Detect_Front();
void Detect_Object();

/*
* 초기화 함수로 아두이노 시작하고 단 한번만 동작됨
*/

void setup() {
	// 디버깅 메시지를 보기 위한 Serial 초기화
	Serial.begin(115200);
	Serial.println("");

	// 휠의 핀 모드를 정의함
	pinMode(LeftWheel_D1, OUTPUT);
	pinMode(LeftWheel_D2, OUTPUT);
	pinMode(RightWheel_D3, OUTPUT);
	pinMode(RightWheel_D4, OUTPUT);

	// 모든 휠을 정지상태로 초기화 한다
	Robot_Stop();

	// 서보 모터 초기화
	servo.attach(ServoPin);
	servo.write(90); // 정면을 바라보도록 설정

	// 초음파 센서의 핀 모드 설정
	pinMode(trigPin, OUTPUT);
	pinMode(echoPin, INPUT);

	//1초 쉼
	delay(1000);
}

void loop() {
	Detect_Front(); // 정면 거리 측정
	if(distanceFront > STOP_DISTANCE) // 정의된 거리보다 크면 앞으로 전진하고 물체 확인
	{
		Robot_Go();
	} else { // 정면의 거리가 정의된 거리보다 작을 경우
		Detect_Object(); // 좌우의 거리를 구한다
		// 둘다 지정거리보다 작으면
		if (distanceRight < STOP_DISTANCE && distanceLeft < STOP_DISTANCE){		
			Robot_180Turn(); // 180도 회전
		}
		else {
			if(distanceRight >= distanceLeft) // 오른쪽 거리가 더 멀면
				Robot_RightTurn(); // 우회전
			else // 왼쪽 거리가 더 멀면
				Robot_LeftTurn(); // 좌회전
		}
	}
}

/*
* 로봇을 180도 회전시킨다
*/

void Robot_180Turn(){
	Serial.println("로봇 180도 회전");
	digitalWrite(LeftWheel_D1, HIGH);
	digitalWrite(LeftWheel_D2, LOW);
	digitalWrite(RightWheel_D3, LOW);
	digitalWrite(RightWheel_D4, HIGH);
	delay(530);	
	Robot_Stop();
	delay(250);
}

/*
* 로봇을 오른쪽으로 회전시킨다
*/
void Robot_RightTurn(){
	Serial.println("로봇 우회전");
	digitalWrite(LeftWheel_D1, LOW);
	digitalWrite(LeftWheel_D2, HIGH);
	digitalWrite(RightWheel_D3, HIGH);
	digitalWrite(RightWheel_D4, LOW);
	delay(300);
	Robot_Stop();
	delay(250);
}

​
/*
* 로봇을 왼쪽으로 회전시킨다
*/
void Robot_LeftTurn(){
	Serial.println("로봇 좌회전");
	digitalWrite(LeftWheel_D1, HIGH);
	digitalWrite(LeftWheel_D2, LOW);
	digitalWrite(RightWheel_D3, LOW);
	digitalWrite(RightWheel_D4, HIGH);
	delay(300);
	Robot_Stop();
	delay(250);
}

/*
* 로봇을 정지킨다
*/
void Robot_Stop(){
	Serial.println("로봇 멈춤");
	digitalWrite(LeftWheel_D1, HIGH);
	digitalWrite(LeftWheel_D2, HIGH);
	digitalWrite(RightWheel_D3, HIGH);
	digitalWrite(RightWheel_D4, HIGH);
}

/*
* 로봇이 앞으로 간다
*/
void Robot_Go(){
	Serial.println("로봇 전진");
	digitalWrite(LeftWheel_D1, HIGH);
	digitalWrite(LeftWheel_D2, LOW);
	digitalWrite(RightWheel_D3, HIGH);
	digitalWrite(RightWheel_D4, LOW);
	delay(300);
	Robot_Stop();
}

/*
* 로봇을 뒤로 움직인다
*/
void Robot_Back(){
	Serial.println("로봇 후진");
	digitalWrite(LeftWheel_D1, LOW);
	digitalWrite(LeftWheel_D2, HIGH);
	digitalWrite(RightWheel_D3, LOW);
	digitalWrite(RightWheel_D4, HIGH);
}

/*
* 초음파 센서의 거리를 구하는 함수
*/
float sensor_distance() {
	float distance = 0;
	
	// 트리거 핀을 이용해 초음파를 내 보내고 대기 모드로 전환
	digitalWrite(trigPin, HIGH);
	delayMicroseconds(20);
	digitalWrite(trigPin, LOW);

	// 초음파가 들어온 시간, 마이크로 초
	// 초당 340 미터를 이동하는데 여기서는 마이크로 초이고
	// 왕복 거리이므로 2로 나눠준다.
	// 복잡하므로 58.5로 나눠도 된다.
	distance = pulseIn(echoPin, HIGH) / 58.8;
	return distance;
}


/*
* 좌, 우 거리를 각각 구한다
*/
void Detect_Object(){
	servo.write(0);
	delay(250);
	distanceRight = sensor_distance();

	servo.write(179);
	delay(250);
	distanceLeft = sensor_distance();
}

/*
* 정면의 거리를 구한다
*/
void Detect_Front(){
	servo.write(90);
	delay(250);
	distanceFront = sensor_distance();
}

 

---

아쉬운점

로봇의 주행을 수 차례 해 봤다면 아쉬운 점이 많을 것이다. 우선 주행을 계속했으면 좋겠는데 중간에 멈칫하는 경우가 좋아보이지 않는다. 그 다음으로는 회전 각도가 정확하지 않아 동작할 때 마다 미세하게 달라진다. 이동 속도가 앞에 장애물이 멀리 있을때는 빨리가다가 장애물이 가까워지면 속도를 천천히 낮췄으면 좋겠다. 가장 중요한 것은 초음파 센서가 감지하는 범위 문제로 바닥이나 얇은 물체가 있어 로봇이 감지를 못해 갈 수 없음에도 계속 전진을 하려고 한다는 점과 초음파 센서의 측정값이 부정확해 자주 물체와 충돌을 한다는 점이다.

​

서보 모터의 한계로 인해 거리를 측정하는데 시간이 꽤 소요된다. 이를 해결하기 위한 방법은 여러개의 초음파 센서를 장착하는 방법이 있을 수 있겠다. 실제로 웹 서핑을 하다보면 초음파 센서를 여러 개를 달고 있는 아두이노 RC Car를 찾을 수 있었다.

​

 

 

https://www.tindie.com/products/arielnh56/octosonarx2-connect-16-x-hc-sr04-to-arduino/

​

모터의 속도는 L298N으로 설정을 할 수 있다. 이를 구현하기 위해서는 초음파 센서를 여러개 달아서 짧은 시간 내에 장애물과의 거리를 측정할 수 있어야 한다.

L298N을 이용해 속도를 조절하는 방법은 D1 ~ D4의 좌우에 Enable 점퍼가 있다. 점퍼란 D1 ~ D4와 같이 하나의 핀이 아니라 두 개의 핀이 있어 두 핀을 연결하거나 단락시켜 어떤 정보를 설정하는 것이라고 할 수 있다. D1의 좌측에 있는 점퍼를 연결해 놓았는데 이는 좌측 모터를 사용한다는 의미이다. 우측 모터도 마찬가지로 enable되어 있다.

 

 

그림의 빨간색 부분에 있는 점퍼를 빼 내고 두 개의 핀 중에서 아래쪽 핀을 아두이노의 digital input에 연결을 하고 0~255 사이의 값을 출력해서 속도를 조절할 수 있다. 255가 최대 속도이고 0이 멈춤이다. 이 핀을 PWM이라고 하는데 Pulse Width Modulation이다.

그리고 정확한 각도로 움직이게 하기 위해서는 스테핑 모터를 사용하거나 또는 우리가 사용한 DC 모터에 엔코더라는 것을 붙여서 사용해야 한다고 한다.

​

얇은 물체는 감지하기가 쉽지 않을 수 있겠으나 바닥에 있는 물체, 로봇의 바퀴가 걸려서 이동할 수 없을 정도의 물체는 어떻게 감지하느냐는 적외선 센서를 사용하면 해결할 수 있을 것이다. 적외선 센서가 일반적으로 30센티까지의 거리를 측정할 수 있다고 하니 로봇의 하단부에 설치해서 거리를 측정하는데 사용하면 유용할 것 같다. SZH-SSBH-002라는 적외선 센서는 2 ~ 30cm까지 측정이 가능하다고 하고 보드에 있는 가변저항을 조절하여 그 거리를 설정할 수 있다고 한다. 그리고 거리 값을 받는 것이 아니고 가변저항에 의해서 설정된 거리에 물체가 있으면 연결된 핀이 HIGH가 되고 그렇지 않으면 LOW가 된다고 한다. 아주 기본적인 센서이다.

​

 

 

SZH-SSBH-002 센서

​

이 센서를 로봇의 전면 하단부에 장착을 하고 매번 loop 함수 시작시에 전면 낮은 부분에 초음파 센서가 감지 못하는 장애물이 있는지 감지하도록 설정을 하면 좋겠다.

​