โปรเจกต์ ROBONOVA-1 กับ Arduino Mega

งานนี้เป็นโปรเจกต์จบการศึกษาสำหรับวิชาหนึ่งในมหาวิทยาลัย ผมมีเวลาไม่มากพอที่จะทำให้มันทำอะไรได้มากกว่านี้ แต่หวังว่าการเขียนโปรแกรมนี้จะเป็นประโยชน์สำหรับใครก็ตามที่พยายามทำอะไรที่คล้ายกัน นี่เป็นโปรเจกต์แรกของผมกับ Arduino ดังนั้นคุณอาจจะพบข้อผิดพลาดบ้าง หวังว่าจะได้รับคำแนะนำและเรียนรู้จากข้อผิดพลาดเหล่านั้น

ROBONOVA-1 เป็นหุ่นยนต์ฮิวแมนนอยด์ที่ประกอบด้วย 16 Servos ผมแนบคู่มือไว้ให้ที่นี่:

http://www.flexwatch.es/archivos/ROBONOVA-Manual-ES.pdf

หากใครที่อ่านไฟล์ PDF ได้สะดวกกว่า ผมได้อัปโหลดรายงานไว้ที่นี่ และคณิตศาสตร์ส่วนใหญ่มีการอธิบายไว้ในนั้น :

https://drive.google.com/open?id=1EcAuCnE-ognE42eKcJgjzBTF5YuyyAgy

สิ่งหลักๆ ที่ผมทำคือการแก้โจทย์ Inverse Kinematics ของหุ่นยนต์ตั้งแต่ช่วงเอวจนถึงข้อเท้า และเมื่อกำหนดทิศทางของจุดปลาย (ข้อเท้า) ได้แล้ว ผมก็สามารถกำหนดตำแหน่งของข้อเท้าที่ต้องการตามการเคลื่อนไหวที่ต้องการได้ (ไม่ว่าจะเป็นการทรงตัวบนขาข้างเดียวหรือการรักษาระดับตัวหุ่นยนต์)

การเชื่อมต่อเหมือนกับการควบคุม Servo ทั่วไป แต่มีความแตกต่างตรงที่มี Servo จำนวนมาก คุณจะไม่สามารถจ่ายไฟให้พวกมันผ่าน Arduino ได้ ดังนั้นคุณจะต้องใช้แหล่งจ่ายไฟภายนอก (และอย่าลืมเชื่อมต่อ GND ทั้งจาก Arduino และแหล่งจ่ายไฟภายนอกเข้าด้วยกัน!)

โปรแกรมถูกแยกออกเป็นส่วนๆ เพื่อให้อ่านง่ายขึ้น โปรแกรมนี้ทำงานเฉพาะส่วนขาเท่านั้น แต่ถ้าคุณมี PWM Pins เพียงพอ คุณก็สามารถทำให้มันทำงานได้ทั้งหมด!

ระบบอ้างอิงสำหรับขาแต่ละข้าง

นี่คือระบบอ้างอิงของขาแต่ละข้างที่กำหนดไว้สำหรับการเคลื่อนที่:

และเมื่อพิจารณาตามกฎมือขวา การเคลื่อนที่ของแต่ละข้อต่อและชื่อของมันคือ:

ฟังก์ชัน Leg()

ฟังก์ชันแรกซึ่งเป็นฟังก์ชันพื้นฐานที่สุด คือฟังก์ชันที่คำนวณมุมผ่าน Inverse Kinematics เพื่อให้ขาแต่ละข้างไปยังตำแหน่งที่กำหนด ฟังก์ชันนี้ต้องการ Variable แบบ double สามตัวที่สัมพันธ์กับระบบอ้างอิงของขาแต่ละข้าง จากนั้นจะคำนวณมุมสำหรับ 3 ข้อต่อ และเติมค่าลงใน Array ที่เกี่ยวข้องกับแต่ละข้อต่อ และเนื่องจาก Variable ที่ส่งคืนเป็น Global Variable ที่กำหนดไว้ที่ตอนต้นของโปรแกรม มุมเหล่านี้จึงสามารถนำไปใช้ในฟังก์ชันอื่นๆ ได้โดยไม่จำเป็นต้องประกาศเรียกใช้ Variable ใหม่

void leftleg(double x, double y, double z){ //,int veloc, bool wait){ Para cuandoutilicemos las variables de VarSpeedServo
 //Solving angles
 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);
 //Position to goleg servos 
 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("Calculateright leg");
 //Solving angles
 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);
 //Position togo leg servos
 postogo[3]=a[3]+oq[3];
 postogo[4]=-a[4]+oq[4];
 postogo[5]=-a[5]+oq[5];
} 

ฟังก์ชัน Servosend()

ฟังก์ชันนี้จะส่ง Servo แต่ละตัวไปยังแต่ละตำแหน่งที่กำหนดไว้ระหว่างฟังก์ชัน Leg และ Gestion เพื่อให้เห็นการเคลื่อนไหวที่ชัดเจน Servo ที่ทำงานจะถูกพิมพ์ออกทาง Serial Monitor โปรดทราบว่ามีการใช้ Library VarSpeedServo.h แทนที่จะเป็น Servo.h ทั่วไป ซึ่งช่วยให้เราสามารถควบคุมความเร็วของ Servos ได้โดยการกำหนดตัวเลขระหว่าง 0-255 โดยที่ 255 คือความเร็วสูงสุด ดังนั้นความเร็วระดับ 4 จึงต่ำมาก แต่นี่จะช่วยลดการสั่นของหุ่นยนต์ในแต่ละการเคลื่อนไหว

void servosend(){
 for (int i = 0; i <= 9; i++) //Until 9 inclusive
 {servo[i].write(postogo[i],4,false); //We can set the speed of the movement.
 Serial.print("Poisitioning : ");
 Serial.println(i);
 delay(15);
 }
} 

ฟังก์ชัน Gestion()

ฟังก์ชันนี้รวมการทำงานของฟังก์ชันขา โดยพิจารณาจากการกระทำที่จะนำไปใช้ และประกอบด้วยการทรงตัวไปข้างหน้าและข้างหลัง Variable ตัวแรกที่เรียกใช้คือเพื่อกำหนดตำแหน่งของขาแต่ละข้าง โดย 3 พิกัดแรกเกี่ยวข้องกับขาซ้าย และกลุ่มที่สองสำหรับขาขวา (ลำดับของ Variable ถูกกำหนดไว้เพื่อให้ง่ายต่อการเปรียบเทียบตำแหน่งของเท้าแต่ละข้าง) ถัดไปคือ Variable “acc”, ซึ่งกำหนดการเคลื่อนไหวที่จะทำและเกี่ยวข้องกับวิธีการวางตำแหน่งข้อเท้า ค่า 0 คือการรักษาระดับหุ่นยนต์ ค่า 1 คือการทรงตัวบนขาซ้าย และค่า 2 คือการทรงตัวบนขาขวา จากนั้น Variable “bal” ใช้สำหรับการทรงตัวไปข้างหน้าและข้างหลังของหุ่นยนต์ ไม่ว่าหุ่นยนต์จะกำลังรักษาระดับหรือกำลังทรงตัวอยู่ ก็สามารถตั้งค่าการทรงตัวนี้ได้ หาก Variable นี้เป็นบวก หุ่นยนต์จะเอียงไปข้างหน้า และหากเป็นลบ หุ่นยนต์จะเอียงไปข้างหลัง และสุดท้ายแต่ไม่ท้ายสุด Variable “off” (ย่อมาจาก offset) ใช้เพื่อเพิ่มหรือลดการ Roll ของเท้าข้างที่หุ่นยนต์กำลังทรงตัวอยู่ หาก Variable นี้เป็นบวก เท้าจะ Roll เพื่อให้ขาสามารถรับน้ำหนักของหุ่นยนต์ได้มากขึ้น Variable นี้จะเปลี่ยนการเคลื่อนไหวเมื่อ “acc” ถูกตั้งค่าเป็น 1 หรือ 2 เท่านั้น

void gestion(double x1,double x2,double y1,double y2,double z1,double z2,int acc,int bal,int off){ //Variables 1 are for the left feet and 2 forthe right one.
 leftleg(x1,y1,z1);
 rightleg(x2,y2,z2);
 switch (acc) {
 case 0: //For leveling the feets
 Serial.print("Leveling");
 //Left Foot
 postogo[6]=-a[2]+a[1]+oq[6]-bal;
 postogo[7]=a[0]+oq[7];
 //Right Foot 
 postogo[8]=a[5]-a[4]+oq[8]+bal;
 postogo[9]=a[3]+oq[9];
 break;
 case 1: //Balance over the LEFT leg
 Serial.print("Balance LEFT");
 //Position to go Left Foot
 postogo[6]=-a[2]+a[1]+oq[6]-bal;
 postogo[7]=oq[7]+off;
 //Positioning Right Foot 
 postogo[8]=a[5]-a[4]+oq[8]+bal;
 postogo[9]=oq[9];
 break;
 case 2: //Balance over RIGHT leg
 Serial.print("Balance RIGHT");
 //Position to go Left Foot
 postogo[6]=-a[2]+a[1]+oq[6]-bal;
 postogo[7]=oq[7];
 //Positioning Right Foot 
 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); //Depart from leveled
 gestion (7,7, -1, 0, 0,0, 1,0,0); //Leveling over 1=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); // Weight in one foot
 gestion (7,7, -3,-3, -1,3, 1,-3,10); //Taking foot in the air tothe front
 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); //Passing weight to legmoved
 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);
}