FRA631 Foundataion of Robotics Class Project 68340700410 พัฒนรัตน์ จีดจีน

หน้าที่ : ควบคุม stepper motor โดยใช้ MKSSERVO42/57D ผ่าน CAN Protocal, ทำ closed loop stepper motor control และ wiring

1. การเรียนรู้ด้าน closed loop stepper motor control

ในส่วนนี้คือการทำ closed loop stepper control เพื่อควบคุมความเร็วขึ้นมาเองโดยใช้ AS5600 และ TB6600 ซึ่งไม่ได้ลงลึกถึง current control สำหรับ control loop นี้มีใว้เพื่อชดเชยความฝืดเล็กน้อยเท่านั้น

(ซ้าย) TB6600, (ขวา) AS5600

เนื่องจาก motor หมุนที่ความถี่สูงสุดไม่เกิน 10Hz (600RPM) จึงเลือกใช้ความถี่ควบคุมที่ 200Hz ซึ่งความหมาย/การทำงานของแตละบล็อคมีดังนี้

CONVERT2MOTORSPEED: แปลงหน่วยให้เป็น rad/s
TRAJECTORY GENERATOR: สร้าง speed profile จากคำสั่งให้มีความสมูท เพื่อป้องกันการสั่งในรูปแบบของ impulse ที่ส่งผลให้ stepper motor เกิด slip
PID: สั่ง speed ชดเชยเมื่อเกิด error ขึ้น
ISR INTERVAL: แปลงค่า speed ที่ได้เป็น interval ที่ใช้สำหรับ tick ของ timer interupt ของ ESP-S
LOWPASS FILTER: กรองสัญญานที่อ่านได้จาก sensor เพื่อลดอาการสั่นของสัญญาณที่เกิดจาก noise

2. การเรียนรู้ด้านการให้ CAN Protocal ควบคุม Step Motor Driver (MKSSERVO42/57D)

MKSSERVO42/57D เป็น closed loop stepper motor controller สำเร็จรูปที่ใช้เทคนิค Field Oriented Control (FOC) ซึ่งทางผู้ผลิตมีสอง Protocal ให้เลือกใช้ คือ RS485 และ CAN สำหรับงานนี้เราเลือก CAN เนื่องจากจำนวนสายไฟที่ใช้ wiring มีเพียง 4 เส้นเมื่อเทียบกับ RS485 คือ VCC, GND, CAN H และ CAN L ทั้งนี้ทางผู้ผลิตไม่ได้จัดทำ Library สำเร็จรูปเพื่อใช้สั่ง ทางเราจึงสร้างขึ้้นมาเอง โดยตัวอย่างรูปแบบการใช้คำสั่ง CAN เช่น

โครงสร้างชุดข้อมูลที่ส่งจากเครื่องคอมพิวเตอร์ไปยังบอร์ดขับมอเตอร์ประกอบด้วยส่วนสำคัญ 5 ไบต์ โดยระบุ CAN ID คือ 01 และกำหนดความยาวข้อมูลที่ 5 ไบต์ เริ่มต้นที่ไบต์ที่ 1 เป็นรหัสคำสั่งระบุค่าคงที่คือ F6H เพื่อเข้าสู่โหมดการทำงาน ต่อมาในไบต์ที่ 2 จะแบ่งพื้นที่ออกเป็น 3 ส่วน คือ บิตที่ 7 ใช้กำหนดทิศทาง บิตที่ 6 ถึง 4 เป็นพื้นที่สำรอง และบิตที่ 3 ถึง 0 จะรวมกับไบต์ที่ 3 ทั้งหมดเพื่อกำหนดค่าความเร็วในการหมุน ในไบต์ที่ 4 จะเป็นส่วนของการกำหนดอัตราเร่งเพื่อความนุ่มนวลในการเคลื่อนที่ และปิดท้ายด้วยไบต์ที่ 5 ซึ่งเป็นค่าตรวจสอบความถูกต้องของข้อมูลเพื่อป้องกันความผิดพลาดระหว่างการรับส่ง

ตัวอย่างการเขียน class เพื่อความง่ายในการดึงคำสั่งมาใช้งาน

เนื่องจากความเร็วที่ driver ตัวนี้สามารถทำได้มีค่าสูงถึง ~1500 RPM การที่จะบรรจุเลขที่มีขนาดมากๆ จึงต้องแบ่งออกเป็น 2 byte คือ byte ที่ 3 และ bit ที่ 0 ถึง 3 ของ byte ที่ 4
เมื่อแปลงค่าความเร็วเป็น binary ตัวแปร speedH จะเก็บค่าของ bit ที่ 8-11 (ได้จาก bitwise operation shift right ไป 8 bit และ and กับ 1111 เพื่อความมั่นใจว่าคัดมา 4 bit จริงๆ) ส่วนตัวแปร speedL จะเก็บค่า bit ที่ 0-10 (ค่าความเร็วมีทะ้งหมด 12 bit)

ตัวอย่างการนำมาใช้