POP-X2 Rover บทที่ 6 เคลื่อนที่ตามเส้นอัตโนมัติ

แผงวงจรตรวจจับการสะท้อนแสงอินฟราเรด ZX-03

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

เนื่องจากค่าที่อ่านได้จาก ZX-03 เป็นค่าการเปลี่ยนแปลงของแรงดันไฟฟ้า เมื่อใช้กับ POP-X2 จะต้องต่อเข้ากับช่องแอนะล็อก (จุดต่อสีแดง) และใช้คำสั่งอ่านค่าแอนะล็อก

การติดตั้ง ZX-03

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

1.นำ ZX-03 ยึดกับเสารองโลหะขนาด 25 มม. ด้วยสกรู 3×10 มม. ทำเหมือนกัน 2 ชุด

2. นำแท่งต่อ 5 รู ยึดกับกับฐาน POP-X2 Rover ใช้สกรู 3×10 มม. ขันยึดด้วยนอต 3 มม.

3. นำ ZX-03 จากขั้นตอนที่ 1 ยึดเข้ากับแท่งต่อ ในตำแหน่งดังรูปด้วยสกรู 3×10 มม.

4. ต่อสายจาก ZX-03 เข้าที่ POP-X2 โดย ZX-03 ด้านซ้ายต่อเข้ากับขา A4 ส่วนด้านขวาต่อกับขา A2 เป็นการจบขั้นตอนการติดตั้งแผงวงจรตรวจจับการสะท้อนแสงอินฟราเรด ZX-03

ฟังก์ชั่น analog()

สำหรับอ่านค่าแอนะล็อกจากอุปกรณ์เซนเซอร์ต่างๆ ที่ต่อเข้ากับขา A0-A6

รูปแบบ

analog(CH)

CH คือช่องแอนะล็อก (สีแดง) A0-A6 ที่ต้องการอ่านค่า

การคืนค่า

ฟังก์ชั่นนี้จะคืนค่าเป็นตัวเลขจำนวนเต็มมีค่าระหว่าง 0-1023

ตัวอย่าง

glcd(1,1,”%d “,analog(0));

เป็นการนำค่าแอนะล็อกที่อ่านได้จากช่อง A0 แสดงผลที่หน้าจอ GLCD

 

ตัวอย่างที่ 18 ทดสอบอ่านค่าจาก ZX-03 แสดงผลที่ GLCD

 
 
#include <popx2.h>
void setup() {
   setTextSize(3);
   glcdMode(3);
}
void loop() {
  glcd(1,1,"L=%d ",analog(4));
  glcd(3,1,"R=%d ",analog(2));
}

การทำงานของโปรแกรม

โปรแกรมกำหนดขนาดตัวอักษรของ GLCD เป็น 3 เท่า สั่งให้บรรทัดที่ 1 แสดงค่าที่อ่านได้จาก ZX-03 ด้านซ้าย (A4) ส่วนบรรทัดที่ 3 แสดงค่าที่อ่านได้จาก ZX-03 ด้านขวา (A2)


การทดสอบ

1. ยกตัว POP-X2 Rover ขึ้นจากพื้น สังเกตค่าที่อ่านได้ จากทั้งด้านซ้ายและด้านขวา (ปกติจะมีค่าน้อยมาก เนื่องจากไม่เกิดการสะท้อน มีค่าเท่ากับ 0 หรือใกล้เคียง)

2. วาง POP-X2 Rover บนพื้น โดยให้ ZX-03 อยู่ที่ตำแหน่งพื้นสีอ่อนหรือสีขาว อ่านค่าที่หน้าจอ GLCD (ค่าจะอยู่ประมาณ 900-1000 เนื่องจากสะท้อนแสงได้ดีมาก) ทำการบันทึกค่าในตารางเพื่อใช้หาค่าเฉลี่ย

3. วาง POP-X2 Rover โดยให้ ZX-03 อยู่ที่ตำแหน่งสีเข้มหรือสีดำ อ่านค่าที่หน้าจอ GLCD (ค่าจะอยู่ประมาณ 200-300 เนื่องจากสะท้อนแสง แต่สะท้อนได้น้อย) ทำการบันทึกค่าในตารางเพื่อใช้หาค่าเฉลี่ย

จากการทดสอบของผู้เขียน จะได้ค่าตามตาราง โดยจะใช้ค่า 630 และ 640 เป็นค่าอ้างอิงต่อไป

ตัวอย่างที่ 19 ตรวจจับมีคนแอบยก แจ้งเตือนด้วยเสียง

หลัักการของตัวอย่างที่ 18 คือ เมื่อวาง POP-X2 Rover บนพื้น ไม่ว่าพื้นสีอะไร ค่าที่อ่านได้น้อยที่สุดจะอยู่ประมาณ 200-300 (ด้านซ้ายอ่านได้ 280) แต่การยก POP-X2 Rover จากพื้น คือไม่เกิดการสะท้อนเลยค่าที่อ่านได้ใกล้เคียง 0 ดังนั้นเมื่อมองเฉพาะ ZX-03 ด้านซ้าย


 
 
#include <popx2.h>
void setup() {
  glcdClear();
}
void loop() {
  if(analog(4)<140){
    sound(3000,200);
    sound(2000,200);  
  }
}
การทำงานของโปรแกรม

ในตัวอย่างนี้จะใช้ ZX-03 ด้านซ้ายเท่านั้น โดยเมื่อค่าจาก ZX-03 อ่านได้น้อยกว่า 140 จะมีเสียง 2 ความถี่เหมือนไซเรนดังต่อเนื่องไปเรื่อยๆ

ตัวอย่างที่ 20 ทำสนามหุ่นยนต์ทดสอบอย่างง่าย

สนามทดสอบจุดประสงค์เพื่อกำหนดเส้นทางการเคลื่อนที่ของ POP-X2 Rover โดยจะใช้เส้นสีดำบนพื้นสีขาว

รายการอุปกรณ์

นำเทปพันสายไฟ แนะนำยี่ห้อ 3M เนื่องจากมีความยืดหยุ่นสูงและเกิดคราบกาวน้อย ติดเส้นลงบนพลาสติกลูกฟูก ลักษณะตามรูป พลาสติกลูกฟูกที่ใช้ควรมีขนาดไม่น้อยกว่า 80×60 CM

หมายเหตุ

กรณีที่พื้นเป็นกระเบื้องเกรนิตโต้สีอ่อน ก็สามารถใช้เป็นพื้นเพื่อติดเทปได้เช่นเดียวกัน เพราะมีความเรียบมากพอเวลาเคลื่อนที่

ตัวอย่างที่ 21 POP-X2 Rover เคลื่อนที่ไปหยุดที่เส้นสีดำ

คำนวณค่าอ้างอิงมาแล้ว สำหรับแยกแยะสีขาวและดำ ถ้าเขียนโค้ดแล้ว POP-X2 Rover ไปหยุดที่เส้นพอดี แสดงว่าค่าที่คำนวณมานี้ถูกต้อง

 
 
#include <popx2.h>
void setup() {
}
void loop() {
 OK();
 while(analog(4)>630){
  fd(50);
 }
 ao();
 sound(3000,100);
}

การทำงานของโปรแกรม

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

แนวคิดการเคลื่อนที่ตามเส้นด้วยเซนเซอร์ 2 ตัว

ตัวอย่างที่ 22 รถเคลื่อนที่ตามเส้นอย่างง่าย

นำแนวคิดด้านบนมาเขียนเป็นโค้ด

 
 
 #include <popx2.h>
int L,R;
void setup() {
  OK();
  setTextColor(GLCD_YELLOW);
  setTextSize(5);
  glcd(1,1,"GO");
}
void loop() {
 L=analog(4);
 R=analog(2);
 if(L>630&&R>640){
    fd(50);
  }
 else if(L<630&&R>640){
    sl(50);
  }
 else if(L>630&&R<640){
    sr(50);
  } 
}
การทำงานของโปรแกรม

ช่วงแรกโปรแกรมรอกดปุ่ม OK จากนั้นแสดงข้อความ “GO” ขนาด 5 เท่าบน GLCD และเข้าทำงานใน void loop()

ในลูปจะใช้ตัวแปร L สำหรับเก็บค่า ZX-03 ด้านซ้าย และ R สำหรับเก็บค่า ZX-03 ด้านขวา ก่อนเอาไปเปรียบเทียบในคำสั่ง if ซึ่งเป็นไปตามรูปแบบการเคลื่อนที่ตามเส้นแบบ 2 เซนเซอร์ 3 เงื่อนไขการตรวจสอบ

เมื่อ ZX-03 อ่านค่าได้มากทั้งคู่ เคลื่อนที่ตรงไป

เมื่อ ZX-03 ด้านซ้ายอ่านค่าได้น้อย เลี้ยวซ้าย

เมื่อ ZX-03 ด้านขวาอ่านค่าได้น้อย เลี้ยวขวา

เพิ่มเติมเงื่อนไขเมื่อเจอเส้นตัด

จากรูปเมื่อ ZX-03 ทั้งด้านซ้ายและด้านขวาตรวจพบเส้น เมื่อเจอกรณีนี้อาจให้ หยุด, เคลื่อนที่ตรง ,เลี้ยวซ้าย หรือเลี้ยวขวาก็ได้ เพื่อไปตามเส้นทางที่กำหนดต่อไป

ตัวอย่างที่ 23 ส่งเสียงแจ้งเตือนเมื่อเจอเส้นตัด

สร้างชุดคำสั่ง else if ขึ้นมาอีกชุดเพื่อตรวจสอบเส้นตัด

 
 
#include <popx2.h>
int L,R;
void setup() {
  OK();
  setTextColor(GLCD_YELLOW);
  setTextSize(5);
  glcd(1,1,"GO");
}
void loop() {
 L=analog(4);
 R=analog(2);
 if(L>630&&R>640){
    fd(50);
  }
 else if(L<630&&R>640){
    sl(50);
  }
 else if(L>630&&R<640){
    sr(50);
  }
 else if (L<630&&R<640){
    fd(70);
    sound(2000,100);
  }  
}
การทำงานของโปรแกรม

เมื่อ else if ชุดสุดท้ายตรวจพบเส้นสีดำ จะสั่งให้เคลื่อนที่ไปข้างหน้าพร้อมสร้างเสียง 0.1 เป็นผลให้เลยข้ามทางแยกไปได้

ตัวอย่างที่ 24 เลี้ยวขวา เมื่อเจอเส้นตัด

 
 
#include <popx2.h>
int L,R;
void setup() {
  OK();
  setTextColor(GLCD_YELLOW);
  setTextSize(5);
  glcd(1,1,"GO");
}
void loop() {
 L=analog(4);
 R=analog(2);
 if(L>630&&R>640){
    fd(50);
  }
 else if(L<630&&R>640){
    sl(50);
  }
 else if(L>630&&R<640){
    sr(50);
  }
 else if (L<630&&R<640){
    fd(70);
    sound(2000,100);
    sr(40);
    delay(600);
  }  
}
การทำงานของโปรแกรม

ค่อยๆ เพิ่มความยาวของโค้ดขึ้นทีละน้อย เพื่อความเข้าใจ โดยแนวคิดเมื่อ ZX-03 ทั้งสองตัวเจอเส้น จะต้องเคลื่อนที่ไปด้านหน้าเล็กน้อยก่อนที่จะสั่งเลี้ยว ดังนั้นคำสั่งเคลื่อนที่และสั่งเลี้ยวของโค้ดก่อนหน้านี้นำมาใช้ได้เลย แล้วต่อท้ายด้ายคำสั่ง sr(40) เพื่อให้เลี้ยวขวา ระยะเวลาที่ใช้อ้างอิงจาก บทที่ 4 ตัวอย่างที่ 13 การเคลื่อนที่เป็นรูปสี่เหลี่ยมได้ ซึ่งอาจปรับค่า delay เพื่อความเหมาะสม ถ้ามากหรือน้อยไป

ประยุกต์เพิ่มเติม

1. ให้เปลี่ยนจากคำสั่ง sr(40) เป็น sl(40) ดูบ้าง เมื่อเจอเส้นตัดแล้วเลี้ยวซ้ายแทนเลี้ยวขวา

2. ทดลองเพิ่มความเร็วการเคลื่อนที่ ถ้าเคลื่อนที่เร็วขึ้น ทดสอบว่าการเลี้ยวยังเลี้ยวได้ถูกต้องทุกครั้งหรือไม่ มีการหลุดออกนอกเส้นทางเวลาเลี้ยวหรือไม่

ตัวอย่างที่ 25 ปรับความแม่นยำในการเลี้ยวด้วยการตรวจสอบเซนเซอร์ขณะเลี้ยว

การเลี้ยวแบบ กะระยะด้วย delay มีโอกาสที่จะเกิดความผิดพลาดได้ง่าย เช่นถ่านใหม่ การเลี้ยวก็มากไป ถ่านอ่อน การเลี้ยวก็น้อยไปเป็นต้น เพื่อให้แม่นยำขึ้นขณะเลี้ยว จะใช้ ZX-03 คอยตรวจสอบว่ากลับไปคร่อมในตำแหน่งเส้นที่ต้องการหรือยัง

 
 
#include <popx2.h>
int L, R;
void setup() {
  OK();
  setTextColor(GLCD_YELLOW);
  setTextSize(5);
  glcd(1, 1, "GO");
}
void loop() {
  L = analog(4);
  R = analog(2);
  if (L > 630 && R > 640) {
    fd(80);
  }
  else if (L < 630 && R > 640) {
    sl(80);
  }
  else if (L > 630 && R < 640) {
    sr(80);
  }
  else if (L < 630 && R < 640) {
    fd(80);
    sound(2000, 100);
    while (analog(2) > 640) {
      sr(80);
    }
    while (analog(2) < 640) {
      sr(80);
    }
  }
}
การทำงานของโปรแกรม

คำสั่งที่เพิ่มเติมก็คือ while ชุดที่ 1 จะวนเลี้ยวขวาไปเรื่อยๆ จนกว่าจะเจอเส้นสีดำ คำสั่ง while ชุดที่ 2 จะวนขวาต่อไปอีกจนกว่าจะเจอสีขาว ซึ่งหมายถึงหลุดมาคร่อมเส้นเหมือนเดิมแล้ว

ข้อสังเกต

1 ความเร็วของการเคลื่อนที่ จะปรับเพิ่มขึ้นเนื่องจากความแม่นยำของการเลี้ยวถูกปรับแก้แล้ว

2. ในช่วงของการเลี้ยว การตรวจสอบจะตรวจสอบด้วยค่า analog(2) ไม่ได้นำค่า R เดิมมาตรวจสอบ เนื่องจากค่า analog(2) เป็นค่าปัจจุบัน เกิดขึ้นขณะกำลังหมุนตัว

 

ประยุกต์เพิ่มเติม

  1. ลองปรับเปลี่ยนโค้ดให้เป็นการเลี้ยวซ้ายดูบ้าง จะต้องปรับแก้ 2 ส่วนคือ คำสั่งเลี้ยว และการตรวจสอบ ZX-03 จะต้องเป็นด้านซ้าย (analog(4))

2. ถ้าต้องการให้การเคลื่อนที่ แยกแรกเลี้ยวทางขวา แยกที่ 2 เลี้ยวทางซ้าย จะต้องปรับแก้โค้ดอย่างไร ลองคิดดูครับ

 

การสร้างฟังก์ชั่น

ฟังก์ชั่นคือการรวบรวมชุดคำสั่งหลายๆ คำสั่งเพื่อทำภารกิจบางอย่าง โดยตั้งเป็นชื่อใหม่

ตัวอย่างที่ 26 ปรับตัวอย่างที่ 25 ให้อยู่ในรูปฟังก์ชั่น

ฟังก์ชั่นเคลื่อนที่ตามเส้นเลี้ยวขวาเมื่อพบทางแยก

 
 
void RRR() {
  while (1) {
    L = analog(4);
    R = analog(2);
    if (L > 630 && R > 640) {
      fd(80);
    }
    else if (L < 630 && R > 640) {
      sl(80);
    }
    else if (L > 630 && R < 640) {
      sr(80);
    }
    else if (L < 630 && R < 640) {
      fd(80);
      sound(2000, 100);
      while (analog(2) > 640) {
        sr(80);
      }
      while (analog(2) < 640) {
        sr(80);
      }
      break;
    }
  }
}

ฟังก์ชั่นเคลื่อนที่ตามเส้นเลี้ยวซ้ายเมื่อพบทางแยก

 
 
void LLL() {
  while (1) {
    L = analog(4);
    R = analog(2);
    if (L > 630 && R > 640) {
      fd(80);
    }
    else if (L < 630 && R > 640) {
      sl(80);
    }
    else if (L > 630 && R < 640) {
      sr(80);
    }
    else if (L < 630 && R < 640) {
      fd(80);
      sound(2000, 100);
      while (analog(4) > 630) {
        sl(80);
      }
      while (analog(4) < 630) {
        sl(80);
      }
      break;
    }
  }
}

 

การทำงาน

เพื่อให้ฟังก์ชั่นทำงานเหมือนกับ void loop() ดังนั้นจะต้องมี while (1) ครอบเอาไว้เพื่อให้วนทำซ้ำไปเรื่อยๆ

ความต้องการที่ 2 บอกเราว่าเมื่อเคลื่อนที่ไปจนเจอเส้นตัดและเลี้ยวเรียบร้อยแล้วให้จบฟังก์ชั่น ดังนั้นจึงต้องมีคำสั่ง break; เพื่อให้หลุดออกจากฟังก์ชั่นนี้ ทำงานที่ฟังก์ชั่นอื่นต่อไป

ตัวอย่างที่ 27 เคลื่อนที่ต่อเนื่องเป็นเลข 8

จากการปูทางด้วยรูปภาพ คงมองภาพเป็นโค้ดออกได้ไม่ยากนักครับ โดยการเคลื่อนที่แบบนี้จะต้องมีการเลี้ยวซ้าย 4 ครั้ง และเลี้ยวขวา 4 ครั้ง วนทำงานต่อเนื่องไปเรื่อยๆ

 
 
#include <popx2.h>
int L, R;
void RRR() {
  while (1) {
    L = analog(4);
    R = analog(2);
    if (L > 630 && R > 640) {
      fd(80);
    }
    else if (L < 630 && R > 640) {
      sl(80);
    }
    else if (L > 630 && R < 640) {
      sr(80);
    }
    else if (L < 630 && R < 640) {
      fd(80);
      sound(2000, 100);
      while (analog(2) > 640) {
        sr(80);
      }
      while (analog(2) < 640) {
        sr(80);
      }
      break;
    }
  }
}
void LLL() {
  while (1) {
    L = analog(4);
    R = analog(2);
    if (L > 630 && R > 640) {
      fd(80);
    }
    else if (L < 630 && R > 640) {
      sl(80);
    }
    else if (L > 630 && R < 640) {
      sr(80);
    }
    else if (L < 630 && R < 640) {
      fd(80);
      sound(2000, 100);
      while (analog(4) > 630) {
        sl(80);
      }
      while (analog(4) < 630) {
        sl(80);
      }
      break;
    }
  }
}
void setup() {
  OK();
  setTextColor(GLCD_YELLOW);
  setTextSize(5);
  glcd(1, 1, "GO");
}
void loop() {
  LLL();
  LLL();
  LLL();
  LLL();
  RRR();
  RRR();
  RRR();
  RRR();
}

 

ยาวหน่อยครับบทนี้ แต่พอจะเห็นภาพรูปแบบการเคลื่อนที่ ที่สามารถโปรแกรมเส้นทางได้กันนะครับ
ตอนต่อไปจะเพิ่มความเข้มข้นขึ้นด้วยชุดแขนจับ Gripper คอยติดตามครับ

Facebook Comments Box

Press Ctrl+C to copy the following code.
"