3...โปรเจค หุ่นยนต์ฮิวแมนนอยด์ 12 DOF เพื่อการศึกษา
โปรเจค หุ่นยนต์ฮิวแมนนอยด์ 12 DOF เพื่อการศึกษา
เมื่อ 7 เดือนที่ผ่านมา
12 DOF Biped Robotic Educational Robot Humanoid Robot Servo Bracket
โปรเจค หุ่นยนต์ฮิวแมนนอยด์ 12 DOF อุปกรณ์ที่ต้องใช้ก็คือ
ขั้นตอนการประกอบ โปรเจค หุ่นยนต์ฮิวแมนนอยด์ 12 DOF เพื่อการศึกษา ส่วนล่างจะคล้ายๆกับ โปรเจค หุ่นยนต์ฮิวแมนนอยด์ 8 DOF เพื่อการศึกษา
เพียงแต่เปลี่ยน Large U Bracket (ด้านบนสุด) จากแนวตั้ง
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiHx7qzZlKiGqtn6AiIkki6HS7kKm_TXDQFXX7ZjKmlvKhvFBzRPv55uKH4TjupG5Jy6HEQj3wT0NzYKUR1yt0qqTpLbBk7Y55kjnF2z_TBVnnEbjGkaZUQZDBfRp2T063HQ_K4ZJEJodaz/s640/1.jpg)
ยึด L Bracket 2 ชิ้น ไปบน Large U Bracket ดังรูป
ยึด Servo Bracket 2 ชิ้น เข้ากับ L Bracket
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhVHQqNnKB6vDldpv7Lgm3kJ5Nz_aU1j1NKwZGKfAiAPQAu_A6s3yEPKbXrFuuAYzkOlOHpLtJdhoWR8dsrE2vAH2Yjw2m_6AGEP48VNpZgLBwEby5B3WKdN4WQdFfvObtYsmxeQ5764sUS/s640/3.jpg)
ยึด เซอร์โวมอเตอร์ MG996R และ Disc Metal 2 ชิ้น เข้ากับ Servo Bracket
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhzgVZKn324Qfy8MtWtkvmKP0C3K7LOFKFQqU7zRRZ1vb-FUWlkQKNe95ADABZmczsBvqfYsL_Gv9ssmNkym_HU6zwwC5RcFM2i1AjZchIpUhgv34ofSoWOmkAOT8p59k-dYhqbkIUw3TFV/s640/4.jpg)
ยึด Tilt U Bracket (ยูเอียง) 2 ชิ้น เข้ากับ Disc Metal ของ เซอร์โวมอเตอร์ MG996R เพื่อเป็น ไหล่ของหุ่นยนต์
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEg0ketX1m_0cPxS_iOpUUloFumjBieC46SkhUTMA2sQOD4CfAqqJ8HwwrJn4hrXXqq39mJSnZlVlNXZm3fsQ-VlCcF_1QahD4j-TJJHOznapBC1TZH9KdbCDyZEvAzBIHQT6MJfB_mwywXg/s640/5.jpg)
ยึด Servo Bracket 2 ชิ้น เข้ากับ Tilt U Bracket
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgyYk5uLdQ_FdRJHheX-7F6yIKSMa6TVSMhUaP-ZhuJIQBVtUwMBNBJe-b37ypff7T7zgXQoRivOlcPyLmwScBsKHDFR8Dk8ZRWjwQAGr5z4_yFXyl2Pwg0F4NNL3l6d7ucx5iAMJpIb3ac/s640/6.jpg)
ประกอบ L Bracket 2 ชิ้น เข้ากับ Short U Bracket
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhZgiNMzrV0IRXjd8gWYfaCgGSWxzWis2PFGFRmEzv0wPUIvKAR6lN_z6_3Tixq16J-1bBWK_XNHAc1tBc0WDdN7DYyDWAEROS2p2ZAjVvUKf9Oz-0rDKEvf-BZhIoQg8FuJgHHh_O6gA-P/s640/7.jpg)
ยึด ชุด L Bracket ที่ประกอบอยู่กับ Short U Bracket เข้ากับ Servo Bracket ทั้ง 2 ด้าน
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgSieyrX0-KWFhuwQTEyUMlrII_oVfFcoqGKypeBfKJSTMoOmJxLERmCL3CICHDFVzvtjaOJFUuIWVNsk1mfBKtsuoggW3MNUO1QdeWt45cz870ISWUEu4O1eWmRxFiW6StYiJdlZHNaG40/s640/8.jpg)
ยึด เซอร์โวมอเตอร์ MG996R เข้าไปที่แขนของหุ่นยนต์ ทั้ง 2 ด้าน
ยึด Long U Bracket เข้ากับ Large U Bracket เพื่อเป็นลำตัวของหุ่นยนต์
และ ยึด Short U Bracket เข้าที่ด้านบนของ Long U Bracket เพื่อเป็นส่วนหัว ของหุ่นยนต์
ภาพรวม หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ด้านหน้าเมื่อประกอบเสร็จ
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiXvN0NPim_4JN62r5UdJXacz3Efz_TTaKe0ll89pUHxK8-TLKiP0L4UGzPqd39gOAJS5ywUF0rHf7P7U1ya6Ux4jUujHU40Sfa1mTBMH9MkbnJ-0rrZhdvi3yXy473bhlZIjud5WvU1xXT/s640/12.jpg)
ด้านหลัง ยึด แผ่นอะคริลิค ขนาด 9 x 11 เซ็นติเมตร จำนวน 1 แผ่น เข้าที่ด้านหลังของหุ่นยนต์
และ ประกอบ Arduino UNO R3 กับ Sensor Shield V5.0เข้า กับ แผ่นอะคริลิค
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiTGHoIurf9_VEjSdcGwFwaQ9C3kj5rj6bAdiEEoFDjedFY1bPFK1ibDE4qdkbtNJ28JpQvM-9O1oP38fIg8TynxoN224YADIDn_3_6_SPlq3RJYL4MtEiUH5JrnKl16OWi5FAinvOREuBc/s640/14.jpg)
ยึด รางถ่านแบบ 18650 (2 ก้อน) นำสายสีแดง ขั้วบวก ของรางถ่าน ต่อเข้ากับ VCC และ สายสีดำ ขั้วลบ ต่อเข้ากับ GND ของ บอร์ด Sensor Shield V5.0
และ ต่อสาย เซอร์โวมอเตอร์ MG996R ทุกตัว เข้าที่ บอร์ด Sensor Shield V5.0
//Servo input pins
rightAnkle.attach(2); // เท้าขวา
rightKnee.attach(3); // เข่าขวา
rightThigh.attach(4); // ต้นขาขวา
rightHip.attach(5); // สะโพกขวา
leftAnkle.attach(6); // เท้าซ้าย
leftKnee.attach(7); // เข่าซ้าย
leftThigh.attach(8); // ต้นขาซ้าย
leftHip.attach(9); // สะโพกซ้าย
leftShoulder.attach(10); // ไหล่ซ้าย
rightShoulder.attach(11); // ไหล่ขวา
leftElbow.attach(12); // ข้อศอกซ้าย
rightElbow.attach(13); // ข้อศอกขวา
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEh1UAnU7wJ6CeZx9PQB39atTkj3n-hFryCEzzXVq1wepKwU3ug6t0-BJYCOrcMSrf0QlaJGpZgtpSbt7NDbdXFKd6X3TB8KRsGn0UGWzO6qzNNAOum3-aXc9XjDmDnTbOKp4Vwel67UWpQj/s640/6034.jpg)
ภาพรวม หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ด้านหลัง เมื่อประกอบเสร็จ
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEj2qMf4wZJ5rJ38sgm6dXCnMASuPHELSAq3kLi6TnjKDaQVUxlyaAxACU3EZBJDw9NZOfNAtO3mjc7Fish7nu7-1t6Tky-FLdtCU7L4qmZiikTfVmkA0PH_Y_gzsgrNAz_gYQamlSfKKi0H/s640/6035.jpg)
ภาพรวม หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ด้านหน้า เมื่อประกอบเสร็จ
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjg7YIlEoxj2QwB1-4h5lg6YoEJ0rjwK1EpPo64r5nRIg6mF1_5yb39qgWJmlvZE-1rKoeVjfr8lp4wMoPyxbMn_jVd0-TBn8oixEKHhfCKJUMqXp3VMjmFsVeq8aSTSAWCH9ccMwyme8FP/s640/1.jpg)
credit :
ลิงค์ youtube ต้นแบบ https://www.youtube.com/watch?v=eP7R-Hlo8pQ
ลิงค์โค้ดต้นแบบ https://www.dropbox.com/s/d2c3mxcu74s84qr/Biped_2.ino?dl=0
เปิดโปรแกรม Arduino (IDE) และ Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3
#include <Servo.h>
int delayVal2 = 25;
int delayVal = 40;
bool time = true;
//Creating Servo Objects
Servo rightAnkle; // เท้าขวา
Servo leftAnkle; // เท้าซ้าย
Servo rightKnee; // เข่าขวา
Servo leftKnee; // เข่าซ้าย
Servo rightThigh; // ต้นขาขวา
Servo leftThigh; // ต้นขาซ้าย
Servo rightHip; // สะโพกขวา
Servo leftHip; // สะโพกซ้าย
Servo leftShoulder; // ไหล่ซ้าย
Servo rightShoulder; // ไหล่ขวา
Servo leftElbow; // ข้อศอกซ้าย
Servo rightElbow; // ข้อศอกขวา
void setup() {
//Servo input pins
rightAnkle.attach(2); // เท้าขวา
rightKnee.attach(3); // เข่าขวา
rightThigh.attach(4); // ต้นขาขวา
rightHip.attach(5); // สะโพกขวา
leftAnkle.attach(6); // เท้าซ้าย
leftKnee.attach(7); // เข่าซ้าย
leftThigh.attach(8); // ต้นขาซ้าย
leftHip.attach(9); // สะโพกซ้าย
leftShoulder.attach(10); // ไหล่ซ้าย
rightShoulder.attach(11); // ไหล่ขวา
leftElbow.attach(12); // ข้อศอกซ้าย
rightElbow.attach(13); // ข้อศอกขวา
delay(2000);
stand();
}
void loop()
{
stand();
turnLeft();
//forward();
}
int rightAnkPos = 90;
int rightKneePos = 90;
int rightThighPos = 90;
int rightHipPos = 90;
int leftAnkPos = 90;
int leftKneePos = 90;
int leftThighPos = 90;
int leftHipPos = 90;
//ปรับค่าตามความเหมาะสม//
void stand ()
{
rightAnkle.write(100); // เท้าขวา
int rightAnkPos = 100;
rightKnee.write(100); // เข่าขวา
int rightKneePos = 100;
rightThigh.write(110); // ต้นขาขวา
int rightThighPos = 110;
rightHip.write(90); // สะโพกขวา
int rightHipPos = 90;
leftAnkle.write(80); // เท้าซ้าย
int leftAnkPos = 90;
leftKnee.write(90); // เข่าซ้าย
int leftKneePos = 90;
leftThigh.write(80); // ต้นขาซ้าย
int leftThighPos = 80;
leftHip.write(92); // สะโพกซ้าย
int leftHipPos = 92;
rightShoulder.write(90); // ไหล่ขวา
leftShoulder.write(90); // ไหล่ซ้าย
rightElbow.write(90); // ข้อศอกขวา
leftElbow.write(90); // ข้อศอกซ้าย
}
int rightShoulderPos = 180;
int rightElbowPos = 90;
int leftShoulderPos = 180;
int leftElbowPos = 90;
void handDemo()
{
stand();
delay(1000);
handsHalf();
delay(250);
handsRight();
delay(250);
handsLeft();
delay(250);
handsRight();
delay(250);
handsHalfdown();
delay(250);
rightHandsUp();
delay(250);
rightHandsDown();
delay(250);
leftHandsUp();
delay(250);
leftHandsDown();
delay(250);
handWave();
}
void handsUp()
{
for (int i = 0; i < 18; i += 1)
{
rightShoulderPos -= 9;
leftShoulderPos -= 10;
leftShoulder.write(leftShoulderPos);
rightShoulder.write(rightShoulderPos );
delay(50);
}
}
void leftHandsUp()
{
for (int i = 0; i < 18; i += 1)
{
leftShoulderPos -= 10;
leftShoulder.write(leftShoulderPos);
delay(50);
}
}
void leftHandsDown()
{
for (int i = 0; i < 18; i += 1)
{
leftShoulderPos += 10;
leftShoulder.write(leftShoulderPos);
delay(50);
}
}
void rightHandsUp()
{
for (int i = 0; i < 18; i += 1)
{
rightShoulderPos -= 9;
rightShoulder.write(rightShoulderPos );
delay(50);
}
}
void rightHandsDown()
{
for (int i = 0; i < 18; i += 1)
{
rightShoulderPos += 9;
rightShoulder.write(rightShoulderPos );
delay(50);
}
}
void handsHalf()
{
for (int i = 0; i < 9; i += 1)
{
rightShoulderPos -= 9;
leftShoulderPos -= 10;
leftShoulder.write(leftShoulderPos);
rightShoulder.write(rightShoulderPos );
delay(50);
}
}
void handsHalfdown()
{
for (int i = 0; i < 9; i += 1)
{
rightShoulderPos += 9;
leftShoulderPos += 10;
leftShoulder.write(leftShoulderPos);
rightShoulder.write(rightShoulderPos );
delay(50);
}
}
void handsDown()
{
for (int i = 0; i < 18; i += 1)
{
rightShoulderPos += 9;
leftShoulderPos += 10;
leftShoulder.write(leftShoulderPos);
rightShoulder.write(rightShoulderPos );
delay(25);
}
}
void handWave()
{
leftThigh.write(75);
rightThigh.write(90);
handsUp();
for (int i = 0; i < 3; i++)
{
handsRight();
handsLeft();
handsRight();
}
delay(250);
handsDown();
}
void handsRight()
{
for (int i = 0; i < 9; i += 1)
{
rightElbowPos += 7;
leftElbowPos += 7;
leftElbow.write(leftElbowPos);
rightElbow.write(rightElbowPos );
delay(25);
}
}
void handsLeft()
{
for (int i = 0; i < 18; i += 1)
{
rightElbowPos -= 7;
leftElbowPos -= 7;
leftElbow.write(leftElbowPos);
rightElbow.write(rightElbowPos );
delay(25);
}
}
void hello()
{
leftHandsUp();
for (int i = 0; i < 3; i += 1)
{
for (int i = 0; i < 9; i += 1)
{
leftElbowPos += 7;
leftElbow.write(leftElbowPos);
delay(25);
}
for (int i = 0; i < 9; i += 1)
{
leftElbowPos -= 7;
leftElbow.write(leftElbowPos);
delay(25);
}
for (int i = 0; i < 9; i += 1)
{
leftElbowPos -= 7;
leftElbow.write(leftElbowPos);
delay(25);
}
for (int i = 0; i < 9; i += 1)
{
leftElbowPos += 7;
leftElbow.write(leftElbowPos);
delay(25);
}
}
delay(500);
leftHandsDown();
}
///////////////Legs//////////////////////
void legs()
{
for (int i = 0; i < 6; i += 1)
{
leftAnkPos += 2;
rightAnkPos -= 2;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(delayVal2);
}
delay(10);
for (int i = 0; i < 6; i += 1)
{
leftAnkPos -= 2;
rightAnkPos += 2;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(delayVal2);
}
delay(10);
for (int i = 0; i < 6 ; i += 1)
{
leftAnkPos -= 2;
rightAnkPos += 2;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(delayVal2);
}
delay(10);
for (int i = 0; i < 6; i += 1)
{
leftAnkPos += 2;
rightAnkPos -= 2;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(delayVal2);
}
delay(10);
}
////////////FORWARD///////////////////////////////////////////////////////
void forward()
{
for (int i = 0; i < 5; i += 1)
{
if (time) {
leftThighPos -= 2;
leftKneePos += 2;
leftThigh.write( leftThighPos);
leftKnee.write(leftKneePos );
delay(delayVal2);
}
}
//Left and Right Ankle Bend Left
for (int i = 0; i < 6; i += 1)
{
leftAnkPos += 2;
rightAnkPos += 2;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(delayVal2);
}
delay(delayVal);
//Straighten Left Leg
for (int i = 0; i < 5; i += 1)
{
leftThighPos += 2;
leftKneePos -= 2;
leftThigh.write( leftThighPos);
leftKnee.write(leftKneePos );
delay(delayVal2);
}
//right leg forward
for (int i = 0; i < 5; i += 1)
{
rightThighPos += 2;
rightKneePos -= 2;
rightThigh.write( rightThighPos);
rightKnee.write(rightKneePos );
delay(delayVal2);
}
//Right and Left Ankle back to Normal
for (int i = 0; i < 6; i += 1)
{
leftAnkPos -= 2;
rightAnkPos -= 2;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(delayVal2);
}
//Right and left Ankle Bend right
for (int i = 0; i < 6 ; i += 1)
{
leftAnkPos -= 2;
rightAnkPos -= 2;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(delayVal2);
}
delay(delayVal);
//Straighten Right Leg
for (int i = 0; i < 5; i += 1)
{
rightThighPos -= 2;
rightKneePos += 2;
rightThigh.write( rightThighPos);
rightKnee.write(rightKneePos );
delay(delayVal2);
}
//Left Leg bend Forward
for (int i = 0; i < 5; i += 1)
{
leftThighPos -= 2;
leftKneePos += 2;
leftThigh.write( leftThighPos);
leftKnee.write(leftKneePos );
delay(delayVal2);
}
//Ankles back to normal
for (int i = 0; i < 6; i += 1)
{
leftAnkPos += 2;
rightAnkPos += 2;
rightAnkle.write(rightAnkPos);
leftAnkle.write(leftAnkPos);
delay(delayVal2);
}
time = false;
/*
*/
}
///////////////LEFT/////////////////////////////////////////
void turnLeft()
{
for (int i = 0; i < 5; i += 1)
{
leftThighPos -= 2;
leftKneePos += 2;
leftThigh.write( leftThighPos);
leftKnee.write(leftKneePos );
rightShoulder.write(90); // ไหล่ขวา
leftShoulder.write(180); // ไหล่ซ้าย
delay(60);
}
//Left and Right Ankle Bend Left
for (int i = 0; i < 5; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
rightShoulder.write(180); // ไหล่ขวา
leftShoulder.write(90); // ไหล่ซ้าย
delay(60);
}
delay(delayVal);
//Left Hip Counter ClockWise,
for (int i = 0; i < 5; i += 1)
{
leftHipPos -= 4;
leftHip.write(leftHipPos);
delay(60);
}
delay(delayVal);
//Left and Right Ankle back to normal
for (int i = 0; i < 5; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
//Left and Right Ankle Bend Right Left Hip Straighten
for (int i = 0; i < 5; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
leftHipPos += 4;
leftHip.write(leftHipPos);
delay(60);
}
delay(delayVal);
//Right Hip Counter Clockwise
for (int i = 0; i < 5; i += 1)
{
rightShoulder.write(90); // ไหล่ขวา
rightHipPos -= 4;
rightHip.write(rightHipPos);
leftHipPos += 4;
leftHip.write(leftHipPos);
delay(60);
}
delay(delayVal);
//Left and Right Ankle Normal
for (int i = 0; i < 5; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
delay(delayVal);
///////SECOND TIME///////////////
//Left and Right Ankle BEND LEFT
for (int i = 0; i < 5; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
leftThighPos += 2;
leftKneePos -= 2;
leftThigh.write( leftThighPos);
leftKnee.write(leftKneePos );
delay(60);
}
delay(delayVal);
//LEFT HIP CC
for (int i = 0; i < 5; i += 1)
{
leftHipPos -= 4;
leftHip.write(leftHipPos);
rightHipPos += 4;
rightHip.write(rightHipPos);
rightShoulder.write(90); // ไหล่ขวา
leftShoulder.write(180); // ไหล่ซ้าย
delay(60);
}
delay(delayVal);
//LEFT RIGHT ANKLE NORMAL
for (int i = 0; i < 5; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
//Left and Right Ankle Bend Right
for (int i = 0; i < 5; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
delay(delayVal);
//Right Hip Counter Clockwise
for (int i = 0; i < 5; i += 1)
{
rightHipPos -= 5;
rightHip.write(rightHipPos);
leftHipPos += 2;
leftHip.write(leftHipPos);
delay(60);
}
//LEFT RIGHT ANKLE NORMAL
for (int i = 0; i < 5; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
//LEFT RIGHT ANKLE BEND LEFT
for (int i = 0; i < 5; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
rightHipPos += 5;
rightHip.write(rightHipPos);
leftHipPos -= 2;
leftHip.write(leftHipPos);
delay(60);
}
//LEFT RIGHT ANKLE STRAIGHT
for (int i = 0; i < 5; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
}
/////////////////TURN RIGHT///////////////////////////////////////////////
void turnRight ()
{
for (int i = 0; i < 5; i += 1)
{
rightThighPos += 1;
rightKneePos -= 1;
rightThigh.write( rightThighPos);
rightKnee.write(rightKneePos );
delay(60);
}
//Left and Right Ankle Bend Right
for (int i = 0; i < 5; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
delay(delayVal);
//right Hip ClockWise, Left leg forward
for (int i = 0; i < 5; i += 1)
{
rightHipPos += 4;
rightHip.write(rightHipPos);
delay(60);
}
delay(delayVal);
//Left and Right Ankle back to normal
for (int i = 0; i < 5; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
//Left and Right Ankle Bend Leftt right Hip Straighten
for (int i = 0; i < 5; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
rightHipPos -= 4;
rightHip.write(rightHipPos);
delay(60);
}
delay(delayVal);
//Left Hip Counter Clockwise
for (int i = 0; i < 5; i += 1)
{
rightHipPos -= 4;
rightHip.write(rightHipPos);
leftHipPos += 4;
leftHip.write(leftHipPos);
delay(60);
}
delay(delayVal);
//Left and Right Ankle Normal
for (int i = 0; i < 5; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
delay(delayVal);
//Second Time
//Left and Right Ankle BEND RIGHT, Right Thigh/Knee Straighten
for (int i = 0; i < 6; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
delay(delayVal);
//Right HIP CW, Left Hip Straighten
for (int i = 0; i < 5; i += 1)
{
rightHipPos += 4;
leftHipPos -= 4;
leftHip.write(leftHipPos);
rightHip.write(rightHipPos);
delay(60);
}
delay(delayVal);
//LEFT RIGHT ANKLE NORMAL
for (int i = 0; i < 6; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
////////Third Time //////////
//Left and Right Ankle Bend Left
for (int i = 0; i < 6; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
delay(delayVal);
//Left Right Hip CW
for (int i = 0; i < 5; i += 1)
{
rightHipPos -= 5;
rightHip.write(rightHipPos);
leftHipPos += 2;
leftHip.write(leftHipPos);
rightThighPos -= 1;
rightKneePos += 1;
rightThigh.write( rightThighPos);
rightKnee.write(rightKneePos );
delay(60);
}
//LEFT RIGHT ANKLE NORMAL
for (int i = 0; i < 6; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
//LEFT RIGHT ANKLE BEND Right
for (int i = 0; i < 6; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
//Right Hip Normal
for (int i = 0; i < 5; i += 1)
{
rightHipPos += 5;
rightHip.write(rightHipPos);
delay(60);
}
//Left Hip Normal
for (int i = 0; i < 5; i += 1)
{
leftHipPos -= 2;
leftHip.write(leftHipPos);
delay(60);
}
//LEFT RIGHT ANKLE STRAIGHT
for (int i = 0; i < 6; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
/*
*/
}
และเพื่อเพิ่มประสิทธิภาพในการจ่ายไฟเลี้ยง เซอร์โวมอเตอร์ MG996R ต้องเพิ่ม Jack 5.5 X 2.1mm สำหรับ เพาเวอร์ซัพพลาย ของ Arduino ตัวผู้ ต่อจากรางถ่าน ขั้วบวก + สีแดง และ ขั้วลบ- สีดำ ของ แบตเตอรี่ 18650 เข้าไปยัง Jack ซัพพลายตัวเมีย ของ Arduino UNO R3 ดังรูปด้วย
เราจะลองเขียนโค้ดง่ายๆ เพื่อให้ หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ก้าวเท้าขวา ไปข้างหน้า
เปิดโปรแกรม Arduino (IDE) และ Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3
#include <Servo.h>
//Creating Servo Objects
Servo rightAnkle; // เท้าขวา
Servo leftAnkle; // เท้าซ้าย
Servo rightKnee; // เข่าขวา
Servo leftKnee; // เข่าซ้าย
Servo rightThigh; // ต้นขาขวา
Servo leftThigh; // ต้นขาซ้าย
Servo rightHip; // สะโพกขวา
Servo leftHip; // สะโพกซ้าย
Servo leftShoulder; // ไหล่ซ้าย
Servo rightShoulder; // ไหล่ขวา
Servo leftElbow; // ข้อศอกซ้าย
Servo rightElbow; // ข้อศอกขวา
void setup() {
//Servo input pins
rightAnkle.attach(2); // เท้าขวา
rightKnee.attach(3); // เข่าขวา
rightThigh.attach(4); // ต้นขาขวา
rightHip.attach(5); // สะโพกขวา
leftAnkle.attach(6); // เท้าซ้าย
leftKnee.attach(7); // เข่าซ้าย
leftThigh.attach(8); // ต้นขาซ้าย
leftHip.attach(9); // สะโพกซ้าย
leftShoulder.attach(10); // ไหล่ซ้าย
rightShoulder.attach(11); // ไหล่ขวา
leftElbow.attach(12); // ข้อศอกซ้าย
rightElbow.attach(13); // ข้อศอกขวา
rightShoulder.write(90); // ไหล่ขวา
leftShoulder.write(90); // ไหล่ซ้าย
rightElbow.write(90); // ข้อศอกขวา
leftElbow.write(90); // ข้อศอกซ้าย
rightAnkle.write(100); // เท้าขวา
rightKnee.write(100); // เข่าขวา
rightThigh.write(100); // ต้นขาขวา
rightHip.write(90); // สะโพกขวา
leftAnkle.write(80); // เท้าซ้าย
leftKnee.write(90); // เข่าซ้าย
leftThigh.write(90); // ต้นขาซ้าย
leftHip.write(92); // สะโพกซ้าย
rightAnkle.write(100); // เท้าขวา
delay(3000);
}
void loop()
{
right_foot();
stand ();
}
//ปรับค่าตามความเหมาะสม//
void stand ()
{
rightShoulder.write(90); // ไหล่ขวา
leftShoulder.write(90); // ไหล่ซ้าย
rightElbow.write(90); // ข้อศอกขวา
leftElbow.write(90); // ข้อศอกซ้าย
rightAnkle.write(100); // เท้าขวา
rightKnee.write(100); // เข่าขวา
rightThigh.write(100); // ต้นขาขวา
rightHip.write(90); // สะโพกขวา
leftAnkle.write(80); // เท้าซ้าย
leftKnee.write(90); // เข่าซ้าย
leftThigh.write(90); // ต้นขาซ้าย
leftHip.write(92); // สะโพกซ้าย
rightAnkle.write(100); // เท้าขวา
}
void right_foot ()
{
rightAnkle.write(65); // บิดเท้าขวาเพื่อส่งเท้าซ้าย
leftAnkle.write(65); // บิดเท้าซ้ายเพื่อยืน
rightKnee.write(80); // ปรับเข่าขวา
delay(1000);
rightAnkle.write(100); // เท้าขวาคืนปรกติ
rightThigh.write(130); // ต้นขาขวาเครื่อนไปข้างหน้า
delay(1000);
leftAnkle.write(80); // เท้าซ้ายปรกติ
leftKnee.write(80); // ปรับเข่าซ้าย
delay(1000);
rightThigh.write(100); // ต้นขาขวาปรกติ
rightKnee.write(100); // เข่าขวาปรกติ
}
วีดีโอผลลัพธ์การทำงานของ หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ก้าวเท้าขวา ไปข้างหน้า
เราจะลองเขียนโค้ดง่ายๆ เพื่อให้ หุ่นยนต์ฮิวแมนนอยด์ 12 DOF หัดเดิน
#include <Servo.h>
//Creating Servo Objects
Servo rightAnkle; // เท้าขวา
Servo leftAnkle; // เท้าซ้าย
Servo rightKnee; // เข่าขวา
Servo leftKnee; // เข่าซ้าย
Servo rightThigh; // ต้นขาขวา
Servo leftThigh; // ต้นขาซ้าย
Servo rightHip; // สะโพกขวา
Servo leftHip; // สะโพกซ้าย
Servo leftShoulder; // ไหล่ซ้าย
Servo rightShoulder; // ไหล่ขวา
Servo leftElbow; // ข้อศอกซ้าย
Servo rightElbow; // ข้อศอกขวา
void setup() {
//Servo input pins
rightAnkle.attach(2); // เท้าขวา
rightKnee.attach(3); // เข่าขวา
rightThigh.attach(4); // ต้นขาขวา
rightHip.attach(5); // สะโพกขวา
leftAnkle.attach(6); // เท้าซ้าย
leftKnee.attach(7); // เข่าซ้าย
leftThigh.attach(8); // ต้นขาซ้าย
leftHip.attach(9); // สะโพกซ้าย
leftShoulder.attach(10); // ไหล่ซ้าย
rightShoulder.attach(11); // ไหล่ขวา
leftElbow.attach(12); // ข้อศอกซ้าย
rightElbow.attach(13); // ข้อศอกขวา
rightShoulder.write(90); // ไหล่ขวา
leftShoulder.write(90); // ไหล่ซ้าย
rightElbow.write(90); // ข้อศอกขวา
leftElbow.write(90); // ข้อศอกซ้าย
rightAnkle.write(100); // เท้าขวา
rightKnee.write(105); // เข่าขวา
rightThigh.write(110); // ต้นขาขวา
rightHip.write(90); // สะโพกขวา
leftAnkle.write(80); // เท้าซ้าย
leftKnee.write(90); // เข่าซ้าย
leftThigh.write(80); // ต้นขาซ้าย
leftHip.write(92); // สะโพกซ้าย
rightAnkle.write(100); // เท้าขวา
delay(3000);
}
void loop()
{
right_foot ();
stand ();
left_foot ();
stand ();
}
//ปรับค่าตามความเหมาะสม//
void stand ()
{
rightShoulder.write(90); // ไหล่ขวา
leftShoulder.write(90); // ไหล่ซ้าย
rightElbow.write(90); // ข้อศอกขวา
leftElbow.write(90); // ข้อศอกซ้าย
rightAnkle.write(100); // เท้าขวา
rightKnee.write(105); // เข่าขวา
rightThigh.write(110); // ต้นขาขวา
rightHip.write(90); // สะโพกขวา
leftAnkle.write(80); // เท้าซ้าย
leftKnee.write(90); // เข่าซ้าย
leftThigh.write(80); // ต้นขาซ้าย
leftHip.write(92); // สะโพกซ้าย
rightAnkle.write(100); // เท้าขวา
}
void right_foot ()
{
rightAnkle.write(60); // บิดเท้าขวาเพื่อส่งเท้าซ้าย
leftAnkle.write(60); // บิดเท้าซ้ายเพื่อยืน
rightKnee.write(80); // ปรับเข่าขวา
delay(1000);
rightAnkle.write(100); // เท้าขวาคืนปรกติ
rightThigh.write(130); // ต้นขาขวาเครื่อนไปข้างหน้า
delay(1000);
leftAnkle.write(80); // เท้าซ้ายปรกติ
delay(1000);
rightThigh.write(100); // ต้นขาขวาปรกติ
rightKnee.write(100); // เข่าขวาปรกติ
}
void left_foot ()
{
leftAnkle.write(100); // บิดเท้าซ้ายเพื่อส่งเท้าขวา
rightAnkle.write(125); // บิดเท้าขวาเพื่อยืน
leftKnee.write(120); // เข่าซ้าย
delay(1000);
leftAnkle.write(70); // เท้าซ้ายคืนปรกติ
leftThigh.write(50); // ต้นขาซ้ายเครื่อนไปข้างหน้า
delay(1000);
rightAnkle.write(100); // เท้าขวาคืนปรกติ
delay(1000);
leftThigh.write(90); // ต้นขาซ้ายปรกติ
leftKnee.write(90); // เข่าซ้ายปรกติ
}
วีดีโอผลลัพธ์การทำงานของ หุ่นยนต์ฮิวแมนนอยด์ 12 DOF หัดเดิน
โปรเจค หุ่นยนต์ฮิวแมนนอยด์ 12 DOF อุปกรณ์ที่ต้องใช้ก็คือ
1. Large U Bracket จำนวน 1 ชิ้น
2. Servo Bracket จำนวน 12 ชิ้น
3. Long U Bracket จำนวน 3 ชิ้น
4. Short U Bracket จำนวน 7 ชิ้น
5. L Bracket จำนวน 4 ชิ้น
6. Tilt U Bracket จำนวน 2 ชิ้น
7. Big Feet จำนวน 2 ชิ้น
8. Disc 25T Metal จำนวน 12 ชิ้น
9. Metal Cup Bearing จำนวน 10 ชิ้น
10. สกรูหัวกลม + น็อตตัวเมีย ขนาด 3 มม. ยาว 10 มม. จำนวน 124 ชิ้น
11. Arduino UNO R3 - Made in italy จำนวน 1 ชิ้น
12. Sensor Shield V5.0 จำนวน 1 ชิ้น
13. เซอร์โวมอเตอร์ MG996R จำนวน 12 ชิ้น
14. Jumper (F2M) cable wire 40pcs 10 cm 2.54mm Female to Male
15. รางถ่านแบบ 18650 ใส่ถ่าน 2 ก้อน
16. แบตเตอรี่ลิเธียม 18650 จำนวน 2 ก้อน
2. Servo Bracket จำนวน 12 ชิ้น
3. Long U Bracket จำนวน 3 ชิ้น
4. Short U Bracket จำนวน 7 ชิ้น
5. L Bracket จำนวน 4 ชิ้น
6. Tilt U Bracket จำนวน 2 ชิ้น
7. Big Feet จำนวน 2 ชิ้น
8. Disc 25T Metal จำนวน 12 ชิ้น
9. Metal Cup Bearing จำนวน 10 ชิ้น
10. สกรูหัวกลม + น็อตตัวเมีย ขนาด 3 มม. ยาว 10 มม. จำนวน 124 ชิ้น
11. Arduino UNO R3 - Made in italy จำนวน 1 ชิ้น
12. Sensor Shield V5.0 จำนวน 1 ชิ้น
13. เซอร์โวมอเตอร์ MG996R จำนวน 12 ชิ้น
14. Jumper (F2M) cable wire 40pcs 10 cm 2.54mm Female to Male
15. รางถ่านแบบ 18650 ใส่ถ่าน 2 ก้อน
16. แบตเตอรี่ลิเธียม 18650 จำนวน 2 ก้อน
ขั้นตอนการประกอบ โปรเจค หุ่นยนต์ฮิวแมนนอยด์ 12 DOF เพื่อการศึกษา ส่วนล่างจะคล้ายๆกับ โปรเจค หุ่นยนต์ฮิวแมนนอยด์ 8 DOF เพื่อการศึกษา
เพียงแต่เปลี่ยน Large U Bracket (ด้านบนสุด) จากแนวตั้ง
เป็นแนวนอน โดยส่วนอื่นๆจะประกอบเหมือนกัน
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiHx7qzZlKiGqtn6AiIkki6HS7kKm_TXDQFXX7ZjKmlvKhvFBzRPv55uKH4TjupG5Jy6HEQj3wT0NzYKUR1yt0qqTpLbBk7Y55kjnF2z_TBVnnEbjGkaZUQZDBfRp2T063HQ_K4ZJEJodaz/s640/1.jpg)
ยึด L Bracket 2 ชิ้น ไปบน Large U Bracket ดังรูป
ยึด Servo Bracket 2 ชิ้น เข้ากับ L Bracket
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhVHQqNnKB6vDldpv7Lgm3kJ5Nz_aU1j1NKwZGKfAiAPQAu_A6s3yEPKbXrFuuAYzkOlOHpLtJdhoWR8dsrE2vAH2Yjw2m_6AGEP48VNpZgLBwEby5B3WKdN4WQdFfvObtYsmxeQ5764sUS/s640/3.jpg)
ยึด เซอร์โวมอเตอร์ MG996R และ Disc Metal 2 ชิ้น เข้ากับ Servo Bracket
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhzgVZKn324Qfy8MtWtkvmKP0C3K7LOFKFQqU7zRRZ1vb-FUWlkQKNe95ADABZmczsBvqfYsL_Gv9ssmNkym_HU6zwwC5RcFM2i1AjZchIpUhgv34ofSoWOmkAOT8p59k-dYhqbkIUw3TFV/s640/4.jpg)
ยึด Tilt U Bracket (ยูเอียง) 2 ชิ้น เข้ากับ Disc Metal ของ เซอร์โวมอเตอร์ MG996R เพื่อเป็น ไหล่ของหุ่นยนต์
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEg0ketX1m_0cPxS_iOpUUloFumjBieC46SkhUTMA2sQOD4CfAqqJ8HwwrJn4hrXXqq39mJSnZlVlNXZm3fsQ-VlCcF_1QahD4j-TJJHOznapBC1TZH9KdbCDyZEvAzBIHQT6MJfB_mwywXg/s640/5.jpg)
ยึด Servo Bracket 2 ชิ้น เข้ากับ Tilt U Bracket
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgyYk5uLdQ_FdRJHheX-7F6yIKSMa6TVSMhUaP-ZhuJIQBVtUwMBNBJe-b37ypff7T7zgXQoRivOlcPyLmwScBsKHDFR8Dk8ZRWjwQAGr5z4_yFXyl2Pwg0F4NNL3l6d7ucx5iAMJpIb3ac/s640/6.jpg)
ประกอบ L Bracket 2 ชิ้น เข้ากับ Short U Bracket
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhZgiNMzrV0IRXjd8gWYfaCgGSWxzWis2PFGFRmEzv0wPUIvKAR6lN_z6_3Tixq16J-1bBWK_XNHAc1tBc0WDdN7DYyDWAEROS2p2ZAjVvUKf9Oz-0rDKEvf-BZhIoQg8FuJgHHh_O6gA-P/s640/7.jpg)
ยึด ชุด L Bracket ที่ประกอบอยู่กับ Short U Bracket เข้ากับ Servo Bracket ทั้ง 2 ด้าน
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgSieyrX0-KWFhuwQTEyUMlrII_oVfFcoqGKypeBfKJSTMoOmJxLERmCL3CICHDFVzvtjaOJFUuIWVNsk1mfBKtsuoggW3MNUO1QdeWt45cz870ISWUEu4O1eWmRxFiW6StYiJdlZHNaG40/s640/8.jpg)
ยึด เซอร์โวมอเตอร์ MG996R เข้าไปที่แขนของหุ่นยนต์ ทั้ง 2 ด้าน
ยึด Long U Bracket เข้ากับ Large U Bracket เพื่อเป็นลำตัวของหุ่นยนต์
และ ยึด Short U Bracket เข้าที่ด้านบนของ Long U Bracket เพื่อเป็นส่วนหัว ของหุ่นยนต์
ภาพรวม หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ด้านหน้าเมื่อประกอบเสร็จ
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiXvN0NPim_4JN62r5UdJXacz3Efz_TTaKe0ll89pUHxK8-TLKiP0L4UGzPqd39gOAJS5ywUF0rHf7P7U1ya6Ux4jUujHU40Sfa1mTBMH9MkbnJ-0rrZhdvi3yXy473bhlZIjud5WvU1xXT/s640/12.jpg)
ด้านหลัง ยึด แผ่นอะคริลิค ขนาด 9 x 11 เซ็นติเมตร จำนวน 1 แผ่น เข้าที่ด้านหลังของหุ่นยนต์
และ ประกอบ Arduino UNO R3 กับ Sensor Shield V5.0เข้า กับ แผ่นอะคริลิค
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiTGHoIurf9_VEjSdcGwFwaQ9C3kj5rj6bAdiEEoFDjedFY1bPFK1ibDE4qdkbtNJ28JpQvM-9O1oP38fIg8TynxoN224YADIDn_3_6_SPlq3RJYL4MtEiUH5JrnKl16OWi5FAinvOREuBc/s640/14.jpg)
ยึด รางถ่านแบบ 18650 (2 ก้อน) นำสายสีแดง ขั้วบวก ของรางถ่าน ต่อเข้ากับ VCC และ สายสีดำ ขั้วลบ ต่อเข้ากับ GND ของ บอร์ด Sensor Shield V5.0
และ ต่อสาย เซอร์โวมอเตอร์ MG996R ทุกตัว เข้าที่ บอร์ด Sensor Shield V5.0
//Servo input pins
rightAnkle.attach(2); // เท้าขวา
rightKnee.attach(3); // เข่าขวา
rightThigh.attach(4); // ต้นขาขวา
rightHip.attach(5); // สะโพกขวา
leftAnkle.attach(6); // เท้าซ้าย
leftKnee.attach(7); // เข่าซ้าย
leftThigh.attach(8); // ต้นขาซ้าย
leftHip.attach(9); // สะโพกซ้าย
leftShoulder.attach(10); // ไหล่ซ้าย
rightShoulder.attach(11); // ไหล่ขวา
leftElbow.attach(12); // ข้อศอกซ้าย
rightElbow.attach(13); // ข้อศอกขวา
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEh1UAnU7wJ6CeZx9PQB39atTkj3n-hFryCEzzXVq1wepKwU3ug6t0-BJYCOrcMSrf0QlaJGpZgtpSbt7NDbdXFKd6X3TB8KRsGn0UGWzO6qzNNAOum3-aXc9XjDmDnTbOKp4Vwel67UWpQj/s640/6034.jpg)
ภาพรวม หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ด้านหลัง เมื่อประกอบเสร็จ
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEj2qMf4wZJ5rJ38sgm6dXCnMASuPHELSAq3kLi6TnjKDaQVUxlyaAxACU3EZBJDw9NZOfNAtO3mjc7Fish7nu7-1t6Tky-FLdtCU7L4qmZiikTfVmkA0PH_Y_gzsgrNAz_gYQamlSfKKi0H/s640/6035.jpg)
ภาพรวม หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ด้านหน้า เมื่อประกอบเสร็จ
![](https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjg7YIlEoxj2QwB1-4h5lg6YoEJ0rjwK1EpPo64r5nRIg6mF1_5yb39qgWJmlvZE-1rKoeVjfr8lp4wMoPyxbMn_jVd0-TBn8oixEKHhfCKJUMqXp3VMjmFsVeq8aSTSAWCH9ccMwyme8FP/s640/1.jpg)
credit :
ลิงค์ youtube ต้นแบบ https://www.youtube.com/watch?v=eP7R-Hlo8pQ
ลิงค์โค้ดต้นแบบ https://www.dropbox.com/s/d2c3mxcu74s84qr/Biped_2.ino?dl=0
เปิดโปรแกรม Arduino (IDE) และ Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3
#include <Servo.h>
int delayVal2 = 25;
int delayVal = 40;
bool time = true;
//Creating Servo Objects
Servo rightAnkle; // เท้าขวา
Servo leftAnkle; // เท้าซ้าย
Servo rightKnee; // เข่าขวา
Servo leftKnee; // เข่าซ้าย
Servo rightThigh; // ต้นขาขวา
Servo leftThigh; // ต้นขาซ้าย
Servo rightHip; // สะโพกขวา
Servo leftHip; // สะโพกซ้าย
Servo leftShoulder; // ไหล่ซ้าย
Servo rightShoulder; // ไหล่ขวา
Servo leftElbow; // ข้อศอกซ้าย
Servo rightElbow; // ข้อศอกขวา
void setup() {
//Servo input pins
rightAnkle.attach(2); // เท้าขวา
rightKnee.attach(3); // เข่าขวา
rightThigh.attach(4); // ต้นขาขวา
rightHip.attach(5); // สะโพกขวา
leftAnkle.attach(6); // เท้าซ้าย
leftKnee.attach(7); // เข่าซ้าย
leftThigh.attach(8); // ต้นขาซ้าย
leftHip.attach(9); // สะโพกซ้าย
leftShoulder.attach(10); // ไหล่ซ้าย
rightShoulder.attach(11); // ไหล่ขวา
leftElbow.attach(12); // ข้อศอกซ้าย
rightElbow.attach(13); // ข้อศอกขวา
delay(2000);
stand();
}
void loop()
{
stand();
turnLeft();
//forward();
}
int rightAnkPos = 90;
int rightKneePos = 90;
int rightThighPos = 90;
int rightHipPos = 90;
int leftAnkPos = 90;
int leftKneePos = 90;
int leftThighPos = 90;
int leftHipPos = 90;
//ปรับค่าตามความเหมาะสม//
void stand ()
{
rightAnkle.write(100); // เท้าขวา
int rightAnkPos = 100;
rightKnee.write(100); // เข่าขวา
int rightKneePos = 100;
rightThigh.write(110); // ต้นขาขวา
int rightThighPos = 110;
rightHip.write(90); // สะโพกขวา
int rightHipPos = 90;
leftAnkle.write(80); // เท้าซ้าย
int leftAnkPos = 90;
leftKnee.write(90); // เข่าซ้าย
int leftKneePos = 90;
leftThigh.write(80); // ต้นขาซ้าย
int leftThighPos = 80;
leftHip.write(92); // สะโพกซ้าย
int leftHipPos = 92;
rightShoulder.write(90); // ไหล่ขวา
leftShoulder.write(90); // ไหล่ซ้าย
rightElbow.write(90); // ข้อศอกขวา
leftElbow.write(90); // ข้อศอกซ้าย
}
int rightShoulderPos = 180;
int rightElbowPos = 90;
int leftShoulderPos = 180;
int leftElbowPos = 90;
void handDemo()
{
stand();
delay(1000);
handsHalf();
delay(250);
handsRight();
delay(250);
handsLeft();
delay(250);
handsRight();
delay(250);
handsHalfdown();
delay(250);
rightHandsUp();
delay(250);
rightHandsDown();
delay(250);
leftHandsUp();
delay(250);
leftHandsDown();
delay(250);
handWave();
}
void handsUp()
{
for (int i = 0; i < 18; i += 1)
{
rightShoulderPos -= 9;
leftShoulderPos -= 10;
leftShoulder.write(leftShoulderPos);
rightShoulder.write(rightShoulderPos );
delay(50);
}
}
void leftHandsUp()
{
for (int i = 0; i < 18; i += 1)
{
leftShoulderPos -= 10;
leftShoulder.write(leftShoulderPos);
delay(50);
}
}
void leftHandsDown()
{
for (int i = 0; i < 18; i += 1)
{
leftShoulderPos += 10;
leftShoulder.write(leftShoulderPos);
delay(50);
}
}
void rightHandsUp()
{
for (int i = 0; i < 18; i += 1)
{
rightShoulderPos -= 9;
rightShoulder.write(rightShoulderPos );
delay(50);
}
}
void rightHandsDown()
{
for (int i = 0; i < 18; i += 1)
{
rightShoulderPos += 9;
rightShoulder.write(rightShoulderPos );
delay(50);
}
}
void handsHalf()
{
for (int i = 0; i < 9; i += 1)
{
rightShoulderPos -= 9;
leftShoulderPos -= 10;
leftShoulder.write(leftShoulderPos);
rightShoulder.write(rightShoulderPos );
delay(50);
}
}
void handsHalfdown()
{
for (int i = 0; i < 9; i += 1)
{
rightShoulderPos += 9;
leftShoulderPos += 10;
leftShoulder.write(leftShoulderPos);
rightShoulder.write(rightShoulderPos );
delay(50);
}
}
void handsDown()
{
for (int i = 0; i < 18; i += 1)
{
rightShoulderPos += 9;
leftShoulderPos += 10;
leftShoulder.write(leftShoulderPos);
rightShoulder.write(rightShoulderPos );
delay(25);
}
}
void handWave()
{
leftThigh.write(75);
rightThigh.write(90);
handsUp();
for (int i = 0; i < 3; i++)
{
handsRight();
handsLeft();
handsRight();
}
delay(250);
handsDown();
}
void handsRight()
{
for (int i = 0; i < 9; i += 1)
{
rightElbowPos += 7;
leftElbowPos += 7;
leftElbow.write(leftElbowPos);
rightElbow.write(rightElbowPos );
delay(25);
}
}
void handsLeft()
{
for (int i = 0; i < 18; i += 1)
{
rightElbowPos -= 7;
leftElbowPos -= 7;
leftElbow.write(leftElbowPos);
rightElbow.write(rightElbowPos );
delay(25);
}
}
void hello()
{
leftHandsUp();
for (int i = 0; i < 3; i += 1)
{
for (int i = 0; i < 9; i += 1)
{
leftElbowPos += 7;
leftElbow.write(leftElbowPos);
delay(25);
}
for (int i = 0; i < 9; i += 1)
{
leftElbowPos -= 7;
leftElbow.write(leftElbowPos);
delay(25);
}
for (int i = 0; i < 9; i += 1)
{
leftElbowPos -= 7;
leftElbow.write(leftElbowPos);
delay(25);
}
for (int i = 0; i < 9; i += 1)
{
leftElbowPos += 7;
leftElbow.write(leftElbowPos);
delay(25);
}
}
delay(500);
leftHandsDown();
}
///////////////Legs//////////////////////
void legs()
{
for (int i = 0; i < 6; i += 1)
{
leftAnkPos += 2;
rightAnkPos -= 2;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(delayVal2);
}
delay(10);
for (int i = 0; i < 6; i += 1)
{
leftAnkPos -= 2;
rightAnkPos += 2;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(delayVal2);
}
delay(10);
for (int i = 0; i < 6 ; i += 1)
{
leftAnkPos -= 2;
rightAnkPos += 2;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(delayVal2);
}
delay(10);
for (int i = 0; i < 6; i += 1)
{
leftAnkPos += 2;
rightAnkPos -= 2;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(delayVal2);
}
delay(10);
}
////////////FORWARD///////////////////////////////////////////////////////
void forward()
{
for (int i = 0; i < 5; i += 1)
{
if (time) {
leftThighPos -= 2;
leftKneePos += 2;
leftThigh.write( leftThighPos);
leftKnee.write(leftKneePos );
delay(delayVal2);
}
}
//Left and Right Ankle Bend Left
for (int i = 0; i < 6; i += 1)
{
leftAnkPos += 2;
rightAnkPos += 2;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(delayVal2);
}
delay(delayVal);
//Straighten Left Leg
for (int i = 0; i < 5; i += 1)
{
leftThighPos += 2;
leftKneePos -= 2;
leftThigh.write( leftThighPos);
leftKnee.write(leftKneePos );
delay(delayVal2);
}
//right leg forward
for (int i = 0; i < 5; i += 1)
{
rightThighPos += 2;
rightKneePos -= 2;
rightThigh.write( rightThighPos);
rightKnee.write(rightKneePos );
delay(delayVal2);
}
//Right and Left Ankle back to Normal
for (int i = 0; i < 6; i += 1)
{
leftAnkPos -= 2;
rightAnkPos -= 2;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(delayVal2);
}
//Right and left Ankle Bend right
for (int i = 0; i < 6 ; i += 1)
{
leftAnkPos -= 2;
rightAnkPos -= 2;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(delayVal2);
}
delay(delayVal);
//Straighten Right Leg
for (int i = 0; i < 5; i += 1)
{
rightThighPos -= 2;
rightKneePos += 2;
rightThigh.write( rightThighPos);
rightKnee.write(rightKneePos );
delay(delayVal2);
}
//Left Leg bend Forward
for (int i = 0; i < 5; i += 1)
{
leftThighPos -= 2;
leftKneePos += 2;
leftThigh.write( leftThighPos);
leftKnee.write(leftKneePos );
delay(delayVal2);
}
//Ankles back to normal
for (int i = 0; i < 6; i += 1)
{
leftAnkPos += 2;
rightAnkPos += 2;
rightAnkle.write(rightAnkPos);
leftAnkle.write(leftAnkPos);
delay(delayVal2);
}
time = false;
/*
*/
}
///////////////LEFT/////////////////////////////////////////
void turnLeft()
{
for (int i = 0; i < 5; i += 1)
{
leftThighPos -= 2;
leftKneePos += 2;
leftThigh.write( leftThighPos);
leftKnee.write(leftKneePos );
rightShoulder.write(90); // ไหล่ขวา
leftShoulder.write(180); // ไหล่ซ้าย
delay(60);
}
//Left and Right Ankle Bend Left
for (int i = 0; i < 5; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
rightShoulder.write(180); // ไหล่ขวา
leftShoulder.write(90); // ไหล่ซ้าย
delay(60);
}
delay(delayVal);
//Left Hip Counter ClockWise,
for (int i = 0; i < 5; i += 1)
{
leftHipPos -= 4;
leftHip.write(leftHipPos);
delay(60);
}
delay(delayVal);
//Left and Right Ankle back to normal
for (int i = 0; i < 5; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
//Left and Right Ankle Bend Right Left Hip Straighten
for (int i = 0; i < 5; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
leftHipPos += 4;
leftHip.write(leftHipPos);
delay(60);
}
delay(delayVal);
//Right Hip Counter Clockwise
for (int i = 0; i < 5; i += 1)
{
rightShoulder.write(90); // ไหล่ขวา
rightHipPos -= 4;
rightHip.write(rightHipPos);
leftHipPos += 4;
leftHip.write(leftHipPos);
delay(60);
}
delay(delayVal);
//Left and Right Ankle Normal
for (int i = 0; i < 5; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
delay(delayVal);
///////SECOND TIME///////////////
//Left and Right Ankle BEND LEFT
for (int i = 0; i < 5; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
leftThighPos += 2;
leftKneePos -= 2;
leftThigh.write( leftThighPos);
leftKnee.write(leftKneePos );
delay(60);
}
delay(delayVal);
//LEFT HIP CC
for (int i = 0; i < 5; i += 1)
{
leftHipPos -= 4;
leftHip.write(leftHipPos);
rightHipPos += 4;
rightHip.write(rightHipPos);
rightShoulder.write(90); // ไหล่ขวา
leftShoulder.write(180); // ไหล่ซ้าย
delay(60);
}
delay(delayVal);
//LEFT RIGHT ANKLE NORMAL
for (int i = 0; i < 5; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
//Left and Right Ankle Bend Right
for (int i = 0; i < 5; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
delay(delayVal);
//Right Hip Counter Clockwise
for (int i = 0; i < 5; i += 1)
{
rightHipPos -= 5;
rightHip.write(rightHipPos);
leftHipPos += 2;
leftHip.write(leftHipPos);
delay(60);
}
//LEFT RIGHT ANKLE NORMAL
for (int i = 0; i < 5; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
//LEFT RIGHT ANKLE BEND LEFT
for (int i = 0; i < 5; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
rightHipPos += 5;
rightHip.write(rightHipPos);
leftHipPos -= 2;
leftHip.write(leftHipPos);
delay(60);
}
//LEFT RIGHT ANKLE STRAIGHT
for (int i = 0; i < 5; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
}
/////////////////TURN RIGHT///////////////////////////////////////////////
void turnRight ()
{
for (int i = 0; i < 5; i += 1)
{
rightThighPos += 1;
rightKneePos -= 1;
rightThigh.write( rightThighPos);
rightKnee.write(rightKneePos );
delay(60);
}
//Left and Right Ankle Bend Right
for (int i = 0; i < 5; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
delay(delayVal);
//right Hip ClockWise, Left leg forward
for (int i = 0; i < 5; i += 1)
{
rightHipPos += 4;
rightHip.write(rightHipPos);
delay(60);
}
delay(delayVal);
//Left and Right Ankle back to normal
for (int i = 0; i < 5; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
//Left and Right Ankle Bend Leftt right Hip Straighten
for (int i = 0; i < 5; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
rightHipPos -= 4;
rightHip.write(rightHipPos);
delay(60);
}
delay(delayVal);
//Left Hip Counter Clockwise
for (int i = 0; i < 5; i += 1)
{
rightHipPos -= 4;
rightHip.write(rightHipPos);
leftHipPos += 4;
leftHip.write(leftHipPos);
delay(60);
}
delay(delayVal);
//Left and Right Ankle Normal
for (int i = 0; i < 5; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
delay(delayVal);
//Second Time
//Left and Right Ankle BEND RIGHT, Right Thigh/Knee Straighten
for (int i = 0; i < 6; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
delay(delayVal);
//Right HIP CW, Left Hip Straighten
for (int i = 0; i < 5; i += 1)
{
rightHipPos += 4;
leftHipPos -= 4;
leftHip.write(leftHipPos);
rightHip.write(rightHipPos);
delay(60);
}
delay(delayVal);
//LEFT RIGHT ANKLE NORMAL
for (int i = 0; i < 6; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
////////Third Time //////////
//Left and Right Ankle Bend Left
for (int i = 0; i < 6; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
delay(delayVal);
//Left Right Hip CW
for (int i = 0; i < 5; i += 1)
{
rightHipPos -= 5;
rightHip.write(rightHipPos);
leftHipPos += 2;
leftHip.write(leftHipPos);
rightThighPos -= 1;
rightKneePos += 1;
rightThigh.write( rightThighPos);
rightKnee.write(rightKneePos );
delay(60);
}
//LEFT RIGHT ANKLE NORMAL
for (int i = 0; i < 6; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
//LEFT RIGHT ANKLE BEND Right
for (int i = 0; i < 6; i += 1)
{
leftAnkPos -= 3;
rightAnkPos -= 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
//Right Hip Normal
for (int i = 0; i < 5; i += 1)
{
rightHipPos += 5;
rightHip.write(rightHipPos);
delay(60);
}
//Left Hip Normal
for (int i = 0; i < 5; i += 1)
{
leftHipPos -= 2;
leftHip.write(leftHipPos);
delay(60);
}
//LEFT RIGHT ANKLE STRAIGHT
for (int i = 0; i < 6; i += 1)
{
leftAnkPos += 3;
rightAnkPos += 3;
leftAnkle.write(leftAnkPos);
rightAnkle.write(rightAnkPos );
delay(60);
}
/*
*/
}
ใส่ถ่าน 18650 จำนวน 2 ก้อน แล้วลองทดสอบ
วีดีโอผลลัพธ์การทำงานของ โปรเจค หุ่นยนต์ฮิวแมนนอยด์ 12 DOF เพื่อการศึกษา
และเพื่อเพิ่มประสิทธิภาพในการจ่ายไฟเลี้ยง เซอร์โวมอเตอร์ MG996R ต้องเพิ่ม Jack 5.5 X 2.1mm สำหรับ เพาเวอร์ซัพพลาย ของ Arduino ตัวผู้ ต่อจากรางถ่าน ขั้วบวก + สีแดง และ ขั้วลบ- สีดำ ของ แบตเตอรี่ 18650 เข้าไปยัง Jack ซัพพลายตัวเมีย ของ Arduino UNO R3 ดังรูปด้วย
เราจะลองเขียนโค้ดง่ายๆ เพื่อให้ หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ก้าวเท้าขวา ไปข้างหน้า
เปิดโปรแกรม Arduino (IDE) และ Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3
#include <Servo.h>
//Creating Servo Objects
Servo rightAnkle; // เท้าขวา
Servo leftAnkle; // เท้าซ้าย
Servo rightKnee; // เข่าขวา
Servo leftKnee; // เข่าซ้าย
Servo rightThigh; // ต้นขาขวา
Servo leftThigh; // ต้นขาซ้าย
Servo rightHip; // สะโพกขวา
Servo leftHip; // สะโพกซ้าย
Servo leftShoulder; // ไหล่ซ้าย
Servo rightShoulder; // ไหล่ขวา
Servo leftElbow; // ข้อศอกซ้าย
Servo rightElbow; // ข้อศอกขวา
void setup() {
//Servo input pins
rightAnkle.attach(2); // เท้าขวา
rightKnee.attach(3); // เข่าขวา
rightThigh.attach(4); // ต้นขาขวา
rightHip.attach(5); // สะโพกขวา
leftAnkle.attach(6); // เท้าซ้าย
leftKnee.attach(7); // เข่าซ้าย
leftThigh.attach(8); // ต้นขาซ้าย
leftHip.attach(9); // สะโพกซ้าย
leftShoulder.attach(10); // ไหล่ซ้าย
rightShoulder.attach(11); // ไหล่ขวา
leftElbow.attach(12); // ข้อศอกซ้าย
rightElbow.attach(13); // ข้อศอกขวา
rightShoulder.write(90); // ไหล่ขวา
leftShoulder.write(90); // ไหล่ซ้าย
rightElbow.write(90); // ข้อศอกขวา
leftElbow.write(90); // ข้อศอกซ้าย
rightAnkle.write(100); // เท้าขวา
rightKnee.write(100); // เข่าขวา
rightThigh.write(100); // ต้นขาขวา
rightHip.write(90); // สะโพกขวา
leftAnkle.write(80); // เท้าซ้าย
leftKnee.write(90); // เข่าซ้าย
leftThigh.write(90); // ต้นขาซ้าย
leftHip.write(92); // สะโพกซ้าย
rightAnkle.write(100); // เท้าขวา
delay(3000);
}
void loop()
{
right_foot();
stand ();
}
//ปรับค่าตามความเหมาะสม//
void stand ()
{
rightShoulder.write(90); // ไหล่ขวา
leftShoulder.write(90); // ไหล่ซ้าย
rightElbow.write(90); // ข้อศอกขวา
leftElbow.write(90); // ข้อศอกซ้าย
rightAnkle.write(100); // เท้าขวา
rightKnee.write(100); // เข่าขวา
rightThigh.write(100); // ต้นขาขวา
rightHip.write(90); // สะโพกขวา
leftAnkle.write(80); // เท้าซ้าย
leftKnee.write(90); // เข่าซ้าย
leftThigh.write(90); // ต้นขาซ้าย
leftHip.write(92); // สะโพกซ้าย
rightAnkle.write(100); // เท้าขวา
}
void right_foot ()
{
rightAnkle.write(65); // บิดเท้าขวาเพื่อส่งเท้าซ้าย
leftAnkle.write(65); // บิดเท้าซ้ายเพื่อยืน
rightKnee.write(80); // ปรับเข่าขวา
delay(1000);
rightAnkle.write(100); // เท้าขวาคืนปรกติ
rightThigh.write(130); // ต้นขาขวาเครื่อนไปข้างหน้า
delay(1000);
leftAnkle.write(80); // เท้าซ้ายปรกติ
leftKnee.write(80); // ปรับเข่าซ้าย
delay(1000);
rightThigh.write(100); // ต้นขาขวาปรกติ
rightKnee.write(100); // เข่าขวาปรกติ
}
วีดีโอผลลัพธ์การทำงานของ หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ก้าวเท้าขวา ไปข้างหน้า
เราจะลองเขียนโค้ดง่ายๆ เพื่อให้ หุ่นยนต์ฮิวแมนนอยด์ 12 DOF หัดเดิน
#include <Servo.h>
//Creating Servo Objects
Servo rightAnkle; // เท้าขวา
Servo leftAnkle; // เท้าซ้าย
Servo rightKnee; // เข่าขวา
Servo leftKnee; // เข่าซ้าย
Servo rightThigh; // ต้นขาขวา
Servo leftThigh; // ต้นขาซ้าย
Servo rightHip; // สะโพกขวา
Servo leftHip; // สะโพกซ้าย
Servo leftShoulder; // ไหล่ซ้าย
Servo rightShoulder; // ไหล่ขวา
Servo leftElbow; // ข้อศอกซ้าย
Servo rightElbow; // ข้อศอกขวา
void setup() {
//Servo input pins
rightAnkle.attach(2); // เท้าขวา
rightKnee.attach(3); // เข่าขวา
rightThigh.attach(4); // ต้นขาขวา
rightHip.attach(5); // สะโพกขวา
leftAnkle.attach(6); // เท้าซ้าย
leftKnee.attach(7); // เข่าซ้าย
leftThigh.attach(8); // ต้นขาซ้าย
leftHip.attach(9); // สะโพกซ้าย
leftShoulder.attach(10); // ไหล่ซ้าย
rightShoulder.attach(11); // ไหล่ขวา
leftElbow.attach(12); // ข้อศอกซ้าย
rightElbow.attach(13); // ข้อศอกขวา
rightShoulder.write(90); // ไหล่ขวา
leftShoulder.write(90); // ไหล่ซ้าย
rightElbow.write(90); // ข้อศอกขวา
leftElbow.write(90); // ข้อศอกซ้าย
rightAnkle.write(100); // เท้าขวา
rightKnee.write(105); // เข่าขวา
rightThigh.write(110); // ต้นขาขวา
rightHip.write(90); // สะโพกขวา
leftAnkle.write(80); // เท้าซ้าย
leftKnee.write(90); // เข่าซ้าย
leftThigh.write(80); // ต้นขาซ้าย
leftHip.write(92); // สะโพกซ้าย
rightAnkle.write(100); // เท้าขวา
delay(3000);
}
void loop()
{
right_foot ();
stand ();
left_foot ();
stand ();
}
//ปรับค่าตามความเหมาะสม//
void stand ()
{
rightShoulder.write(90); // ไหล่ขวา
leftShoulder.write(90); // ไหล่ซ้าย
rightElbow.write(90); // ข้อศอกขวา
leftElbow.write(90); // ข้อศอกซ้าย
rightAnkle.write(100); // เท้าขวา
rightKnee.write(105); // เข่าขวา
rightThigh.write(110); // ต้นขาขวา
rightHip.write(90); // สะโพกขวา
leftAnkle.write(80); // เท้าซ้าย
leftKnee.write(90); // เข่าซ้าย
leftThigh.write(80); // ต้นขาซ้าย
leftHip.write(92); // สะโพกซ้าย
rightAnkle.write(100); // เท้าขวา
}
void right_foot ()
{
rightAnkle.write(60); // บิดเท้าขวาเพื่อส่งเท้าซ้าย
leftAnkle.write(60); // บิดเท้าซ้ายเพื่อยืน
rightKnee.write(80); // ปรับเข่าขวา
delay(1000);
rightAnkle.write(100); // เท้าขวาคืนปรกติ
rightThigh.write(130); // ต้นขาขวาเครื่อนไปข้างหน้า
delay(1000);
leftAnkle.write(80); // เท้าซ้ายปรกติ
delay(1000);
rightThigh.write(100); // ต้นขาขวาปรกติ
rightKnee.write(100); // เข่าขวาปรกติ
}
void left_foot ()
{
leftAnkle.write(100); // บิดเท้าซ้ายเพื่อส่งเท้าขวา
rightAnkle.write(125); // บิดเท้าขวาเพื่อยืน
leftKnee.write(120); // เข่าซ้าย
delay(1000);
leftAnkle.write(70); // เท้าซ้ายคืนปรกติ
leftThigh.write(50); // ต้นขาซ้ายเครื่อนไปข้างหน้า
delay(1000);
rightAnkle.write(100); // เท้าขวาคืนปรกติ
delay(1000);
leftThigh.write(90); // ต้นขาซ้ายปรกติ
leftKnee.write(90); // เข่าซ้ายปรกติ
}
วีดีโอผลลัพธ์การทำงานของ หุ่นยนต์ฮิวแมนนอยด์ 12 DOF หัดเดิน
ความคิดเห็น
แสดงความคิดเห็น