28/05/2026
/*
หุ่นยนต์สำหรับเดินในเขาวงกต - ใช้อัลกอริทึมติดตามกำแพง
วิธีการทำงาน:
- ใช้เซนเซอร์ IR 2 ตัว สำหรับตรวจจับสิ่งกีดขวาง
- ใช้กฎมือขวา (Right-Hand Rule) ในการแก้เขาวงกต
- หุ่นยนต์จะเดินไปชิดกำแพงแล้วติดตามกำแพงเพื่อหาทางออก
การเชื่อมต่อ:
- motor2: มอเตอร์ซ้าย
- motor3: มอเตอร์ขวา
- ir_left (pin 10): เซนเซอร์ IR ซ้าย
- ir_right (pin 9): เซนเซอร์ IR ขวา
หลักการ:
- เมื่อไม่มีสิ่งกีดขวาง = เดินตรง
- เมื่อมีสิ่งกีดขวางด้านซ้าย = เลี้ยวขวา
- เมื่อมีสิ่งกีดขวางด้านขวา = เลี้ยวซ้าย
- เมื่อเจอทางตัน (มีสิ่งกีดขวางทั้งสองข้าง) = หันกลับ 180 องศา
*/
AF_DCMotor motor1(1, MOTOR12_1KHZ);
AF_DCMotor motor2(2, MOTOR12_1KHZ); // มอเตอร์ซ้าย
AF_DCMotor motor3(3, MOTOR34_1KHZ); // มอเตอร์ขวา
AF_DCMotor motor4(4, MOTOR34_1KHZ);
const int ir_left = 10; // เซนเซอร์ IR ซ้าย
const int ir_right = 9; // เซนเซอร์ IR ขวา
// int speedSet_L = 150; // ความเร็วมอเตอร์ซ้าย (0-255)
// int speedSet_R = 140; // ความเร็วมอเตอร์ขวา (0-255)
int speedSet_L = 120; // ความเร็วมอเตอร์ซ้าย (0-255)
int speedSet_R = 110; // ความเร็วมอเตอร์ขวา (0-255)
void setup() {
Serial.begin(11520);
pinMode(ir_left, INPUT);
pinMode(ir_right, INPUT);
Serial.println("Robot Ready");
delay(3000);
}
void loop() {
bool leftObstacle = (digitalRead(ir_left) == LOW);
bool rightObstacle = (digitalRead(ir_right) == LOW);
if (leftObstacle && rightObstacle) {
// กรณีที่ 1: เจอทางตัน หรือกำแพงบีบทั้งสองข้าง
moveBackward(); delay(200); // ถอยหลังนิดนึงกันติด
turnAround(); delay(900);
}
else if (leftObstacle) {
// กรณีที่ 2: เจอกำแพงซ้าย -> ต้องเลี้ยวขวาหนีกำแพง
turnRight();
delay(300);
}
else if (rightObstacle) {
// กรณีที่ 3: เจอกำแพงขวา -> ต้องเลี้ยวซ้ายหนีกำแพง
turnLeft();
delay(300);
}
else {
// กรณีที่ 4: ทางสะดวก -> เดินหน้า
moveForward();
}
delay(50);
}
void moveStop() {
motor2.run(RELEASE);
motor3.run(RELEASE);
}
void moveForward() {
motor2.setSpeed(speedSet_L);
motor3.setSpeed(speedSet_R);
motor2.run(FORWARD);
motor3.run(FORWARD);
}
void moveBackward() {
motor2.setSpeed(speedSet_L);
motor3.setSpeed(speedSet_R);
motor2.run(BACKWARD);
motor3.run(BACKWARD);
}
void turnRight() {
motor2.setSpeed(speedSet_L-30);
motor3.setSpeed(speedSet_R-30);
motor2.run(FORWARD);
motor3.run(BACKWARD);
}
void turnLeft() {
motor2.setSpeed(speedSet_L-30);
motor3.setSpeed(speedSet_R-30);
motor2.run(BACKWARD);
motor3.run(FORWARD);
}
void turnAround() {
// หันกลับ 180 องศา โดยการหมุนซ้าย
motor2.setSpeed(speedSet_L);
motor3.setSpeed(speedSet_R);
motor2.run(FORWARD);
motor3.run(BACKWARD);
}
