ในส่วนของผมจะทำหน้าที่การเขียนโปรแกรมแก้เขาวงกตและควบคุมหุ่นยนต์solve mazeซึ่งสิ่งที่ได้มีดังนี้
- สร้างโปรแกรมsolve mazeด้วยภาษาPython โดยการสร้างแผนที่ด้วย array
โปรแกรมจะสร้างเขาวงกตและจำลองการเดินของหุ่นโดยเมทริกซ์จะทำการหมุนตามการหันหัวของหุ่น เพื่อให้สามารถควบคุมหุ่นและรับข้อมูลจากได้ง่ายขึ้น และเมื่อมีคำสั่งsolve ก็จะทำการหมุนกลับมา ณ มุมเดิมเพื่อให้โปรแกรมสามารถแก้เขาวงกตได้
โดยการรับค่าคำสั่งจาก mqtt คำสั่งมีดังนี้
s เดินตรงไปข้างหน้า2จุด
r เลี้ยวขวาพร้อมกับหมุนเมทริกทวนเข็ม90องศา
l เลี้ยวซ้ายพร้อมกับหมุนเมทริกตามเข็ม 90องศา
b กลับหลังพร้อมกับหมุนเมทริกไป180องศา
solve เพื่อหาหาเส้นทางที่สั้นที่สุดจากตำแหน่งที่ได้กำหนดไว้ตอนแรก พร้อมกับระบุทิศที่ต้องไปเมื่อเจอทางแยก
return เพื่อค้นหาเส้นทางกลับมาที่จุดเดิม โดยการย้อนลำดับ array แล้วสลับซ้ายเป็นขวา หรือ ขวาเป็นซ้าย
2.สร้างหุ่นยนต์ โดยหุ่นยนต์ที่ใช้จะเป็นหุ่นยนต์ที่สร้างขึ้นโดยใช้ raspberry pi 5เป็นตัวคอยรับค่าจาก ROS2,ส่งค่าไปที่mqtt,รับข้อมูลsensor และส่งคำสั่งการเคลื่อนที่ไปให้arduino ผ่านUART
จากภาพ หุ่นยนต์จะเคลื่อนที่ด้วยstepper motorผ่านdriver A4988บนCNC shield มีultrasonic sensor คอยตรวจสอบทางแยก ใช้แบตเตอรี่ LiPo ในการจ่ายไฟให้กับระบบ และจะมีการติดตั้ง lidarเพิ่มเติม เพื่อตรวจสอบตำแหน่งปัจจุบันหุ่นเทียบกับจุดเริ่มต้นที่Ros2คำนวณมาให้
Arduino จะทำหน้าที่คอยควบคุม motor เพียงอย่างเดียวเท่านั้นโดยการรับค่าจากUART และจะวิ่งไปตามจำนวณstepที่ตั้งค่าไว้ ซึ่งสามารถเปลี่ยนแปลงได้จากการส่งผ่าน mqtt และเมื่อเคลื่อนที่เสร็จจะส่งข้อความให้ Raspberry pi เพื่อบอกว่าคำสั่งสำเร็จแล้ว Raspberry pi จะได้ดำเนินการขั้นต่อไป
ในRaspberry pi จะแบ่งการทำงานออกเป็น 4เฟสคือ phase0(stand by) phase1(สำรวจแผนที่ ) phase2(เริ่มเดินหลังจากsolve) และphase3(เดินกลับ) โดยphaseแรกคือการเปิดโปรแกรมเพื่อเชื่อมต่อและรอคำสั่งเริ่มต้นจากmqttซึ่งก็คือ start/1 เพื่อเริ่มเดิน โดยตัวหุ่นจะเดินไปทีละช่องพร้อมกับส่งmqttไปบอกโปรแกรมแก้เขาวงกตว่าเดินไปทางไหนเมื่อเดินเสร็จหุ่นจะตรวจสอบทางแยกเพื่อทำการตัดสินใจ ตัวหุ่นจะเลือกเดินไปทางขวาตลอด หากทางขวาเป็นกำแพงจะเดินไปข้างหน้า หากทางข้างหน้ายังไปไม่ได้ก็จะเลี้ยวซ้าย และเมื่อพบว่าเป็นทางตัวจึงหันหลังกลับ ตัวLidar จะคอสส่งข้อมูลไปให้ROS2เพื่อดูว่าหุ่นกำลังอยู่ตำแหน่งที่เท่าไหร่เมื่อเทียบกับจุดเริ่มต้น ถ้าหากหุ่นวิ่งกลับมาที่จุด (0,0) ก็จะสั่งsolve และระบุจุดที่ต้องไป แล้วจึงเริ่มเดิน เมื่อพบทางแยกหุ่นจำส่งข้อความ askไปที่mqttเพื่อให้โปรแกรมส่งข้อมูลของแยกแรกกลับมาแล้วเตรียมแยกต่อไปเอาไว้รอหุ่นถามอีกครั้ง และเมื่อพบว่าหุ่นไปถึงที่หมายแล้วก็จะสั่ง return เพื่อรับข้อมูลทางแยกขากลับแล้วทำเหมือนรอบขาไป เมื่อกลับมาถึงจุด0,0 ก็นับว่าหุ่นสามารถแก้เขาวงกตได้สำเร็จ
จากระบบที่กล่าวมาข้างต้น ยังเหลือการทดสอบบนสนามจริงจึงสามารถมีการปรับแก้ได้ในอนาคต
สิ่งที่ได้เรียนรู้ในแต่ละสัปดาห์ คือการที่ได้รู้จักการทำให้โปรแกรมต่างสามารถสื่อสารและทำงานร่วมกันได้จริงๆผ่านโปรโตคอลต่างๆเช่น mqtt และ TCP ซึ่งเมื่อก่อนมักจะทำระบบต่างๆไว้ในโปรแกรมเดียวจึงทำให้เกิดข้อจำกัดด้าน Hardwareต่างๆ นอกจากนี้ยังได้ทดลองเขียนโปรแกรมควบคุมหุ่นยนต์ด้วย Pythonซึ่งค่อนข้างสะดวกเพราะมีlibraryสำหรับ algorithmจำนวนมาก