ชื่อโปรเจกต์: ROBONOVA-1 with Arduino Mega
โปรเจกต์นี้เป็นตัวจบสมัยพี่เรียนมหาลัยเองแหละน้อง ตอนนั้นเวลาน้อยไปหน่อยเลยทำฟีเจอร์ได้ไม่เยอะเท่าไหร่ แต่พี่หวังว่าการเขียนโปรแกรมชุดนี้จะมีประโยชน์กับพวกเราที่อยากจะทำอะไรคล้ายๆ กันนะ นี่เป็นโปรเจกต์แรกที่พี่ลองเล่นกับ [Arduino](https://s.shopee.co.th/7fUgFAWSki) เลยล่ะ ถ้าเจอจุดไหนผิดพลาดก็ทักได้นะน้อง พี่พร้อมรับฟังและเรียนรู้ไปกับพวกเราเสมอ
เจ้า ROBONOVA-1 ตัวนี้เป็นหุ่นยนต์ Humanoid ที่ประกอบไปด้วย [Servo](https://s.shopee.co.th/7fUgFAWSki) ถึง 16 ตัว ส่วนคู่มือการใช้งานพี่แปะไว้ให้แล้ว ไปลองอ่านดู:
ถ้าใครถนัดอ่านพวก Report แบบ PDF มากกว่า พี่ก็อัปโหลดรายงานที่อธิบายพวกสมการคณิตศาสตร์ส่วนใหญ่ไว้ให้ตรงนี้แล้วนะ :
หลักๆ ที่พี่ทำคือการแก้สมการ Inverse Kinematics ของหุ่นยนต์ตั้งแต่ช่วงเอวไปจนถึงข้อเท้า (Ankle) พอเรากำหนดทิศทางของจุดปลายเท้าได้แล้ว พี่ก็สามารถสั่งการให้ข้อเท้าขยับไปตามตำแหน่งที่ต้องการได้ จะให้ทรงตัวขาเดียวหรือปรับระดับสมดุลตัวหุ่นก็จัดไปวัยรุ่น
การเชื่อมต่อก็เหมือนการคุม Servo ทั่วไปนั่นแหละ แต่มีจุดต่างที่สำคัญมากคือ Servo มันเยอะจัด น้องจะเอาไฟจาก Arduino มาเลี้ยงตรงๆ ไม่ได้นะ เดี๋ยวบอร์ดไหม้! ต้องใช้ External Source หรือแหล่งจ่ายไฟแยกข้างนอก (และห้ามลืมเด็ดขาด! ต้องต่อ GND จาก Arduino และแหล่งจ่ายไฟนอกเข้าด้วยกันด้วยนะน้อง! ไม่งั้นระบบรวนไม่รู้ด้วยนะ)
โค้ดโปรแกรมพี่แยกไว้เป็นบล็อกๆ จะได้อ่านง่ายหน่อย ตอนนี้พี่เขียนเน้นไปที่ส่วนขา แต่ถ้าพวกเรามี PWM Pins เหลือเฟือ ก็จัดเต็มให้มันขยับทั้งตัวได้เลย!
Reference sistem สำหรับขาแต่ละข้าง
นี่คือระบบอ้างอิง (Reference System) ของขาแต่ละข้างที่พี่กำหนดไว้สำหรับการเคลื่อนที่นะ:
และถ้าอิงตามกฎมือขวา (Right hand rule) การเคลื่อนที่ของแต่ละข้อต่อ (Joint) และชื่อเรียกก็ตามนี้เลยน้อง:
ฟังก์ชัน Leg()
ฟังก์ชันแรกและเบสิกที่สุด คือตัวที่ใช้คำนวณมุมผ่าน Inverse Kinematics เพื่อให้ขาแต่ละข้างไปถึงตำแหน่งที่สั่ง พี่ใช้ Variable แบบ Double 3 ตัวที่สัมพันธ์กับระบบอ้างอิงของขา จากนั้นมันจะคำนวณมุมสำหรับ 3 Joints แล้วเอาค่าไปใส่ใน Array ของแต่ละข้อต่อ เนื่องจากตัวแปรที่ส่งค่ากลับเป็น Global Variable ที่ประกาศไว้ตอนต้นโปรแกรม มุมพวกนี้เลยเอาไปใช้ในฟังก์ชันอื่นได้เลยโดยไม่ต้องประกาศใหม่ให้วุ่นวาย
void leftleg(double x, double y, double z){ //,int veloc, bool wait){ สำหรับตอนใช้ตัวแปรของ VarSpeedServo
//คำนวณมุม
Serial.println("CalculoPierna pierna izquierda");
q[0]=atan(y/x);
q[2]=acos((x*x+y*y+z*z-l1*l1-l2*l2)/(2*l1*l2));
q[1]=atan((l2*sin(q[2]))/(l1+l2*cos(q[2])))+atan(z/sqrt(x*x+y*y));
a[0]=(int)(q[0]*180/Pi);
a[1]=(int)(q[1]*180/Pi);
a[2]=(int)(q[2]*180/Pi);
//ตำแหน่งที่ Servo ขาต้องไป
postogo[0]=a[0]+oq[0];
postogo[1]=a[1]+oq[1];
postogo[2]=a[2]+oq[2];
}
void rightleg(double x, double y, double z){
Serial.println("Calculate right leg");
//คำนวณมุม
q[3]=atan(y/x);
q[5]=acos((x*x+y*y+z*z-l1*l1-l2*l2)/(2*l1*l2));
q[4]=atan((l2*sin(q[5]))/(l1+l2*cos(q[5])))+atan(z/sqrt(x*x+y*y));
a[3]=(int)(q[3]*180/Pi);
a[4]=(int)(q[4]*180/Pi);
a[5]=(int)(q[5]*180/Pi);
//ตำแหน่งที่ Servo ขาต้องไป
postogo[3]=a[3]+oq[3];
postogo[4]=-a[4]+oq[4];
postogo[5]=-a[5]+oq[5];
}
ฟังก์ชัน Servosend()
ฟังก์ชันนี้มีหน้าที่สั่งให้ Servo แต่ละตัวขยับไปที่ตำแหน่งที่กำหนดไว้ในฟังก์ชันขาและการจัดการอื่นๆ เพื่อให้เราเห็นภาพการขยับ พี่เลยสั่งให้ Print ค่า Servo ที่กำลังทำงานผ่าน Serial monitor ด้วยนะ อ้อ! สังเกตดีๆ พี่ใช้ Library VarSpeedServo.h นะน้อง ไม่ใช่ Servo.h ธรรมดาทั่วไป เพราะมันทำให้เราคุมความเร็ว Servo ได้ โดยกำหนดค่า 0-255 (255 คือเร็วสุด) พี่ตั้งความเร็วไว้ที่ 4 ซึ่งช้ามาก แต่มันช่วยลดอาการสั่นของหุ่นตอนขยับได้ดีเลยล่ะ หล่อเท่เลยงานนี้
void servosend(){
for (int i = 0; i <= 9; i++) //ไปจนถึงลำดับที่ 9
{servo[i].write(postogo[i],4,false); //ตั้งความเร็วการเคลื่อนที่ได้ตรงนี้
Serial.print("Poisitioning : ");
Serial.println(i);
delay(15);
}
}
ฟังก์ชัน Gestion()
ฟังก์ชันนี้คือ "ตัวจัดการ" ที่รวมการทำงานของฟังก์ชันขาเข้าด้วยกัน โดยจะดูว่าต้องทำ Action อะไรและคุมการทรงตัวหน้า-หลังด้วย ตัวแปรแรกๆ ที่เรียกใช้คือการกำหนดตำแหน่งขา โดย 3 พิกัดแรกเป็นของขาซ้าย และกลุ่มที่สองเป็นของขาขวา (เรียงแบบนี้จะได้เทียบตำแหน่งเท้าสองข้างง่ายๆ) ต่อมาคือตัวแปร "acc" เอาไว้กำหนดว่าจะให้หุ่นทำอะไร ซึ่งเกี่ยวกับการวางตำแหน่งข้อเท้า ถ้าเป็น 0 คือยืนระนาบปกติ, 1 คือทรงตัวด้วยขาซ้าย, และ 2 คือทรงตัวด้วยขาขวา ส่วนตัวแปร “bal” เอาไว้คุมสมดุลหน้า-หลัง ถ้าค่าเป็นบวกหุ่นจะโน้มไปข้างหน้า ถ้าเป็นลบจะเอนไปข้างหลัง และสุดท้ายแต่สำคัญไม่แพ้กันคือตัวแปร “off” (ย่อมาจาก offset) เอาไว้เพิ่มหรือลดการ Roll ของเท้าข้างที่หุ่นทรงตัวอยู่ ถ้าค่าเป็นบวก เท้าจะ Roll เพื่อให้ขารับน้ำหนักหุ่นได้ดีขึ้น ตัวแปรนี้จะทำงานเฉพาะตอน "acc" เป็น 1 หรือ 2 เท่านั้นนะน้อง
void gestion(double x1,double x2,double y1,double y2,double z1,double z2,int acc,int bal,int off){ //ตัวแปร 1 คือเท้าซ้าย 2 คือเท้าขวา
leftleg(x1,y1,z1);
rightleg(x2,y2,z2);
switch (acc) {
case 0: //สำหรับปรับระดับเท้าให้ขนานพื้น
Serial.print("Leveling");
//เท้าซ้าย
postogo[6]=-a[2]+a[1]+oq[6]-bal;
postogo[7]=a[0]+oq[7];
//เท้าขวา
postogo[8]=a[5]-a[4]+oq[8]+bal;
postogo[9]=a[3]+oq[9];
break;
case 1: //ทรงตัวด้วยขาซ้าย
Serial.print("Balance LEFT");
//ตำแหน่งเท้าซ้าย
postogo[6]=-a[2]+a[1]+oq[6]-bal;
postogo[7]=oq[7]+off;
//ตำแหน่งเท้าขวา
postogo[8]=a[5]-a[4]+oq[8]+bal;
postogo[9]=oq[9];
break;
case 2: //ทรงตัวด้วยขาขวา
Serial.print("Balance RIGHT");
//ตำแหน่งเท้าซ้าย
postogo[6]=-a[2]+a[1]+oq[6]-bal;
postogo[7]=oq[7];
//ตำแหน่งเท้าขวา
postogo[8]=a[5]-a[4]+oq[8]+bal;
postogo[9]=oq[9]-off;
break;
}
servosend();
delay(1000);
}
ฟังก์ชัน Step()
ฟังก์ชันนี้รวมจุดเคลื่อนที่ต่างๆ เพื่อให้หุ่นก้าวเดินได้หนึ่งก้าว พี่พยายามเลียนแบบการเดินของมนุษย์จริงๆ ในแต่ละก้าวที่เดินไปข้างหน้า แต่มันจะมีจุดต่างกันนิดหน่อยในแต่ละก้าว เพราะตำแหน่งของสายไฟมันส่งผลต่อการขยับไม่เหมือนกันในแต่ละข้าง สู้งานนะน้อง!
void rightstep() {
Serial.print("Right Step");
delay(1000);
gestion (8,8, 0, 0, 0,0, 0,0,0); //เริ่มจากท่ากางขาปกติ
gestion (7,7, -1, 0, 0,0, 1,0,0); //เอียงไปทางซ้าย (LEFT)
gestion (7,9, -2,-1, -1,1, 1,0,0);
gestion (7,9, -2,-2, -1,1, 1,5,5);
gestion (7,8, -3,-3, -1,1, 1,2,10); // ถ่ายน้ำหนักลงเท้าข้างเดียว
gestion (7,7, -3,-3, -1,3, 1,-3,10); //ยกขาขึ้นกลางอากาศแล้วเหวี่ยงไปข้างหน้า
gestion (7,7, -3,-3, -1,4, 1,-3,10); //
gestion (7,9, -3,-3, 0,4, 1,-3,10);
gestion (7,9, -2,-2, 0,4, 1,0,10);
gestion (8,8, -1,-1, 0,4, 0,5,0);
gestion (8,8, -1,-1, 0,4, 2,5,5);
gestion (8,8, -1,-1, 0,4, 2,10,10); //ถ่ายน้ำหนักไปที่ขาข้างที่ขยับไป
gestion (6,8, 0, 0, 4,4, 2,10,10);
gestion (8,8, 0, 0, 4,4, 2,10,10);
gestion (8,8, 0, 0, 0,0, 0,5,0);
delay(1000);
}
รายละเอียดเทคนิคเพิ่มเติม (Expanded Technical Details)
อัปเกรดการควบคุมหุ่นยนต์ 2 ขา (Bipedal Robotic Control Upgrade)
โปรเจกต์นี้เป็นการอัปเกรดหุ่นยนต์ Humanoid สุดคลาสสิกอย่าง ROBONOVA-1 โดยเปลี่ยนสมองกลจากเดิมมาเป็น Arduino Mega 2560 ที่แรงกว่าเดิมเยอะ ทำให้เราคุมท่าทางได้ละเอียดขึ้นและเชื่อมต่อ Sensor เพิ่มได้เพียบ
- Multi-Servo Coordination: บอร์ด Mega ตัวนี้คุม HSR-8498HB digital servos แรงบิดสูงได้ถึง 16 ตัว ผ่าน hardware PWM pins 15 ช่อง และใช้ timer software มาช่วยเสริม ทำให้การเดินและการทรงตัวดูลื่นไหลเหมือนมีชีวิตจริงๆ
- Inertial Measurement (IMU): พี่ใส่ MPU6050 เข้าไปเพื่อทำระบบ "Self-Balancing" หรือทรงตัวอัตโนมัติ ถ้าหุ่นเริ่มเอียงเกินไป Arduino จะสั่งแก้ท่าทางที่ Servo ตรงข้อเท้า (Ankle) และหัวเข่า (Knee) ทันทีเพื่อรักษาจุดศูนย์ถ่วงไว้ ไม่ให้ล้มหน้าคะมำแน่นอน
ตรรกะการเดินขั้นสูง (High-Level Gait Logic)
- Kinematic Calculation: ตัว Mega จะคำนวณ Inverse Kinematics (IK) แบบ Real-time เพื่อหาองศาของ Servo ที่แม่นยำที่สุดสำหรับตำแหน่งเท้าที่เราต้องการ พี่ไม่ได้ใช้วิธี Hard-code แบบจำท่าทางเป็นเฟรมๆ (Frames) เหมือนสมัยก่อนแล้วนะ แบบนี้มันยืดหยุ่นกว่าเยอะ จัดไปวัยรุ่น!