Code โปรเจค หุ่นยนต์เดินตามเส้น 5 เซ็นเซอร์ BFD-1000

โปรเจค หุ่นยนต์เดินตามเส้น 5 เซ็นเซอร์ BFD-1000 อุปกรณ์ที่ต้องใช้ก็คือ

     1. 2WD Smart Car Robot Chassis Kits

     2. Arduino UNO R3 - Made in italy

     3. Motor Drive Module L298N

     4. สาย Jumper Female to Male ยาว 20cm.

     5. สาย Jumper Male to Male ยาว 20cm.

     6. รางถ่านแบบ 18650 ใส่ถ่าน 2 ก้อน

     7. แบตเตอรี่ลิเธียม 18650 จำนวน 2 ก้อน

     8. เสารองแผ่นพีซีบีโลหะแบบเหลี่ยม 6 mm

     9. สกรูหัวกลมน็อตตัวเมีย ขนาด 3มม ยาว12มม.

   
     10. 5 Channel Line Tracking Sensor Module (BFD-1000)

     11. สกรูหัวกลมน็อตตัวเมีย ขนาด 3มม ยาว35 มม.

 เริ่มต้นด้วยการ ประกอบ 2WD Smart Car Robot Chassis Kits

เชื่อมต่อ รางถ่าน 18650 กับ Motor Drive Module L298N
และ Motor Drive Module L298N กับ Arduino UNO R3

ภาพต่อวงจรทั้งหมดตามรูป

หมายเหตุ : ที่ Motor Drive Module L298N ถ้ามี Jumper อยู่ที่ขา ENA และ ENB ของ บอร์ด L298N  ให้ถอดออก

หลังจากนั้นให้ทดสอบเบื้องต้น ว่าการหมุนของล้อถูกต้องหรือไม่ โดย

เปิดโปรแกรม Arduino (IDE) และ Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3



// Motor A pins (enableA = enable motor, pinA1 = forward, pinA2 = backward)
int enableA = 3;
int pinA1 = 6;
int pinA2 = 7;

//Motor B pins (enabledB = enable motor, pinB2 = forward, pinB2 = backward)
int enableB = 5;
int pinB1 = 8;
int pinB2 = 9;

//This lets you run the loop a single time for testing
boolean run = true;

void setup() {
 pinMode(enableA, OUTPUT);
 pinMode(pinA1, OUTPUT);
 pinMode(pinA2, OUTPUT);

 pinMode(enableB, OUTPUT);
 pinMode(pinB1, OUTPUT);
 pinMode(pinB2, OUTPUT);
}
void loop() {
  if (run) {
    delay(2000);
    enableMotors();
    //Go forward
    forward(200);
    //Go backward
    backward(200);
    //Turn left
    turnLeft(400);
    coast(200);
    //Turn right
    turnRight(400);
    coast(200);
    //This stops the loop
    run = false; 
  }
}

//Define high-level H-bridge commands

void enableMotors()
{
 motorAOn();
 motorBOn();
}

void disableMotors()
{
 motorAOff();
 motorBOff();
}

void forward(int time)
{
 motorAForward();
 motorBForward();
 delay(time);
}

void backward(int time)
{
 motorABackward();
 motorBBackward();
 delay(time);
}

void turnLeft(int time)
{
 motorABackward();
 motorBForward();
 delay(time);
}

void turnRight(int time)
{
 motorAForward();
 motorBBackward();
 delay(time);
}

void coast(int time)
{
 motorACoast();
 motorBCoast();
 delay(time);
}

void brake(int time)
{
 motorABrake();
 motorBBrake();
 delay(time);
}
//Define low-level H-bridge commands

//enable motors
void motorAOn()
{
 digitalWrite(enableA, HIGH);
}

void motorBOn()
{
 digitalWrite(enableB, HIGH);
}

 //disable motors
void motorAOff()
{
 digitalWrite(enableB, LOW);
}

void motorBOff()
{
 digitalWrite(enableA, LOW);
}

 //motor A controls
void motorAForward()
{
 digitalWrite(pinA1, HIGH);
 digitalWrite(pinA2, LOW);
}

void motorABackward()
{
 digitalWrite(pinA1, LOW);
 digitalWrite(pinA2, HIGH);
}

//motor B controls
void motorBForward()
{
 digitalWrite(pinB1, HIGH);
 digitalWrite(pinB2, LOW);
}

void motorBBackward()
{
 digitalWrite(pinB1, LOW);
 digitalWrite(pinB2, HIGH);
}

//coasting and braking
void motorACoast()
{
 digitalWrite(pinA1, LOW);
 digitalWrite(pinA2, LOW);
}

void motorABrake()
{
 digitalWrite(pinA1, HIGH);
 digitalWrite(pinA2, HIGH);
}

void motorBCoast()
{
 digitalWrite(pinB1, LOW);
 digitalWrite(pinB2, LOW);
}

void motorBBrake()
{
 digitalWrite(pinB1, HIGH);
 digitalWrite(pinB2, HIGH);
}

ใส่ แบตเตอรี่ลิเธียม 18650 จำนวน 2 ก้อน ไปที่ รางถ่าน และ ตรวจสอบขั้วของแบตเตอรี่ ใส่ถุกต้องหรือไม่

โปรแกรมนี้จะทำงานเพียง 1 ครั้ง ถ้าต้องการทดลองใหม่ให้ถอด แบตเตอรี่ออก (หรือ ปิดเปิด สวิทช์ไฟใหม่) แล้วใส่เข้าไปใหม่ เมื่อล้อหมุน ตรวจสอบการหมุน ขอล้อต่างๆถูกต้องหรือไม่ ถ้าต่อวงจรถูกต้อง ล้อ ทั้งสองข้างจะหมุนไปข้างหน้า 1ครั้ง กลับหลัง 1 ครั้ง และ หมุนล้อเดียวข้างละหนึ่งครั้งแล้วจึงหยุด ถ้าไม่ถูกต้องให้แก้ไข เช่นการต่อขั้วของมอเตอร์ผิด เป็นต้น

ถ้าทุกอย่างถูกต้อง ทดลอง ยกลงวางที่พื้นแล้วทดสอบ อีกครั้ง ถอด แบตเตอรี่ออก (หรือ ปิดเปิด สวิทช์ไฟใหม่) แล้วใส่เข้าไปใหม่ ถ้าทุกอย่างถูกต้อง รถจะเครื่อนไปข้างหน้า-ถอยหลัง แล้ว เลี้ยวซ้าย แล้ว จึงกลับสู่ตำแหน่งเดิม

จากนั้นประกอบ 5 Channel Line Tracking Sensor Module (BFD-1000) เข้ากับ บอร์ด Arduino UNO

ใช้ สกรูหัวกลมน็อตตัวเมีย ขนาด 3มม ยาว35 มม. ยึด 5 Channel กับตัวหุ่นยนต์

Module -> Arduino

S1 -> A1

S2 -> A2

S3 -> A3

S4 -> A4

S5 -> A5

VCC -> 5V

GND -> GND

ทดสอบการทำงานของ BFD-1000 โดย  Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3


/*

read analog data from A1-A5 and send to PC via Serial port

*/



int sensor_1 , sensor_2 , sensor_3 , sensor_4 , sensor_5 ; //optical sensor values

String tmp ;

int ADC_stabilize = 5 ;



void setup ( ) {

   // initialize the serial communication:

   Serial . begin ( 9600 ) ;

}



void loop ( ) {

   //take a snapshot

   sensor_1 = digitalRead ( A1 ) ;

   delay ( ADC_stabilize ) ; //stabilize

   sensor_1 = digitalRead ( A1 ) ;

   delay ( ADC_stabilize ) ;



   sensor_2 = digitalRead ( A2 ) ;

   delay ( ADC_stabilize ) ;

   sensor_2 = digitalRead ( A2 ) ;

   delay ( ADC_stabilize ) ;



   sensor_3 = digitalRead ( A3 ) ;

   delay ( ADC_stabilize ) ;

   sensor_3 = digitalRead ( A3 ) ;

   delay ( ADC_stabilize ) ;



   sensor_4 = digitalRead ( A4 ) ;

   delay ( ADC_stabilize ) ;

   sensor_4 = digitalRead ( A4 ) ;

   delay ( ADC_stabilize ) ;



   sensor_5 = digitalRead ( A5 ) ;

   delay ( ADC_stabilize ) ;

   sensor_5 = digitalRead ( A5 ) ;

   delay ( ADC_stabilize ) ;

 

   tmp = " S1=" + String ( sensor_1 ) + " S2=" + String ( sensor_2 ) + " S3=" + String ( sensor_3 ) + " S4=" + String ( sensor_4 ) + " S5=" + String ( sensor_5 ) ;



   // send the value of analog inputs:

   Serial . println ( tmp ) ;



   // wait a bit for next reading

   delay ( 1000 ) ; //1000=1 sec


}


เมื่อ Upload เสร็จ ให้ทดลองนำเทปสีดำ แปะลงที่พื้น นำหุ่นยนต์วางส่วนที่ตัวทีอยู่กึ่งกลาง อยู่ในพื้นที่เทปสีดำ

เปิด Serial Monitor ขึ้นมา โดยไปที่  Tools -> Serial Monitor

ทดสอบการทำงาน เซ็นเซอร์ โดย เครื่อนที่หุ่นยนต์ ซ้าย - ขวา ถ้าเซ็นเซอร์ ตัวทีอยู่กึ่งกลาง อยู่ในพื้นที่เทปสีดำ จะได้ผลลัพธ์ S1=1 S2=1 S3=0 S4=1 S5=1 แสดงว่า BFD-1000 นั้น พร้อมทำงานแล้วครับ

หลักการทำงานของ เซ็นเซอร์ BFD-1000 ก็คือถ้าพบพื้นสีขาว ค่าที่อ่านได้ = 1 (NOLINE) และ เมื่อพบเทปสีดำ ค่าที่อ่านได้  = 0 (LINE)

เราจึงะนำค่าที่แตกต่างกันของเซ็นเซอร์ ทั้ง 5 ตัว  มาใช้เป็นเงื่อนไขตัวอย่างในการเขียนโปรแกรม ดังนี้

 12345

 11011 ให้หุ่นยนต์เดินหน้า
 11101 ให้หุ่นยนต์เลี้ยวขวา
 11001 ให้หุ่นยนต์เลี้ยวขวา
 10111 ให้หุ่นยนต์เลี้ยวซ้าย
 10011 ให้หุ่นยนต์เลี้ยวซ้าย
 00011 ให้หุ่นยนต์เลี้ยวซ้าย 90 องศา
 00111 ให้หุ่นยนต์เลี้ยวซ้าย 90 องศา
 11000 ให้หุ่นยนต์เลี้ยวขวา 90 องศา
 11100 ให้หุ่นยนต์เลี้ยวขวา 90 องศา
 เคสอื่นๆ ให้หุ่นยนต์เดินหน้า

จากนั้นทดสอบ หุ่นยนต์เดินตามเส้น 5 เซ็นเซอร์ BFD-1000 ได้โดย  Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3



// Motor A pins (enableA = enable motor, pinA1 = forward, pinA2 = backward)
int enableA = 3;
int pinA1 = 6;
int pinA2 = 7;

//Motor B pins (enabledB = enable motor, pinB2 = forward, pinB2 = backward)
int enableB = 5;
int pinB1 = 8;
int pinB2 = 9;

int sensor[5] = {0, 0, 0, 0, 0};

void setup()
{
  pinMode(enableA, OUTPUT);
  pinMode(pinA1, OUTPUT);
  pinMode(pinA2, OUTPUT);

  pinMode(enableB, OUTPUT);
  pinMode(pinB1, OUTPUT);
  pinMode(pinB2, OUTPUT);
}

void loop()
{

  enableMotors();
  analogWrite(enableA, 120);  // ปรับค่าความเร็วของหุ่นยนต์
  analogWrite(enableB, 120);  // ปรับค่าความเร็วของหุ่นยนต์

  sensor[0] = digitalRead(A1);
  sensor[1] = digitalRead(A2);
  sensor[2] = digitalRead(A3);
  sensor[3] = digitalRead(A4);
  sensor[4] = digitalRead(A5);

  if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 1))
    forward(1);
  else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 1))
    turnRight(1);
  else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 1))
    turnRight(1);
  else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1))
    turnLeft(1);
  else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 1))
    turnLeft(1);
  else if ((sensor[0] == 0) && (sensor[0] == 0) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 1))
    turnLeft(250);
  else if ((sensor[0] == 0) && (sensor[0] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1))
    turnLeft(250);
  else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0))
    turnRight(250);
  else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0))
    turnRight(250);
  else
    forward(1);
}

//Define high-level H-bridge commands

void enableMotors()
{
  motorAOn();
  motorBOn();
}

void disableMotors()
{
  motorAOff();
  motorBOff();
}

void forward(int time)
{
  motorAForward();
  motorBForward();
  delay(time);
}

void backward(int time)
{
  motorABackward();
  motorBBackward();
  delay(time);
}

void turnLeft(int time)
{
  motorABackward();
  motorBForward();
  delay(time);
}

void turnRight(int time)
{
  motorAForward();
  motorBBackward();
  delay(time);
}

void coast(int time)
{
  motorACoast();
  motorBCoast();
  delay(time);
}

void brake(int time)
{
  motorABrake();
  motorBBrake();
  delay(time);
}
//Define low-level H-bridge commands

//enable motors
void motorAOn()
{
  digitalWrite(enableA, HIGH);
}

void motorBOn()
{
  digitalWrite(enableB, HIGH);
}

//disable motors
void motorAOff()
{
  digitalWrite(enableB, LOW);
}

void motorBOff()
{
  digitalWrite(enableA, LOW);
}

//motor A controls
void motorAForward()
{
  digitalWrite(pinA1, HIGH);
  digitalWrite(pinA2, LOW);
}

void motorABackward()
{
  digitalWrite(pinA1, LOW);
  digitalWrite(pinA2, HIGH);
}

//motor B controls
void motorBForward()
{
  digitalWrite(pinB1, HIGH);
  digitalWrite(pinB2, LOW);
}

void motorBBackward()
{
  digitalWrite(pinB1, LOW);
  digitalWrite(pinB2, HIGH);
}

//coasting and braking
void motorACoast()
{
  digitalWrite(pinA1, LOW);
  digitalWrite(pinA2, LOW);
}

void motorABrake()
{
  digitalWrite(pinA1, HIGH);
  digitalWrite(pinA2, HIGH);
}

void motorBCoast()
{
  digitalWrite(pinB1, LOW);
  digitalWrite(pinB2, LOW);
}

void motorBBrake()
{
  digitalWrite(pinB1, HIGH);
  digitalWrite(pinB2, HIGH);
}


วีดีโอผลลัพธ์การทำงานของ โปรเจค หุ่นยนต์เดินตามเส้น 5 เซ็นเซอร์ BFD-1000