Picking Robot EP.06 Black or White Object Testing

ตอนที่ 6 ฟังก์ชั่นตรวจสอบสีของกระป๋อง

การติดตั้งเซนเซอร์เช็คสีกระป๋อง

นอกจาก 4 ตัวที่ใช้เคลื่อนที่ตามเส้นแล้ว ยังมีตัวที่ 5 สำหรับเช็คกระป๋องอีกด้วย โดยติดตั้งเซนเซอร์ ZX-03 ไว้ที่ช่อง 22/A4 การอ่านค่าใช้คำสั่ง analog(4)

ทดสอบอ่านค่าสีของกระป๋อง แสดงผล GLCD

ก่อนอื่นต้องทดสอบอ่านค่ากระป๋องกันก่อนว่า ถ้าเอากระป๋องสีดำหรือสีขาวมาวางแล้ว ให้ผลที่แตกต่างกันอย่างไร โดยเขียนโปรแกรมตามนี้

การทดสอบอ่านสีกระป๋อง

ทดลองนำกระป๋องสีขาวมาเสียบเข้าที่ด้านหน้าของ Gripper ทดลองเลื่อนไปเลื่อนมาแล้วดูค่าน้อยที่สุดที่เป็นไปได้ที่จะเกิดขึ้นกับกระป๋องสีขาว บันทึกค่าเอาไว้ จากการทดลองอ่านค่าได้ 160

ทดลองนำกระป๋องสีดำมาเสียบเข้าที่ Gripper บ้าง แล้วอ่านค่ามากที่สุดที่เกิดขึ้นได้กับกระป๋องสีดำ จากการทดลองอ่านค่าได้ 40
นำทั้งสองค่ามาบวกกันหารสอง ก็ได้เป็นค่ากลางสำหรับแยกแยกกระป๋องสีขาวและกระป๋องสีดำ  ดังนั้นค่ากลางที่เกิดขึ้นนี้คือ (160+40 ) /2 = 100 

ลำดับเหตุการณ์กันก่อน

ก่อนจะเขียนฟังก์ชั่นอ่านค่าสี ลองมาลำดับเหตุการณ์การทำงานของหุ่นยนต์กันก่อน

1. หุ่นยนต์เดินออกจากจุดเริ่มต้น 

2. หุ่นยนต์เดินตามเส้น เจอเส้นตัด 3 แยกหรือ 4 แยก ให้เลี้ยวซ้ายหรือขวา หรือตรงไป (ฟังก์ชั่น LLL() RRR() FFF()

3. หุ่นยนต์เจอเส้นตัดทางแยก ตำแหน่งยกกระป๋อง (ฟังก์ชั่น PICK() )

4. หุ่นยนต์เดินตามเส้น เจอเส้นตัด 3 แยกหรือ 4 แยก ให้เลี้ยวซ้ายหรือขวา หรือตรงไป (ฟังก์ชั่น LLL() RRR() FFF()

5. หุ่นยนต์เจอเส้นตัดทางแยก ตำแหน่งวางกระป๋อง (ฟังก์ชั่น PUSH() )

ฟังก์ชั่นอ่านค่าสีจากกระป๋อง

จากลำดับเหตุการณ์ ส่วนที่จะแทรกเพิ่มเข้าไปสำหรับเช็คกระป๋องก็คือหัวข้อที่ 3 หลังจากยกกระป๋องเรียบร้อยแล้ว โดยอ้างอิงจากฟังก์ชั่น PICK() เดิมคือ

ตำแหน่งที่จะวางฟังก์ชั่นเช็คกระป๋อง จะต้องอยู่ก่อนกรอบเส้นประ "ถอยหลังหมุนตัวจนเจอเส้นตัด" ดังนั้นฟังก์ชั่น PICK() จะกลายเป็น

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

ตัวอย่างฟังก์ชั่น WHITE()   

ตัวอย่างฟังก์ชั่น BLACK()

สังเกตุว่า ชุดคำสั่งถอยหลังหมุนตัวจนเจอเส้นตัดจากเดิมที่อยู่ในฟังก์ชั่น PICK() ถูกย้ายมาไว้ที่ฟังก์ชั่น WHITE() กับ BLACK()  เรียบร้อยแล้วครับ 

Picking Robot EP.05 SERVO Gripper Control

ตอนที่ 5 ฟังก์ชั่นยกกระป๋อง และวางกระป๋อง

หลักการทำงานของเซอร์โวมอเตอร์

  เซอร์โวมอเตอร์ มีสายต่อใช้งาน 3 เส้นดังรูป มีวงจรขับมอเตอร์พร้อมทั้งวงจรป้อนกลับเพื่อให้รับรู้ตำแหน่งของตัวเองอยู่ภายใน การใช้งานถ้าเป็น Arduino และ POP-XT จะสั่งงานผ่านฟังก์ชั่น Servo( CH,POS)

โดย   CH    คือ ช่องของเซอร์โว POP-XT มีช่องขับ 3 ช่อง

          POS คือ ตำแหน่งของเซอร์โวอยู่ระหว่าง 0 ถึง 180 แต่ถ้าเรากำหนดเป็น -1 จะเป็นการยกเลิกการใช้เซอร์โวตัวนั้น

ตัวอย่าง
      servo(1,100); 
เป็นการสั่งให้เซอร์โวเคลื่อนที่ไปที่ตำแหน่ง 100 องศา

ทดสอบยกกระป๋อง

        จากสรีระของหุ่นยนต์ที่ใช้เซอร์โวเพียงตัวเดียวในการยกกระป๋อง  จึงเป็นเรื่องง่ายที่จะควบคุมเซอร์โวให้ทำงานทั้งคีบ ยก และวาง ก่อนอื่นต้องทดสอบหาตำแหน่งของเซอร์โว สำหรับการยกและการวางก่อน ด้วยโปรแกรมทดสอบการทำงานของเซอร์โว

จากโปรแกรมตัวอย่าง มีคำสั่งพิเศษที่เพิ่มเติมคือคำสั่ง map ใช้สำหรรับการแปลงค่าตำแหน่งของ knob ให้เป็นองศาของเซอร์โว ซึ่งสะดวกมาก ไม่ต้องเขียนสมการให้วุ่นวาย หลังจากแปลงค่าแล้วก็นำค่าที่ได้แสดงที่หน้าจอ GLCD และส่งค่าไปยังเซอร์โวมอเตอร์ด้วย

ทดลองหมุน knob จน Gripper กางออกเยอะที่สุด บันทึกค่าที่แสดงที่หน้าจอเอาไว้

ทดลองหมุน knob จน Gripper หนีบกระป๋องแล้วยก บันทึกค่าที่แสดงที่หน้าจอเอาไว้

นำค่าที่ได้มาสร้างเป็นฟังก์ชั่น ยกกระป๋อง (PICK)

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

ข้อสังเกตเพิ่มเติมของฟังก์ชั่นนี้ก็คือ โปรแกรมจะหลุดจากลูปเดินตามเส้นโดยเงื่อนไขว่า analog(2) หรือ analog(3) เจอสีดำ แต่ในการเขียนโปรแกรมแทนที่เราจะใช้เครื่องหมาย OR || เรากลับใช้เป็นเครื่องหมาย AND && แทน ลองไปทบทวนวิชาตรรกศาสตร์หรือการทำงานของลอจิกดูนะครับ

ต่อด้วยฟังก์ชั่น วางกระป๋อง (PUSH)

พอเป็น Idea สำหรับไปพัฒนาต่อได้นะครับ ตอนหน้า มาว่ากันถึงวิธีการแยกสีขาวและสีดำของกระป๋องกันบ้างครับ

Picking Robot EP.04 Turn 90 degree

ตอนที่  4  ฟังก์ชั่นเลี้ยว  90  องศาเมื่อเจอทางแยก

สำหรับทางแยกที่เป็นลักษณะ 3 แยก การเลี้ยว 90 องศาก็ใช้หลักการเหมือนการเดินตรงข้ามทางแยก 

การเลี้ยวขวา 90 องศา

จากรูปมองเหตุการณ์เป็น 4 ขั้นตอนดังนี้

1. เมื่อหุ่นยนต์เดินตามเส้นไปเรื่อยๆ แล้วเซนเซอร์ A3 เจอทางแยก

2. ให้หุ่นยนต์เดินตรงไปข้างหน้าจนตัวหุ่นยนต์อยู่คร่อมจุดตัดทางแยกพอดี

3. หมุนตัวไปทางขวา โดยถ้าเซนเซอร์ A1 ยังเจอสีขาวอยู่ให้หมุนทางขวาไปเรื่อยๆ 

4. เมื่อเซนเซอร์ A1 เจอสีดำก็ให้หมุนตัวทางขวาต่อไปอีกจนกว่า เซนเซอร์ A1 จะเจอสีขาวอีกครั้ง ซึ่งก็จะเป็นการไปคร่อมเส้นด้านขวาพอดี

จากรูปนำมาเขียนเป็นฟังก์ชั่นสำหรับการเลี้ยวขวา 90 องศาได้ดังนี้

ถ้าดูการทำงานของฟังก์ชั่น เมื่อเทียบกับฟังก์ชั่น FFF() สำหรับการเดินหน้าก่อนหน้านี้จะเห็นว่ามีส่วนที่คล้ายคลึงกันมาก โดยส่วนหลัก ๆ คือการที่หุ่นยนต์เคลื่อนที่ตามเส้นไปเรื่อยๆ จะหลุดจากลูปของการเคลื่อนที่ตามเส้นก็ต่อเมื่อ เซนเซอร์ A3 เจอสีดำเท่านั้นและเมื่อหลุดจากลูปมาแล้ว ก็ทำงานตาม 4 ขั้นตอนที่บอกไป จนจบฟังก์ชั่น หุ่นยนต์ก็จะเลี้ยวขวา 90 องศาได้อย่างสมบูรณ์

การเลี้ยวซ้าย 90 องศา

การเลี้ยวซ้ายก็จะมีลักษณะเดียวกันกับการเลี้ยวขวา แต่เปลี่ยนการตรวจจับเซนเซอร์เป็นคนละรูปแบบดังนี้

นำมาเขียนเป็นฟังก์ชั่นสำหรับเดินตามเส้นเจอแยกแล้วเลี้ยวซ้ายได้ดังนี้

ตัวอย่างให้หุ่นยนต์เคลื่อนที่ตามเส้นทางที่กำหนด 

สามารถเรียกฟังก์ชั่นเลี้ยวซ้าย เลี้ยวขวา เดินตรงของหุ่นยนต์มาใช้ได้ ยกตัวอย่างสนามง่ายๆ ดังรูป

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

Picking Robot EP.03 Line Tracker with 4 Sensor

ตอนที่ 3 การเคลื่อนที่ตามเส้นโดยใช้เซนเซอร์ 4 ตัว

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

เขียนโปรแกรมอ่านค่าตัวตรวจจับการสะท้อน

เขียนโปรแกรมอ่านค่าเซนเซอร์ทั้ง 4 ตัว เอาค่ามาแสดงที่หน้าจอ GLCD กันก่อน

หน้าจอของ POP-XT จะแสดงผลลัพธ์การอ่านค่าจากเซนเซอร์ออกมา 

ซึ่งจะตรงกับตำแหน่งของเซนเซอร์ที่ติดตั้งอยู่ด้านใต้หุ่นยนต์ดังรูป

อ่านค่าจากพื้นสนามสีขาว

ให้นำหุ่นยนต์ไปวางบริเวณพื้นผิวสีขาว  โยกหุ่นยนต์ไปมาจนได้ค่าต่ำสุดที่เกิดขึ้นได้จากพื้นผิวสีขาว ของเซนเซอร์แต่ละตัวออกมา บันทึกค่าเอาไว้ครับ โดยปกติพื้นผิวสีขาวค่าสะท้อนแสงได้ดี จะอ่านค่าได้มาก ประมาณ 600-1000 โดยประมาณ

อ่านค่าจากเส้นสีดำ

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

การหาค่ากลาง

ก็คือการนำเอาค่าทั้งสีขาวและสีดำมาหาค่าเฉลี่ยครับ โดยถ้าเป็นตามตัวอย่าง ผมจะเลือกค่า 600 และ 100 ดังนั้น ค่ากลางก็มีค่าเท่ากับ (600+100) / 2 ค่าเฉลี่ยเท่ากับ 350  ซึ่งค่านี้จะนำไปใช้เป็นค่าอ้างอิงต่อไป เพื่อให้การปรับแต่งทำได้ง่าย เราจะเอาค่านี้เก็บไว้เป็นตัวแปรชื่อ Ref 

int Ref=350;

หลักการเคลื่อนที่ตามเส้น

ปกติการเคลื่อนที่ตามเส้นโดยใช้เซนเซอร์สองตัว เกิดขึ้นได้ 4 เงื่อนไข 3 เงื่อนไขพื้นฐาน จะเป็นดังรูป

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

เคลื่อนที่ตามเส้นอย่างง่าย

สำหรับโค้ดของ เซนเซอร์ 2 ตัวนำมาเขียนเป็นฟังก์ชั่นชื่อ track ได้ตามนี้

จากตัวอย่างข้างบนก็นำหุ่นยนต์วิ่งบนสนามตามรูปนี้ได้เลย 

เคลื่อนที่ตามเส้นแบบมี 3 แยกด้านหน้า

แต่ถ้าเป็นทางแยกที่มีลักษณะดัง 3 แยก จำเป็นต้องมีเซนเซอร์เพิ่มเติมเพื่อแยกแยะเงื่อนไขที่เพิ่มขึ้น 

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

จากโปรแกรมทุก ๆ ครั้งที่หุ่นยนต์เจอทางแยกจะเดินตรงพร้อมทั้งส่องเสียงออกลำโพงทุก ๆ 0.2 วินาทีเสมอ ตอนหน้าจะมาเพิ่มเติม การเลี้ยวซ้ายและเลี้ยวขวาเมื่อเจอทางแยกนะครับ

ปล. อย่าลืมทบทวนการทำงานของคำสั่ง while ให้ดีนะครับ เพราะเราจะใช้มันเยอะมาก ต่อจากนี้...

Picking Robot EP.02 Picking Robot Building

ตอนที่ 2 สร้างหุ่นยนต์ Picking Robot

จากโจทย์และสนามหุ่นยนต์ในตอนที่ 1 เพื่อให้บรรลุภารกิจเราจะต้องมีหุ่นยนต์มาทำหน้าที่นี้ พระเอกของเราคงไม่พ้น POP-BOT XT

เซนเซอร์ ZX-03

จากเดิม หุ่นยนต์ POP-BOT XT จะใช้เซนเซอร์แค่สองตัวในการเคลื่อนที่ตามเส้นทำให้ จะเดินได้ 4 เงื่อนไขคือ

•     คร่อมเส้น
•     ด้านซ้ายเจอเส้น สีดำ
•     ด้านขวาเจอเส้นสีดำ
•     เจอสีดำทั้งคู่ ซึ่งก็คือเส้นตัดทางแยก (เป็นสี่แยก)

แต่เมื่อเจอทางแยกที่ เป็น 3 แยก เซนเซอร์สองตัวไม่เพียงพอแล้ว จะต้องเพิ่มเซนเซอร์เข้าไปดังรูป

Gripper

 ก็เป็นส่วนสำคัญสำหรับงานนี้ หุ่นยนต์ตัวนี้เลือกใช้ Gripper ของ Parallax  ซึ่งใช้เซอร์โวมอเตอร์แค่ตัวเดียว ไม่ต้องดัดแปลงมากนัก โดยติดตั้งยึดเข้ากับแผ่นกริดที่ด้านหน้าของหุ่นยนต์

    ส่วนตัวเซอร์โวก็ย้ายไปติดด้านหลังเพื่อรักษาสมดุลย์ของตัวหุ่นยนต์เอาไว้  สำหรับลวดที่ลิงก์ระหว่างเซอร์โวกับแขนจับ ถ้าในชุดที่มีให้ยาวไม่พอแนะนำว่า ให้เอาลวดที่ใช้ทำราวตากผ้ามาใช้ได้เลย ดัดงอ
ให้พอเหมาะ

ส่วนเพิ่มเติมอีกส่วนก็คือ จะต้องทำแขนค้ำระหว่างเซอร์โวและ Gripper เพื่อไม่ให้เวลาดึงเซอร์โว แล้วทั้งสองส่วนงอเข้าหากัน โดยใช้แท่งต่อและฉากโลหะเข้ามาช่วย

สำหรับการตรวจจับกระป๋อง ก็ต้องมีเซนเซอร์ ZX-03 อีก 1 ตัวมาทำหน้าที่นี้ โดยยึดเข้าที่ Gripper โดยตรงดังรูป

แบตเตอรี่ 

 ปกติ POPBOT-XT จะใช้ถ่านอัลคาไลน์หรือถ่านชาร์จครับ แต่เมื่อต้องการให้หุ่นยนต์เคลื่อนที่แม่นยำ เป็นเวลนาน ๆ ถ่านที่เหมาะสมก็คือ Li-Po (ลิเธี่ยมโพลิเมอร์) หรือ Li-ion (ลิเธี่ยม ไอออน) ขนาด 2 เซลล์ 7.4V 1500 mA ติดตั้งไว้ด้านใต้ของ POP-XT ดังรูป

ที่เหลือก็เสียบสายมอเตอร์ เสียบสายเซอร์โว และสายเซนเซอร์ สุดท้ายเสียบสายแบตเตอรี่ และเตรียมพร้อมสำหรับการเขียนโปรแกรมในตอนต่อไปครับ

Picking Robot EP.03 Line Tracker with 4 Sensor

Picking Robot EP.01 โจทย์และสนามหุ่นยนต์

ตอนที่ 1 โจทย์และสนามทดสอบหุ่นยนต์เคลื่อนย้ายกระป๋อง

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

ลักษณะสนาม

 
 พื้นสนามเป็นพื้นพลาสวูดสีขาว กว้าง 120x100 CM โดยประมาณ
 เส้นสีดำของสนามใช้เทปพันสายไฟยี่ห้อ 3 M เดินเส้นทั้งสนาม

ลักษณะกระป๋อง

 
 เป็นกระป๋องน้ำผลไม้ ทรงกลมเส้นผ่านศูนย์กลางประมาณ  5 CM และสูง  13 CM มีสองสีคือสีขางและสีดำ

Picking Robot EP.02 Picking Robot Building