เดินตามเส้น มอบคอมพิวเตอร์ cp

///////////////////////////////////////////////////////////////////////////////////////
// Robot 2WD Line Tracking Cross Version 2.3 //
// Create : Mr.Chartthai Santhanaboun by #EarthElectronics #ThaiRoboShop #ThaiRobo3D //
// Date : 12/08/2024 //
// Hardware : Robot 2WD, Arduino Nano, Line Sensor Module //
// Software : Version 2.3 //
///////////////////////////////////////////////////////////////////////////////////////

#include <EEPROM.h> // เรียกใช้งานไลบรารี EEPROM

//L298 Connection
const int motor_IN1 = 11; // กำหนดขา GPIO ของบอร์ด L298N ที่เชื่อมต่อกับ UNO
const int motor_IN2 = 6;
const int motor_IN3 = 5;
const int motor_IN4 = 3;

//Line Tracking Sensor
const int line_1 = A0; // กำหนดขา GPIO ของเซ็นเซอร์ตรวจจับเส้น ที่เชื่อมต่อกับ UNO
const int line_2 = A1;
const int line_3 = A2;
const int line_4 = A3;
const int line_5 = A4;
const int line_6 = A5;
const int line_7 = A6;
const int line_8 = A7;

//Switch Start
const int switch_start = 12; // กำหนดขา GPIO ของบอร์ด Switch Module ที่เชื่อมต่อกับ UNO

// Pin Passive Buzzer
const int buzzer = 4; // กำหนดขา GPIO ของบอร์ด ลำโพง Buzzer

//Line Value
int lineValue_1, lineValue_2, lineValue_3, lineValue_4, // สร้างตัวแปรเก็บค่าการตรวจจับเส้นของเซ็นเซอร์แต่ละตำแหน่ง
lineValue_5, lineValue_6, lineValue_7, lineValue_8;

int midValue_1, midValue_2, midValue_3, midValue_4, // สร้างตัวแปรเก็บค่าแบ่งค่ากลางระหว่างขาวกับดำ
midValue_5, midValue_6, midValue_7, midValue_8;

int blackValue_1, blackValue_2, blackValue_3, blackValue_4, // สร้างตัวแปรเก็บค่าสีดำ
blackValue_5, blackValue_6, blackValue_7, blackValue_8;

int whiteValue_1, whiteValue_2, whiteValue_3, whiteValue_4, // สร้างตัวแปรเก็บค่าสีขาว
whiteValue_5, whiteValue_6, whiteValue_7, whiteValue_8;

//Line Detect
const int black = 1; // สร้างตัวแปลเพื่อระบุค่าสีดำ
const int white = 0; // สร้างตัวแปลเพื่อระบุค่าสีขาว

//Speed motor
int speed_l = 200; int speed_r = 200; // ตั้งค่าความเร็วมอเตอร์ซ้ายและขวาสามารถตั้งค่าได้ตั้งแต่ 0-255
int offset_speed_1 = 10; // ตั้งค่าความต่างของความเร็วมอเตอร์แล้วแต่จะใช้ก็ได้ไม่ใช้ก็ได้
int offset_speed_2 = 30;
int offset_speed_3 = 50;

// Timer สำหรับหน่วงเวลา จะกำหนดกี่ตัวก็ได้
int time1 = 130; //ค่าหน่วงเวลา ใช้สำหรับการเดินหน้าของหุ่นยนต์
int time2 = 300; //ค่าหน่วงเวลา ใช้สำหรับการเลี้ยวของหุ่นยนต์
int time3 = 10; //ค่าหน่วงเวลา ใช้สำหรับคั้นคำสั่งในการเคลื่อนที่
int time4 = 1000; //ค่าหน่วงเวลา ใช้สำหรับการหยุดหุ่นยนต์เพื่อตรวจจับสีของสิ่งของ
int time5 = 400; //ค่าหน่วงเวลา ใช้สำหรับการถอยหลัง
int time6 = 50; //ค่าหน่วงเวลา ใช้สำหรับตอนเลี้ยวแต่ละแยก

void setup() {
Serial.begin(115200); // ประกาศ Buadrate เริ่มต้นอยู่ที่ 115200
Serial.println(“Robot_Ready”); // พิมพ์ข้อความโชว์ผ่านทาง Serial Monitor ว่า “Robot_Ready”

initial(); //Setting INPUT, OUTPUT

//////////////////////////////ตรวจเช็คการเข้าโหมดเก็บค่าเส้น///////////////////////////////////////////
if (digitalRead(switch_start) == 0) { //ถ้าเริ่มโปรแกรมปุ่มถูกกดแต่แรกจะเข้าโหมดเก็บค่าเส้น
write_eeprom(); //ฟังก์ชันเก็บค่าเส้นใหม่
}
else { //ถ้าเริ่มโปรแกรมมาปุ่มกดไม่ถูกกดโปรแกรมจะทำการดึงค่าเส้นที่บันทึกไว้มาใช้เลย
read_eeprom(); //ฟังก์ชันอ่านค่าเส้น
}
////////////////////////////////////////////////////////////////////////////////////////////////

wait_start(); //รอการกดปุ่ม Start เริ่มต้นการทำงาน

//////////////////////////////////////////////////////////////////////////////////
/////////////////////พื้นที่สำหรับเขียนโปรแกรมเดินตามเส้น//////////////////////////////
// forward();
// delay(1000);

forward_tracking();
left_tracking();
forward_tracking();
forward_tracking();
left_tracking();
forward_tracking();
forward_tracking();
left_tracking();
forward_tracking();
left_tracking();
forward_tracking();

// forward();
// delay(1000);
// STOP();

///////////////////////////////////////////////////////////////////////////////
/////////////////////////พื้นที่วางชื่อฟังก์ชั่นที่ใช้งานได้สำหรับคัดลอกนำไปวางทางด้านบน//////////
// forward_tracking(); // ฟังก์ชันการเดินหน้าไปด้วยตรวจจับเส้นไปด้วย
// right_tracking(); // ฟังก์ชันการเลี้ยวขวาจนกว่าจะเจอเส้น
// left_tracking(); // ฟังก์ชันการเลี้ยวซ้ายจนกว่าจะเจอเส้น
// forward(); // ฟังก์ชันเดินหน้า
// backward(); // ฟังก์ชันถอยหลัง
// left(); // ฟังก์ชันเลี้ยวซ้ายแบบลดความเร็ว
// right(); // ฟังก์ชันเลี้ยวขวาแบบลดความเร็ว
// turnLeft(); // ฟังก์ชันหมุนซ้าย 2 ล้อสวนทาง
// turnRight(); // ฟังก์ชันหมุนขวา 2 ล้อสวนทาง
// STOP();
///////////////////////ตัวอย่างท่าเดินถอยหลังแล้วกลับหลังหัน///////////////////////////////
// backward();delay(time5); // ถอยหลัง
// right_tracking(); // ฟังก์ชัน การเลี้ยวขวาตรงทางแยก
// forward_tracking(); // ฟังก์ชันการเดินหน้าไปด้วยตรวจจับเส้นไปด้วย

//////////////////////////////////////////////////////////////////////////////////
}

void loop() {
}

//ห้ามแก้
void initial() { //ฟังก์ชันตั้งค่าเริ่มต้นของ INPUT, OUTPUT
//Setting OUTPUT GPIO
pinMode(motor_IN1, OUTPUT); // ตั้งค่าให้มอเตอร์เป็น OUTPUT
pinMode(motor_IN2, OUTPUT);
pinMode(motor_IN3, OUTPUT);
pinMode(motor_IN4, OUTPUT);
pinMode(buzzer, OUTPUT); // ตั้งค่าให้ buzzer เป็น OUTPUT

//Setting INPUT GPIO
pinMode(switch_start, INPUT); // ตั้งค่าให้ปุ่มกดเป็น INPUT

//เริ่มต้นโปรแกรมให้หุ่นยนต์หยุดทำงานก่อน
analogWrite(motor_IN1, 0); analogWrite(motor_IN3, 0);
analogWrite(motor_IN2, 0); analogWrite(motor_IN4, 0);

tone1(); // ให้ buzzer ส่งเสียงแจ้งเตือน 1 ครั้ง
delay(300);
}

//ห้ามแก้
void memory_black() { //ฟังก์ชันเก็บค่าสีดำ
for (int i = 0; i <= 20; i++) {
blackValue_1 += analogRead(line_1);
blackValue_2 += analogRead(line_2);
blackValue_3 += analogRead(line_3);
blackValue_4 += analogRead(line_4);
blackValue_5 += analogRead(line_5);
blackValue_6 += analogRead(line_6);
blackValue_7 += analogRead(line_7);
blackValue_8 += analogRead(line_8);
delay(20);
}
blackValue_1 /= 20 * 4;
blackValue_2 /= 20 * 4;
blackValue_3 /= 20 * 4;
blackValue_4 /= 20 * 4;
blackValue_5 /= 20 * 4;
blackValue_6 /= 20 * 4;
blackValue_7 /= 20 * 4;
blackValue_8 /= 20 * 4;

tone1();tone1();

Serial.print(“Black : “); Serial.print(blackValue_1); Serial.print(‘\t’);
Serial.print(blackValue_2); Serial.print(‘\t’);
Serial.print(blackValue_3); Serial.print(‘\t’);
Serial.print(blackValue_4); Serial.print(‘\t’);
Serial.print(blackValue_5); Serial.print(‘\t’);
Serial.print(blackValue_6); Serial.print(‘\t’);
Serial.print(blackValue_7); Serial.print(‘\t’);
Serial.println(blackValue_8);
}

//ห้ามแก้
void memory_white() { //ฟังก์ชันเก็บค่าสีขาว
for (int i = 0; i <= 20; i++) {
whiteValue_1 += analogRead(line_1);
whiteValue_2 += analogRead(line_2);
whiteValue_3 += analogRead(line_3);
whiteValue_4 += analogRead(line_4);
whiteValue_5 += analogRead(line_5);
whiteValue_6 += analogRead(line_6);
whiteValue_7 += analogRead(line_7);
whiteValue_8 += analogRead(line_8);
delay(20);
}
whiteValue_1 /= 20 * 4;
whiteValue_2 /= 20 * 4;
whiteValue_3 /= 20 * 4;
whiteValue_4 /= 20 * 4;
whiteValue_5 /= 20 * 4;
whiteValue_6 /= 20 * 4;
whiteValue_7 /= 20 * 4;
whiteValue_8 /= 20 * 4;

tone1();tone1();

Serial.print(“White : “); Serial.print(whiteValue_1); Serial.print(‘\t’);
Serial.print(whiteValue_2); Serial.print(‘\t’);
Serial.print(whiteValue_3); Serial.print(‘\t’);
Serial.print(whiteValue_4); Serial.print(‘\t’);
Serial.print(whiteValue_5); Serial.print(‘\t’);
Serial.print(whiteValue_6); Serial.print(‘\t’);
Serial.print(whiteValue_7); Serial.print(‘\t’);
Serial.println(whiteValue_8);
}

//ห้ามแก้
void compare() { //ฟังก์ชันหาค่ากึ่งกลางระหว่างสีขาวกับดำ
midValue_1 = (blackValue_1 + whiteValue_1) / 2;
midValue_2 = (blackValue_2 + whiteValue_2) / 2;
midValue_3 = (blackValue_3 + whiteValue_3) / 2;
midValue_4 = (blackValue_4 + whiteValue_4) / 2;
midValue_5 = (blackValue_5 + whiteValue_5) / 2;
midValue_6 = (blackValue_6 + whiteValue_6) / 2;
midValue_7 = (blackValue_7 + whiteValue_7) / 2;
midValue_8 = (blackValue_8 + whiteValue_8) / 2;

EEPROM.write(21, midValue_1);
EEPROM.write(22, midValue_2);
EEPROM.write(23, midValue_3);
EEPROM.write(24, midValue_4);
EEPROM.write(25, midValue_5);
EEPROM.write(26, midValue_6);
EEPROM.write(27, midValue_7);
EEPROM.write(28, midValue_8);

Serial.print(“Mid : “); Serial.print(midValue_1); Serial.print(‘\t’);
Serial.print(midValue_2); Serial.print(‘\t’);
Serial.print(midValue_3); Serial.print(‘\t’);
Serial.print(midValue_4); Serial.print(‘\t’);
Serial.print(midValue_5); Serial.print(‘\t’);
Serial.print(midValue_6); Serial.print(‘\t’);
Serial.print(midValue_7); Serial.print(‘\t’);
Serial.println(midValue_8);
}

//ห้ามแก้
void write_eeprom() { //ฟังก์ชันบันทึกค่าเส้นใหม่
for (int i = 0 ; i < 30 ; i++) {
EEPROM.write(i, 0);
}
while (digitalRead(switch_start) == 0) {
;
}
tone1();
tone1();
tone1();

wait_start(); //รอการกดปุ่ม Start
memory_black(); // เก็บค่าสีดำ
wait_start(); //รอการกดปุ่ม Start
memory_white(); // เก็บค่าสีขาว
compare(); // วิเคราะห์ค่าเส้น

Serial.println(“Write EEPROM Complete”);
}

//ห้ามแก้
void read_eeprom() { //ฟังก์ชันอ่านค่าเส้นที่บันทึกไว้
midValue_1 = EEPROM.read(21);
midValue_2 = EEPROM.read(22);
midValue_3 = EEPROM.read(23);
midValue_4 = EEPROM.read(24);
midValue_5 = EEPROM.read(25);
midValue_6 = EEPROM.read(26);
midValue_7 = EEPROM.read(27);
midValue_8 = EEPROM.read(28);
tone1(); tone1(); tone1();
Serial.println(“Read EEPROM Complete”);
}

//ห้ามแก้
void wait_start() { //ฟังก์ชันรอการกดปุ่ม
while (digitalRead(switch_start) != 0) { //ติดลูปค้างไว้ถ้ายังไม่กดปุ่ม
;
}
Serial.println(“Push Switch”);
delay(300);
}

//ห้ามแก้
void readSensor() { //ฟังก์ชันอ่านค่าเซ็นเซอร์
lineValue_1 = analogRead(line_1)/4; lineValue_2 = analogRead(line_2)/4;
lineValue_3 = analogRead(line_3)/4; lineValue_4 = analogRead(line_4)/4;
lineValue_5 = analogRead(line_5)/4; lineValue_6 = analogRead(line_6)/4;
lineValue_7 = analogRead(line_7)/4; lineValue_8 = analogRead(line_8)/4;

if (lineValue_1 > midValue_1) {
lineValue_1 = black;
} else {
lineValue_1 = white;
}

if (lineValue_2 > midValue_2) {
lineValue_2 = black;
} else {
lineValue_2 = white;
}

if (lineValue_3 > midValue_3) {
lineValue_3 = black;
} else {
lineValue_3 = white;
}

if (lineValue_4 > midValue_4) {
lineValue_4 = black;
} else {
lineValue_4 = white;
}

if (lineValue_5 > midValue_5) {
lineValue_5 = black;
} else {
lineValue_5 = white;
}

if (lineValue_6 > midValue_6) {
lineValue_6 = black;
} else {
lineValue_6 = white;
}

if (lineValue_7 > midValue_7) {
lineValue_7 = black;
} else {
lineValue_7 = white;
}

if (lineValue_8 > midValue_8) {
lineValue_8 = black;
} else {
lineValue_8 = white;
}

Serial.print(“Line :”); Serial.print(‘\t’);
Serial.print(lineValue_1); Serial.print(‘\t’);
Serial.print(lineValue_2); Serial.print(‘\t’);
Serial.print(lineValue_3); Serial.print(‘\t’);
Serial.print(lineValue_4); Serial.print(‘\t’);
Serial.print(lineValue_5); Serial.print(‘\t’);
Serial.print(lineValue_6); Serial.print(‘\t’);
Serial.print(lineValue_7); Serial.print(‘\t’);
Serial.println(lineValue_8);
}

void forward_tracking() { //ฟังก์ชันแทร๊กเส้นไปข้างหน้า
while (1) { //ลูปในการอ่านเส้น จะวนลูปแทร๊กเส้นจนกว่าจะเจอแยก
readSensor(); //กระโดดไปฟังก์ชันอ่านค่าเส้น

/////////////////////////////เงื่อนไขที่ใช้ในการตรวจจับเส้น////////////////////////////////////////
/////////////////////สามารถเพิ่มลดเงื่อนไขได้ ตัวอย่างนี้ใช้เซ็นเซอร์ 6 ตำแหน่ง////////////////////////////////////////
if (lineValue_2 == white && lineValue_3 == white && lineValue_4 == white && lineValue_5 == white && lineValue_6 == white && lineValue_7 == white) {
forward();
}
else if (lineValue_2 == white && lineValue_3 == white && lineValue_4 == white && lineValue_5 == white && lineValue_6 == white && lineValue_7 == black) {
turnRight();
}
else if (lineValue_2 == white && lineValue_3 == white && lineValue_4 == white && lineValue_5 == white && lineValue_6 == black && lineValue_7 == black) {
turnRight();
}
else if (lineValue_2 == white && lineValue_3 == white && lineValue_4 == white && lineValue_5 == black && lineValue_6 == black && lineValue_7 == black) {
oneRight();
}
else if (lineValue_2 == white && lineValue_3 == white && lineValue_4 == white && lineValue_5 == white && lineValue_6 == black && lineValue_7 == white) {
oneRight();
}
else if (lineValue_2 == white && lineValue_3 == white && lineValue_4 == white && lineValue_5 == black && lineValue_6 == black && lineValue_7 == white) {
right();
}
else if (lineValue_2 == white && lineValue_3 == white && lineValue_4 == black && lineValue_5 == black && lineValue_6 == black && lineValue_7 == white) {
right();
}
else if (lineValue_2 == white && lineValue_3 == white && lineValue_4 == white && lineValue_5 == black && lineValue_6 == white && lineValue_7 == white) {
forward();
}
else if (lineValue_2 == white && lineValue_3 == white && lineValue_4 == black && lineValue_5 == black && lineValue_6 == white && lineValue_7 == white) {
forward();
}
else if (lineValue_2 == white && lineValue_3 == white && lineValue_4 == black && lineValue_5 == white && lineValue_6 == white && lineValue_7 == white) {
forward();
}
else if (lineValue_2 == white && lineValue_3 == black && lineValue_4 == black && lineValue_5 == black && lineValue_6 == white && lineValue_7 == white) {
left();
}
else if (lineValue_2 == white && lineValue_3 == black && lineValue_4 == black && lineValue_5 == white && lineValue_6 == white && lineValue_7 == white) {
left();
}
else if (lineValue_2 == white && lineValue_3 == black && lineValue_4 == white && lineValue_5 == white && lineValue_6 == white && lineValue_7 == white) {
oneLeft();
}
else if (lineValue_2 == black && lineValue_3 == black && lineValue_4 == black && lineValue_5 == white && lineValue_6 == white && lineValue_7 == white) {
oneLeft();
}
else if (lineValue_2 == black && lineValue_3 == black && lineValue_4 == white && lineValue_5 == white && lineValue_6 == white && lineValue_7 == white) {
turnLeft();
}
else if (lineValue_2 == black && lineValue_3 == white && lineValue_4 == white && lineValue_5 == white && lineValue_6 == white && lineValue_7 == white) {
turnLeft();
}
else {
forward();
}

//////////////////////////////////////////////////////////// เงื่อนไขเมื่อเจอทางแยก ////////////////////////////////////////////////////////////////
if (lineValue_2 == black && lineValue_3 == black && lineValue_4 == black && lineValue_5 == black && lineValue_6 == black && lineValue_7 == black) {
forward();
delay(time1);
STOP();
delay(time3); tone1();
// forward();
break;
}

else if (lineValue_1 == black && lineValue_2 == black && lineValue_3 == black && lineValue_4 == black) {
forward();
delay(time1);
STOP();
delay(time3); tone1();
// left_tracking();
break;
}

else if (lineValue_5 == black && lineValue_6 == black && lineValue_7 == black && lineValue_8 == black) {
forward();
delay(time1);
STOP();
delay(time3); tone1();
// right_tracking();
break;
}

}
}

void left_tracking() { //ฟังก์ชันแทร๊กเส้นเลี้ยวซ้ายจนเจอเส้นใหม่
turnLeft();
delay(time2);
while (analogRead(line_4)/4 < midValue_4) { //วนลูปเมื่อยังอ่านค่าเส้นเป็นสีขาว
turnLeft();
} delay(time6);
STOP();delay(time3); tone1();
}

void right_tracking() { //ฟังก์ชันแทร๊กเส้นเลี้ยวขวาจนเจอเส้นใหม่
turnRight();
delay(time2);
while (analogRead(line_5)/4 < midValue_5) { //วนลูปเมื่อยังอ่านค่าเส้นเป็นสีขาว
turnRight();
} delay(time6);
STOP();delay(time3); tone1();
}

void forward() { //ฟังก์ชันเดินหน้า
// Serial.println(“forward”);
analogWrite(motor_IN1, speed_l); analogWrite(motor_IN2, 0);
analogWrite(motor_IN3, speed_r); analogWrite(motor_IN4, 0);
}

void backward() { //ฟังก์ชันถอยหลัง
// Serial.println(“backward”);
analogWrite(motor_IN1, 0); analogWrite(motor_IN2, speed_l);
analogWrite(motor_IN3, 0); analogWrite(motor_IN4, speed_r);
}

void left() { //ฟังก์ชันเลี้ยวซ้ายช้าๆลดความเร็ว
// Serial.println(“left”);
analogWrite(motor_IN1, speed_l – offset_speed_3); analogWrite(motor_IN2, 0);
analogWrite(motor_IN3, speed_r – offset_speed_1); analogWrite(motor_IN4, 0);
}

void right() { //ฟังก์ชันเลี้ยวขวาช้าๆลดความเร็ว
// Serial.println(“right”);
analogWrite(motor_IN1, speed_l – offset_speed_1); analogWrite(motor_IN2, 0);
analogWrite(motor_IN3, speed_r – offset_speed_3); analogWrite(motor_IN4, 0);
}

void oneLeft() { //ฟังก์ชันเลี้ยวซ้ายล้อซ้ายหยุดล้อขวาหมุน
// Serial.println(“left”);
analogWrite(motor_IN1, 0); analogWrite(motor_IN2, 0);
analogWrite(motor_IN3, speed_r – offset_speed_1); analogWrite(motor_IN4, 0);
}

void oneRight() { //ฟังก์ชันเลี้ยวขวาล้อขวาหยุดล้อซ้ายหมุน
// Serial.println(“right”);
analogWrite(motor_IN1, speed_l – offset_speed_1); analogWrite(motor_IN2, 0);
analogWrite(motor_IN3, 0); analogWrite(motor_IN4, 0);
}

void turnLeft() { //ฟังก์ชันเลี้ยวซ้ายสองล้อสวนทางกัน
// Serial.println(“turnLeft”);
analogWrite(motor_IN1, 0); analogWrite(motor_IN2, speed_l – offset_speed_2);
analogWrite(motor_IN3, speed_r – offset_speed_2); analogWrite(motor_IN4, 0);
}

void turnRight() { //ฟังก์ชันเลี้ยวขวาสองล้อสวนทางกัน
// Serial.println(“turnRight”);
analogWrite(motor_IN1, speed_l – offset_speed_2); analogWrite(motor_IN2, 0);
analogWrite(motor_IN3, 0); analogWrite(motor_IN4, speed_r – offset_speed_2);
}

void STOP() { //ฟังก์ชันหยุด
analogWrite(motor_IN1, 255); analogWrite(motor_IN2, 255);
analogWrite(motor_IN3, 255); analogWrite(motor_IN4, 255);
}

void tone1() { //ฟังก์ชันสร้างเสียงให้ลำโพงดัง 1 ครั้ง
digitalWrite(buzzer, LOW);
delay(70);
digitalWrite(buzzer, HIGH);
delay(70);
}