ในส่วนของ Mobility จะใช้ Motor Dynamixel MX-106 ทั้งหมด 4 ตัว โดยทั้ง 4 ตัวนี้จะทำการสั่งค่าและรับค่าโดยใช้ DYNAMIXEL SDK Package ผ่านการสั่งการด้วยภาษา Python บน Framework ROS
1 . ลำดับการทำงานของ Node โปรแกรม Moblility
ในส่วนของ Mobility มีลำดับการทำงานของโปรแกรม ดังนี้
- รับคำสั่งทิศทางในการเคลื่อนที่ของหุ่นยนต์ จาก Broker MQTT
- คำนวณทิศทางการหมุนของล้อ Mecanum ทั้ง 4 ล้อ เพื่อให้ได้ทิศทางในการเคลื่อนที่ของหุ่นยนต์ตามที่ต้องการและสามารถปรับความเร็วในการเคลื่อนที่ได้ โดยหุ่นยนต์จะสามารถเคลื่อนที่ไปได้ในทุกทิศทาง รวมถึงสามารถหมุนรอบตัวเองได้
- ทำการอ่านค่าความเร็วการหมุนจาก Dynamixel Motor ทั้ง 4 ตัว เพื่อคำนวณความเร็วเชิงเส้นและความเร็วเชิงมุม จากนั้นทำการส่งค่ากลับไปที่ส่วนของ User ผ่าน Broker MQTT
- จะมีตัวรับคำสั่งจาก Lidar Detection Node เพื่อทำการสั่งให้ Motor ทุกตัวหยุดกรณีที่หุ่นยนต์นั้นใกล้ที่จะชนสิ่งกีดขวาง
2. ส่วนของการคำนวณการหมุนของล้อ Mecanum
สมการที่ใช้ในการควบคุมการทำงานของมอเตอร์ให้สอดคล้องกับการทำงานของล้อ
จากสมการทางด้านซ้ายมือจะทำการให้ตัวแปร Vx, Vy, Wz เป็น Input ที่มาจากส่วนของ User Interface โดยตัวแปรที่เป็นค่า parameter R, Lx, Ly จะเป็นค่าคงที่ที่ได้มาจากตัวหุ่นยนต์ และตัวแปร W1, W2, W3, W4 จะเป็น output ซึ่งเป็นค่าความเร็วเชิงมุมของล้อของแต่ละล้อที่จะนำไปควบคุมความเร็วของ Motor เพื่อให้หุ่นยน์สามารถเคลื่อนที่ได้ตามความเร็วและทิศทางตามที่ต้องการ
Library และ Package ที่ใช้
ในส่วนของการสั่งงาน motor dynamixel และรับค่าจาก motor dynamixel จะใช้ package ของ DYNAMIXEL SDK
สามารถดูเพิ่มเติมได้ที่ : https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/
Source Code ในส่วนของ Mobility : https://github.com/pipogood/fromJETSOn/blob/main/mobotrun/script/mobility.py