6.2.4. 장애물을 스스로 피하는 자동차 만들기

CODE 6.6 | 장애물을 피하는 자동차 만들기

01	// 모터 제어 핀 설정
02	int rightMotor1 = 5;
03	int rightMotor2 = 6;
04	int leftMotor1 = 9;
05	int leftMotor2 = 10;
06
07	// 초음파 센서 핀 설정
08	int trigPin = 4;
09	int echoPin = 3;
10
11	void setup() {
12	// 모터 핀을 출력 모드로 설정
13	pinMode(rightMotor1, OUTPUT);
14	pinMode(rightMotor2, OUTPUT);
15	pinMode(leftMotor1, OUTPUT);
16	pinMode(leftMotor2, OUTPUT);
17
18	// 초음파 센서 핀 설정
19	pinMode(trigPin, OUTPUT);
20	pinMode(echoPin, INPUT);
21
22	Serial.begin(9600);
23	}
24
25	void loop() {
26	// 1. 초음파 센서로 거리 측정하기
27	digitalWrite(trigPin, LOW);
28	delayMicroseconds(2);
29	digitalWrite(trigPin, HIGH);
30	delayMicroseconds(10);
31	digitalWrite(trigPin, LOW);
32
33	long duration = pulseIn(echoPin, HIGH);
34	long distance = duration * 0.034 / 2;
35
36	// 2. 장애물이 20cm 안으로 가까워졌을 때 (후진 후 우회전)
37	if (distance > 0 && distance < 20) {
38	// 후진 (1초)
39	digitalWrite(rightMotor1, LOW);
40	digitalWrite(rightMotor2, HIGH);
41	digitalWrite(leftMotor1, LOW);
42	digitalWrite(leftMotor2, HIGH);
43	delay(1000);
44
45	// 오른쪽으로 제자리 회전 (0.5초)
46	digitalWrite(rightMotor1, LOW);
47	digitalWrite(rightMotor2, HIGH);
48	digitalWrite(leftMotor1, HIGH);
49	digitalWrite(leftMotor2, LOW);
50	delay(500);
51	}
52	// 3. 장애물이 없을 때 (전진)
53	else {
54	digitalWrite(rightMotor1, HIGH);
55	digitalWrite(rightMotor2, LOW);
56	digitalWrite(leftMotor1, HIGH);
57	digitalWrite(leftMotor2, LOW);
58	}
59	Serial.println(distance);
60	delay(50); // 센서가 안정적으로 측정하도록 짧은 대기 시간 추가
61	}