12 DOF Biped Robotic Educational Robot Humanoid Robot Servo Bracket
โปรเจค หุ่นยนต์ฮิวแมนนอยด์ 12 DOF อุปกรณ์ที่ต้องใช้ก็คือ
ขั้นตอนการประกอบ โปรเจค หุ่นยนต์ฮิวแมนนอยด์ 12 DOF เพื่อการศึกษา ส่วนล่างจะคล้ายๆกับ โปรเจค หุ่นยนต์ฮิวแมนนอยด์ 8 DOF เพื่อการศึกษา
เพียงแต่เปลี่ยน Large U Bracket (ด้านบนสุด) จากแนวตั้ง
ยึด L Bracket 2 ชิ้น ไปบน Large U Bracket ดังรูป
ยึด Servo Bracket 2 ชิ้น เข้ากับ L Bracket
ยึด เซอร์โวมอเตอร์ MG996R และ Disc Metal 2 ชิ้น เข้ากับ Servo Bracket
ยึด Tilt U Bracket (ยูเอียง) 2 ชิ้น เข้ากับ Disc Metal ของ เซอร์โวมอเตอร์ MG996R เพื่อเป็น ไหล่ของหุ่นยนต์
ยึด Servo Bracket 2 ชิ้น เข้ากับ Tilt U Bracket
ประกอบ L Bracket 2 ชิ้น เข้ากับ Short U Bracket
ยึด ชุด L Bracket ที่ประกอบอยู่กับ Short U Bracket เข้ากับ Servo Bracket ทั้ง 2 ด้าน
ยึด เซอร์โวมอเตอร์ MG996R เข้าไปที่แขนของหุ่นยนต์ ทั้ง 2 ด้าน
ยึด Long U Bracket เข้ากับ Large U Bracket เพื่อเป็นลำตัวของหุ่นยนต์
และ ยึด Short U Bracket เข้าที่ด้านบนของ Long U Bracket เพื่อเป็นส่วนหัว ของหุ่นยนต์
ภาพรวม หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ด้านหน้าเมื่อประกอบเสร็จ
ด้านหลัง ยึด แผ่นอะคริลิค ขนาด 9 x 11 เซ็นติเมตร จำนวน 1 แผ่น เข้าที่ด้านหลังของหุ่นยนต์
และ ประกอบ Arduino UNO R3 กับ Sensor Shield V5.0เข้า กับ แผ่นอะคริลิค
ยึด รางถ่านแบบ 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); // ข้อศอกขวา
ภาพรวม หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ด้านหลัง เมื่อประกอบเสร็จ
ภาพรวม หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ด้านหน้า เมื่อประกอบเสร็จ
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 (ด้านบนสุด) จากแนวตั้ง
เป็นแนวนอน โดยส่วนอื่นๆจะประกอบเหมือนกัน
ยึด L Bracket 2 ชิ้น ไปบน Large U Bracket ดังรูป
ยึด Servo Bracket 2 ชิ้น เข้ากับ L Bracket
ยึด เซอร์โวมอเตอร์ MG996R และ Disc Metal 2 ชิ้น เข้ากับ Servo Bracket
ยึด Tilt U Bracket (ยูเอียง) 2 ชิ้น เข้ากับ Disc Metal ของ เซอร์โวมอเตอร์ MG996R เพื่อเป็น ไหล่ของหุ่นยนต์
ยึด Servo Bracket 2 ชิ้น เข้ากับ Tilt U Bracket
ประกอบ L Bracket 2 ชิ้น เข้ากับ Short U Bracket
ยึด ชุด L Bracket ที่ประกอบอยู่กับ Short U Bracket เข้ากับ Servo Bracket ทั้ง 2 ด้าน
ยึด เซอร์โวมอเตอร์ MG996R เข้าไปที่แขนของหุ่นยนต์ ทั้ง 2 ด้าน
ยึด Long U Bracket เข้ากับ Large U Bracket เพื่อเป็นลำตัวของหุ่นยนต์
และ ยึด Short U Bracket เข้าที่ด้านบนของ Long U Bracket เพื่อเป็นส่วนหัว ของหุ่นยนต์
ภาพรวม หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ด้านหน้าเมื่อประกอบเสร็จ
ด้านหลัง ยึด แผ่นอะคริลิค ขนาด 9 x 11 เซ็นติเมตร จำนวน 1 แผ่น เข้าที่ด้านหลังของหุ่นยนต์
และ ประกอบ Arduino UNO R3 กับ Sensor Shield V5.0เข้า กับ แผ่นอะคริลิค
ยึด รางถ่านแบบ 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); // ข้อศอกขวา
ภาพรวม หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ด้านหลัง เมื่อประกอบเสร็จ
ภาพรวม หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ด้านหน้า เมื่อประกอบเสร็จ
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 หัดเดิน