Code โปรเจค หุ่นยนต์เดินตามเส้น 2 เซ็นเซอร์ Arduino + L298P

โปรเจค นี้จะเป็นการนำ Arduino UNO R3 + L298P Motor Shield และ เซ็นเซอร์ 1 Channel TCRT5000 Tracker Sensor มาทำเป็น หุ่นยนต์เดินตามเส้น 2 เซ็นเซอร์  โดยให้หุ่นยนต์ตัดสินใจ เดินตามเส้น โดยอัตโนมัติ ด้วยการตรวจจับ เส้นสีดํา พื้นสีขาว

อุปกรณ์ที่ใช้

1. 2WD Smart Car Robot Chassis Kits

2. Arduino UNO R3 - Made in italy

3. L298P Motor Shield Board

4. Jumper (M2M) cable 20cm Male to Male

5. Jumper (F2M) cable 30cm Female to Female

6. สกรูหัวกลม+น็อตตัวเมีย ยาว 12 มม.

7. รางถ่าน 18650 แบบ 2 ก้อน

8. ถ่านชาร์จ 18650 Panasonic NCR18650B 3.7v  จำนวน 2 ก้อน

9. 1 Channel TCRT5000 Tracker Sensor จำนวน 2 ตัว

10. เสารองแผ่นพีซีบีแบบโลหะ ยาว 25 มม.

หลักการทํางานหุ่นยนต์เดินตามเส้น

หุ่นยนต์และมนุษย์มีหลักการทำงานที่เหมือนกัน คือ หน่วยรับข้อมูลเข้า (Input Unit) หน่วยประมวลผล (Process Unit) และหน่วยแสดงผล (Output Unit) ดังนั้นการที่หุ่นยนต์จะเคลื่อนที่ไปให้ตรงเป้าหมาย หุ่นยนต์จะต้องมีอุปกรณ์ที่จะตรวจสอบตำแหน่งและส่งข้อมูลที่ได้ไปยังหน่วยประมวลผล เพื่อให้มอเตอร์ทำการแสดงผลโดยการไปยังเป้าหมายต่อไป อุปกรณ์พื้นฐานที่ใช้ในการตรวจสอบตำแหน่งนั้น คือ โมดูลเซ็นเซอร์แสง TCRT5000 Tracker Sensor

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

หากเซ็นเซอร์ซ้ายมาพบพื้นสีดำ  และ เซ็นเซอร์ขวาพบพื้นสีขาว ( 0 , 1 ) ให้หุ่นยนต์เลี้ยวไปทางด้านซ้าย

หากเซ็นเซอร์ขวามาพบพื้นสีดำ  และ เซ็นเซอร์ซ้ายพบพื้นสีขาว ( 1 , 0 ) ให้หุ่นยนต์เลี้ยวไปทางด้านขวา

หากเซ็นเซอร์ทั้งสองมาอยู่บนเส้นสีดำทั้งคู่ ( 0 , 0 ) ให้หุ่นยนต์หยุด

1. ประกอบหุ่นยนต์

เริ่มต้นด้วยการ ประกอบ หุ่นยนต์ Arduino UNO + L298P และ ทดสอบการเคลื่อนที่ของหุ่นยนต์ ตามลิงค์ บทความด้านล่าง


https://robotsiam.blogspot.com/2017/12/arduino-uno-l298p.html

2. ประกอบ เซ็นเซอร์ 1 Channel TCRT5000


ใช้ จั้มเปอร์ เมีย-เมีย 30cm เชื่อมต่อ เซ็นเซอร์ ทั้ง 2 ตัว เข้ากับ L298P ตามรูปด้านล่าง 

เซ็นเซอร์ (ซ้าย) <-> L298P

GND <-> GND

VCC <-> +5

OUT <-> A0

เซ็นเซอร์ (ขวา) <-> L298P

GND <-> GND

VCC <-> +5

OUT <-> A1 

ภาพรวมการเชื่อมต่อระหว่าง L298P กับ เซ็นเซอร์ ด้านซ้ายและด้านขวา

การเชื่อมต่อสายที่ A0 และ A1

ด้านขวาของหุ่นยนต์

ด้านล่างของหุ่นยนต์

ด้านหน้าของหุ่นยนต์

จากนั้นใช้สาย USB เชื่อมต่อระหว่าง คอมพิวเตอร์ กับ Arduino UNO R3

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

/*
  read analog data from A0-A1 and send to PC via Serial port
*/

int sensor_L , sensor_R ; //optical sensor values
String tmp ;
int ADC_stabilize = 5 ;

void setup ( ) {
  // initialize the serial communication:
  Serial . begin ( 9600 ) ;
}

void loop ( ) {
  //take a snapshot
  sensor_L = digitalRead ( A0 ) ;
  delay ( ADC_stabilize ) ; //stabilize
  sensor_L = digitalRead ( A0 ) ;
  delay ( ADC_stabilize ) ;

  sensor_R = digitalRead ( A1 ) ;
  delay ( ADC_stabilize ) ;
  sensor_R = digitalRead ( A1 ) ;
  delay ( ADC_stabilize ) ;

  tmp = "L=" + String ( sensor_L ) +  " R=" + String ( sensor_R ) ;

  // send the value of analog inputs:
  Serial . println ( tmp ) ;

  // wait a bit for next reading
  delay ( 1000 ) ; //1000=1 sec

}

ไปที่ Tools > Board เลือกเป็น Arduino/Genuino UNO

ไปที่ Tools > Port แล้วเลือกพอร์ตที่ปรากฏ (กรณีใช้เครื่องคอมพิวเตอร์ที่มี COM Port มากกว่าหนึ่ง  ให้เลือกตัวอื่นที่ไม่ใช่ COM1)

ในตัวอย่างเลือกเป็น "COM14"

(ถ้ายังว่าง หรือ เป็น COM1 ให้ตรวจสอบการติดตั้งไดร์เวอร์ การต่อสาย USB ของ Arduino UNO)

(ถ้าไม่พบให้ทำการติดตั้งไดร์เวอร์ก่อน) 
การติดตั้งโปรแกรม Arduino (IDE) และ การติดตั้งไดร์เวอร์

กดปุ่ม  เพื่ออัพโหลด

หากสามารถอัพโหลดโปรแกรมลงบอร์ดได้สำเร็จ จะแสดงคำว่า Done uploading. ที่แถบด้านล่าง

นำเทปพันสายไฟสีดำ แปะที่พื้นสีขาว ให้ 2 เซ็นเซอร์ ของหุ่นยนต์อยู่บริเวณ พื้นสีขาว

แล้วไปที่ Tools -> Serial Monitor

เลือก Both NL & CR และ เลือก 9600 baud

Serial Monitor จะแสดง L=1 R=1

และเมื่อ เซ็นเซอร์ด้านซ้ายของหุ่นยนต์  อยู่บริเวณเทปสีดำ

และ เซ็นเซอร์ด้านขวาของหุ่นยนต์อยู่บริเวณพื้นสีขาว

Serial Monitor จะแสดง L=0 R=1

และเมื่อ เซ็นเซอร์ด้านขวาของหุ่นยนต์  อยู่บริเวณเทปสีดำ

และ เซ็นเซอร์ด้านซ้ายของหุ่นยนต์อยู่บริเวณพื้นสีขาว

Serial Monitor จะแสดง L=1 R=0

แสดงว่า การเชื่อมต่อ เซ็นเซอร์ ทั้ง 2 ตัว นั้นถูกต้อง และ พร้อมใช้งานแล้ว

3. ทดสอบการทำงาน

Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3





/*

    Line Following Robot with Arduino UNO + L298P
    For more details visit:
    https://robotsiam.blogspot.com/2018/01/2-arduino-l2989p.html

*/

/*-------definning Outputs------*/

int MA1 = 12;     // Motor A1
int MA2 =  3;     // Motor A2
int PWM_A =  10;   // Speed Motor A

int MB1 =  13;     // Motor B1
int MB2 =  8;     // Motor B2
int PWM_B =  11;  // Speed Motor B

int SPEED = 130;  // Speed PWM สามารถปรับความเร็วได้ถึง 0 - 255


/*-------definning Sensor------*/

int sensor_L ,  sensor_R ; //optical sensor values


void setup() {

  //Setup Channel A
  pinMode(12, OUTPUT); //Motor A1
  pinMode(3, OUTPUT); //Motor A2
  pinMode(10, OUTPUT); //Speed PWM Motor A

  //Setup Channel B
  pinMode(13, OUTPUT);  //Motor B1
  pinMode(8, OUTPUT);  //Motor B2
  pinMode(11, OUTPUT); //Speed PWM Motor B
  
}

void loop() {


  sensor_L = digitalRead (A0) ;
  sensor_R = digitalRead (A1) ;


  if ((sensor_L == 1) && (sensor_R == 1) )
    Forward(1);

  else if ((sensor_L == 0) && (sensor_R == 1) )
    turnLeft(1);

  else if ((sensor_L == 1) && (sensor_R == 0) )
    turnRight(1);

  else if ((sensor_L == 0) && (sensor_R == 0) ) {
    Backward(20);
    Stop(1000);
  }

  else
    Forward(1);

}

void Backward(int time)
{
  digitalWrite(MA1, LOW);
  digitalWrite(MA2, HIGH);
  analogWrite(PWM_A, SPEED);

  digitalWrite(MB1, HIGH);
  digitalWrite(MB2, LOW);
  analogWrite(PWM_B, SPEED);

  delay(time);
}

void Forward (int time)
{
  digitalWrite(MA1, HIGH);
  digitalWrite(MA2, LOW);
  analogWrite(PWM_A, SPEED);

  digitalWrite(MB1, LOW);
  digitalWrite(MB2, HIGH);
  analogWrite(PWM_B, SPEED);

  delay(time);
}

void turnLeft(int time)
{
  digitalWrite(MA1, HIGH);
  digitalWrite(MA2, LOW);
  analogWrite(PWM_A, SPEED);

  digitalWrite(MB1, LOW);
  digitalWrite(MB2, LOW);
  analogWrite(PWM_B, 0);

  delay(time);
}

void turnRight(int time)
{
  digitalWrite(MA1, LOW);
  digitalWrite(MA2, LOW);
  analogWrite(PWM_A, 0);

  digitalWrite(MB1, LOW);
  digitalWrite(MB2, HIGH);
  analogWrite(PWM_B, SPEED);

  delay(time);
}

void Stop(int time)
{
  digitalWrite(MA1, LOW);
  digitalWrite(MA2, LOW);
  analogWrite(PWM_A, 0);

  digitalWrite(MB1, LOW);
  digitalWrite(MB2, LOW);
  analogWrite(PWM_B, 0);

  delay(time);

}



ใส่ถ่าน แบบ 18650 จำนวน 2 ก้อน เข้าที่รางถ่าน เปิดเพาเวอร์สวิตซ์สำหรับเปิดปิด (ON / OFF ) ทดสอบการทำงาน กับ สนาม เส้นสีดํา พื้นสีขาว  ตาม วีดีโอผลลัพธ์การทำงานของ หุ่นยนต์เดินตามเส้น 2 เซ็นเซอร์  Arduino + L298P




วีดีโอผลลัพธ์ หุ่นยนต์เดินตามเส้น 2 เซ็นเซอร์  Arduino + L298P