Surrogate Robot : Robot’s head Control (Part 1)
การทำงานโดยรวมของการควบคุมส่วนหัวของหุ่นยนต์
หลักการทำงานในการส่งข้อมูลการหมุนของตัว VR headset เพื่อนำข้อมูลนั้นไปสั่งงาน Servo motor ทั้งระบบนี้จะมีขั้นตอนดังนี้
- คอมพิวเตอร์จากฝั่งผู้ใช้งนอ่านค่าการหมุนของ VR headset ว่าในแต่ละแกนมีการหมุนไปทั้งหมดกี่องศา
- ส่งข้อมูลที่ได้รับมาด้วย TCP (Transmission Control Protocol) ไปที่ Onboard computer ในที่นี้ใช้บอร์ด LattePanda
- นำข้อมูลที่รับมาจากคอมพิวเตอร์ฝั่งผู้ใช้ส่งไปที่บอร์ด Microcontroller NUCLEO-F411RE ผ่าน Serial Communication
- บอร์ด NUCLEO จะทำการแยกข้อมูลที่รับมาเป็น string ให้แปลงเป็น int ทั้งหมด 3 ค่า คือ ค่า X , Y และ Z พร้อมกับนำค่าเหล่านี้ไปควบคุม servo motor ผ่านการส่งข้อมูลแบบ UART
ในบทความนี้เราจะทำในเฉพาะส่วนของข้อ 3 และข้อ 4 ดังรูปที่ 2
การเขียนโปรแกรมควบคุม Servo motor
ก่อนการเริ่มการเขียนฟังก์ชันการควบคุม Servo motor นั้นเราจำเป็นจะต้องตั้งค่าข้อมูลบางอย่างเพื่อให้บอร์ด Nucleo และมอเตอร์สามารถสื่อสารและเข้าใจกันได้ ดังรูปที่ 3
บรรทัดที่ 1–2 : นำไลบรารี่เข้ามาใช้เพื่อควบคุม Servo motor
บรรทัดที่ 8 : ตั้งค่า pin ในการสื่อสารกันระหว่างบอร์ด Nucleo และ Dynamixel servo motor โดย PA_11 และ PA_12 เป็นตำแหน่ง TX pin และ RX pin ตามลำดับ พร้อมทั้งตั้งค่าการความเร็วในการส่งข้อมูล (Baud rate) เท่ากับ 1000000 บิตต่อวินาที (bits per second; bps)
บรรทัดที่ 9 : ตั้งค่า pin ในการสื่อสารกันระหว่างบอร์ด LattePanda และบอร์ด Nucleo โดย D1 และ D0 เป็นตำแหน่ง TX pin และ RX pin ตามลำดับ พร้อมทั้งตั้งค่า Baud rate เท่ากับ 115200 bps
หลังจากทำการตั้งค่าเสร็จเรียบร้อยแล้ว ขั้นตอนต่อไปคือการเขียนโปรแกรมเพื่อควบคุม Servo motor ของแต่ละแกนให้ได้ตามองศาการหมุนของ VR headset ที่ส่งมา ตั้งชื่อฟังก์ชันนี้ว่า RobotHeadControl() มีพารามิเตอร์ทั้งหมด 3 ตัวแปรได้แก่ tilt, pan และ swing โดยทั้ง 3 ตัวแปรเป็นตัวแปรประเภททศนิยม (float)
บรรทัดที่ 53–55 : นำองศาการหมุนที่ได้รับมาแต่ละแกน มาแปลงอัตราส่วนให้เหมาะสมกับการสั่งงานมอเตอร์ โดยค่าที่ใช้ควบคุมตำแหน่งขอมอเตอร์จะอยู่ในช่วง 0–1023 เทียบเป็นองศาจะเท่ากับ 0–300 องศา
บรรทัดที่ 57–59 : นำค่าที่แปลงอัตราส่วนแล้วไปใส่ในฟังก์ชัน setPosition() มีพารามิเตอร์ 3 ตัว คือ MotorID, Position และ Speed โดย MotorID ให้ใส่ไอดีของมอเตอร์ที่เราต้องการควบคุม Position ใส่ตำแหน่งที่เราต้องการให้มอเตอร์หมุนไป และ Speed ความเร็วในการหมุนของมอเตอร์ในที่นี้ตั้งค่าไว้ที่ 200
เท่านี้เราก็สามารถสั่งให้มอเตอร์แต่ละตัวหมุนตามองศาที่เราต้องการได้ทันทีที่ฟังก์ชัน RobotHeadControl() ทำงาน
การรับข้อมูลมาจาก Onboard Computer
ก่อนที่เราจะนำข้อมูลองศาต่างๆ มาใช้ในการควบคุมมอเตอร์ เราจำเป็นจะต้องรับข้อมูลเหล่านี้มาจาก Onboard computer อย่างเช่นบอร์ด LattePanda ซะก่อน โดยรูปแบบของข้อมูลที่ส่งมาจะมีลักษณะคือ “(x, y, z)/” อยู่ในรูปแบบของตัวแปร string โดย x, y และ z หมายถึง องศาการหมุนจากจุดเริ่มต้นของแกน x,y และ z ตามลำดับ ข้อมูลตัวสุดท้ายจะปิดท้ายด้วยสัญลักษณ์ “/” เพื่อให้ทราบว่าเป็นจุดสิ้นสุดของข้อมูลแต่ละชุดแล้ว การส่งข้อมูลผ่าน Serial Communication สามารถดูได้ดังรูปที่ 5
บรรทัดที่ 19 : เป็นการสร้าง Serial Interrupt ขึ้นมา โดยเมื่ออุปกรณ์ device (ในที่นี้เป็นบอร์ด Nucleo) ได้รับข้อมูลเข้ามา ฟังก์ชัน getData() จะทำงานทันที ซึ่งฟังก์ชัน getData() นี้จะทำการรับข้อมูลเข้ามาทีละตัว และเก็บข้อมูลทั้งหมดไว้ในตัวแปร buffer ดังรูปที่ 6
บรรทัดที่ 20 : ทำการเช็คเมื่อข้อมูลใน buffer ปิดท้ายด้วยสัญลักษณ์ “/” ก็จะไปทำที่บรรทัดต่อๆ ไป
บรรทัดที่ 24 -28 : ทำการแยกข้อมูล (x, y, z) ด้วยฟังก์ชัน SplitPos ดังรูปที่ 7 ให้เป็นค่า int ปกติแล้วใส่ในตัวแปรชื่อ X_axis, Y_axis และ Z_axis ตามลำดับ จากนั้นนำตัวแปรทั้ง 3 ตัวไปใส่ในฟังก์ชัน RobotHeadControl() เพื่อควบคุมหัวต่อไป
บรรทัดที่ 30–31 : รีเซ็ตค่า buffer และ index ให้หมดเพื่อรอรับข้อมูลชุดถัดไป
สุดท้ายเราก็จะสามารถนำค่าที่ได้รับมาสั่งงาน Servo motor ตามที่เราต้องการได้เลย