โปรเจค หุ่นยนต์ฮิวแมนนอยด์ Arduino 8 DOF เพื่อการศึกษา

โปรเจค หุ่นยนต์ฮิวแมนนอยด์ Arduino 8 DOF เพื่อการศึกษา

8 DOF Biped Robotic Educational Robot Humanoid Robot Servo Bracket

Humanoid Robot หรือ หุ่นยนต์ฮิวแมนนอยด์ คือ หุ่นยนต์เคลื่อนไหวคล้ายมนุษย์นั้นเอง โดยโครงสร้างของหุ่นยนต์จะถูกออกแบบมาจากร่างกายของมนุษย์ ซึ่งโดยทั่วไป จะมีส่วนประกอบของหัว ลำตัว 2 แขน 2 ขา และอื่นๆ ซึ่งหุ่นยนต์ฮิวแมนนอยด์บางตัวอาจจะมีใบหน้า ที่ประกอบไปด้วยตา และปากอีกด้วย เพื่อให้การออกแบบดูคล้ายมนุษย์มากขึ้น

เราจะเรียกข้อต่อจุดหมุนว่าเป็นองศาอิสระ (Degrees Of Freedom: DOF) หมายถึงมันสามารถที่จะเคลื่อนไหวได้อย่างอิสระภายใต้ระยะจุดหมุนที่หมุนได้ ถ้าเปรียบเทียบกับแขนมนุษย์ที่สามารถยกแขนให้เคลื่อนที่จากตำแหน่งไปสู่ ตำแหน่งหนึ่ง ซึ่งในโปรเจคนี้ เราจะใช้จุดหมุน 8 DOF  ด้วยกัน สำหรับการเคลื่อนไหว 2 ขา ของหุ่นยนต์ จากเอวลงไป เพื่อศึกษาจุดหมุน 8 DOF ของหุ่นยนต์

โปรเจค หุ่นยนต์ฮิวแมนนอยด์ 8 DOF อุปกรณ์ที่ต้องใช้ก็คือ

     1. Large U Bracket  จำนวน 1 ชิ้น

     2. Servo Bracket จำนวน 8 ชิ้น

     3. Long U Bracket จำนวน  2 ชิ้น

     

     4. Short U Bracket จำนวน  4 ชิ้น

      

     5. Big Feet จำนวน 2 ชิ้น

     6. Disc 25T Metal จำนวน 8 ชิ้น

     7. Metal Cup Bearing จำนวน 6 ชิ้น
   
     8. สกรูหัวกลม + น็อตตัวเมีย ขนาด 3 มม. ยาว 10 มม. จำนวน 72 ชิ้น

     9. Arduino UNO R3 - Made in italy จำนวน 1 ชิ้น

    10. Sensor Shield V5.0  จำนวน 1 ชิ้น

    11. เซอร์โวมอเตอร์ MG996R จำนวน 8 ชิ้น
  
    12. Jumper (F2M) cable wire 40pcs 10 cm 2.54mm Female to Male

     

    13. รางถ่านแบบ 18650 ใส่ถ่าน 2 ก้อน

    14. แบตเตอรี่ลิเธียม 18650 จำนวน 2 ก้อน

เริ่มต้นโดย นำ Large U Brackets ซึ่งเปรียบเสมือน ลำตัวส่วนเอว ของหุ่นยนต์ ประกอบเข้ากับ Servo Bracket ซึ่งเปรียบเสมือน ต้นขาขวา ของหุ่นยนต์ โดย สกรูหัวกลม + น็อตตัวเมีย ขนาด 3 มม. ยาว 10 มม.

ประกอบ เซอร์โวมอเตอร์ MG996R เข้ากับ  Servo Bracket ตัวที่ 1

ประกอบ บอร์ด Sensor Shield V5.0 เข้ากับ บอร์ด Arduino UNO R3

รายละเอียดเพิ่มเติม  : https://robotsiam.blogspot.com/2016/09/arduino-sensor-shield-v50.html

เราจะเซตค่าองศาของ เซอร์โวมอเตอร์ ก่อนที่จะประกอบขั้นตอนต่อไปทุกๆตัว  ซึ่งการตั้งองศา ขึ้นอยู่กับความเหมาะสมของหุ่นยนต์  การออกแบบ และ การเขียนโปรแกรมคอมพิวเตอร์ควบคุมซึ่งต้อง สัมพันธ์กัน

ซึ่งในที่นี้ องศาที่่เราจะตั้ง เป็น 90 องศา  ขั้นตอนคือ ต่อสาย เซอร์โวมอเตอร์ MG996R เข้ากับ บอร์ด Sensor Shield V5.0 สายสีส้มเข้าที่ ขา 5 สายสีแดง เข้าที่ ขา V และ สายสีน้ำตาล เข้าที่ ขา G

เปิดโปรแกรม Arduino (IDE) และ Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3

#include <Servo.h>

Servo myservo;  //ประกาศตัวแปร myservo เรียกใช้งานคำสั่งจาก Library

void setup()

{

  myservo.attach(5); //ให้ Digital Pin 5 เป็นขาของ Servo

  myservo.write(90);  //หมุน 90 องศา

  delay(1000);

  myservo.write(45); //หมุน 45 องศา

  delay(1000);

  myservo.write(90);  //หมุน 90 องศา

  delay(1000);

  myservo.write(135); //หมุน 135 องศา

  delay(1000);

  myservo.write(90);  //หมุน 90 องศา

  delay(1000);

}

void loop()

{

  myservo.write(90);  //หมุน 90 องศา

  delay(1000);

}

เมื่อได้องศาที่ต้องการแล้ว จึงประกอบ Disc Metal เข้ากับ เซอร์โวมอเตอร์ MG996R ตัวที่ 1 และนำสกรูสีดำ ที่มาพร้อม ถุงอุปกรณ์เสริม MG996R เข้าที่แกนกลาง

และ นำสกรู จำนวน 4 ตัว ที่มาพร้อมกับ Disc Metal

ยึด  Servo Bracket ตัวที่ 2 เข้ากับ Disc Metal ของ  เซอร์โวมอเตอร์ ตัวที่ 1

นำ Metal Cup ยึด Long U Bracket เข้า กับ Servo Bracket ตัวที่ 2 (ซึ่งจะเป็นข้อต่อจุดหมุน)

ประกอบ เซอร์โวมอเตอร์ MG996R ตัวที่ 2 และ Disc Metal ยึดเข้ากับ Long U Bracket

และเมื่อทดสอบการทำงาน เซอร์โวมอเตอร์ MG996R  กับ บอร์ด Sensor Shield V5.0 สายสีส้มเข้าที่ ขา 5 สายสีแดง เข้าที่ ขา V และ สายสีน้ำตาล เข้าที่ ขา G จะเห็นความเคลื่อนไหว และ กลับมาอยู่ตำแหน่งเดิม คือ 90 องศา

ยึด Servo Bracket ตัวที่ 3 เข้ากับ Long U Bracket

นำ Metal Cup ยึด Short U Bracket ตัวที่ 1 เข้า กับ Servo Bracket ตัวที่ 3

ประกอบ เซอร์โวมอเตอร์ MG996R ตัวที่ 3 และ Disc Metal ยึดเข้ากับ Short U Bracket ตัวที่ 1

ยึด Short U Bracket ตัวที่ 2 เข้ากับ Short U Bracket ตัวที่ 1

ประกอบเท้าของหุ่นยนต์

ยึด Servo Bracket ตัวที่ 4 เข้ากับ Big Feet

นำ Metal Cup ยึด เท้าของหุ่นยนต์ที่ประกอบแล้ว (Servo Bracket ตัวที่ 4) เข้ากับ Short U Bracket ตัวที่ 2 (ด้านล่าง)

ประกอบ เซอร์โวมอเตอร์ MG996R ตัวที่ 4 และ Disc Metal ยึดเข้ากับ Short U Bracket ตัวที่ 2 และ เมื่อยกตั้งขึ้นจะได้ดังรูปด้านล่าง

ประกอบขาด้านซ้ายของหุ่นยนต์ ขั้นตอนคล้ายกับการประกอบขาด้านขวา เพียงแต่ทิศทางของ เซอร์โวมอเตอร์ MG996R จะกลับด้านกัน

เมื่อประกอบสำเร็จ ยืนขึ้น จะได้ หุ่นยนต์ฮิวแมนนอยด์ 8 DOF ตามรูปด้านล่าง

ตัด แผ่นอะคริลิค ขนาด 9  x 11 เซ็นติเมตร จำนวน 2 แผ่น

ด้านหน้ายึด บอร์ด Sensor Shield V5.0 ทีประกอบกับ บอร์ด Arduino UNO R3

ด้านหลัง ยึด รางถ่านแบบ 18650 (2 ก้อน) นำสายสีแดง ขั้วบวก ของรางถ่าน ต่อเข้ากับ VCC และ สายสีดำ ขั้วลบ ต่อเข้ากับ GND ของ บอร์ด Sensor Shield V5.0

ต่อสาย เซอร์โวมอเตอร์ MG996R ทั้ง 8 ตัวเข้ากับ บอร์ด Sensor Shield V5.0 (G,V,S) ตั้งแต่ ขา 2 ถึง ขา 9 โดยตรวจสอบตำแหน่งการต่อให้ถูกต้อง

  //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);  // สะโพกซ้าย

เปิดโปรแกรม 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;  // สะโพกซ้าย


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);  // สะโพกซ้าย

  delay(2000);
  stand();


}

void loop()
{}

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(90);   // เข่าขวา
  int rightKneePos = 90;
  rightThigh.write(100);  // ต้นขาขวา
  int rightThighPos = 100;
  rightHip.write(90);   // สะโพกขวา
  int rightHipPos = 90;
  leftAnkle.write(90);  // เท้าซ้าย
  int leftAnkPos = 90;
  leftKnee.write(90);  // เข่าซ้าย
  int leftKneePos = 90;
  leftThigh.write(80);  // ต้นขาซ้าย
  int leftThighPos = 80;
  leftHip.write(92);  // สะโพกซ้าย
  int leftHipPos = 92;


}

เมื่อหุ่นยนต์ยืนแล้ว ตรวจการยืนของห่นยนต์ว่าเหมาะสมหรือไม่  ถ้าไม่แก้ไขค่าองศา ของเซอร์โวมอเตอร์ MG996R ที่ void stand () 

แล้ว Upload ใหม่ (ทดสอบจนกว่าจะสมบูรณ์)

แก้ไข void stand ()  ของโค้ดนี้ตามที่ได้ค่าจากโคดด้านบน แล้วจึง Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3

#include <Servo.h>

int delayVal = 40;

//Creating Servo Objects

Servo rightAnkle; // เท้าขวา

Servo leftAnkle;  // เท้าซ้าย

Servo rightKnee; // เข่าขวา

Servo leftKnee;   // เข่าซ้าย

Servo rightThigh; // ต้นขาขวา

Servo leftThigh;  // ต้นขาซ้าย

Servo rightHip;  // สะโพกขวา

Servo leftHip;  // สะโพกซ้าย

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);  // สะโพกซ้าย

  delay(1000);

  stand();

  

}

void loop()

{

stand();

turnLeft();

}

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(90);   // เข่าขวา

  int rightKneePos = 90;

  rightThigh.write(100);  // ต้นขาขวา

  int rightThighPos = 100;

  rightHip.write(90);   // สะโพกขวา

  int rightHipPos = 90;

  leftAnkle.write(90);  // เท้าซ้าย

  int leftAnkPos = 90;

  leftKnee.write(90);  // เข่าซ้าย

  int leftKneePos = 90;

  leftThigh.write(80);  // ต้นขาซ้าย

  int leftThighPos = 80;

  leftHip.write(92);  // สะโพกซ้าย

  int leftHipPos = 92;

}

///////////////LEFT/////////////////////////////////////////

void turnLeft()

{

  for (int i = 0; i < 5; i += 1)

  {

    leftThighPos -= 2;

    leftKneePos += 2;

    leftThigh.write( leftThighPos);

    leftKnee.write(leftKneePos );

    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 );

    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)

  {

    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);

    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);

  }

}

ถอดสาย USB ออก แล้ว ใส่ถ่าน  18650 จำนวน 2 ก้อน แล้วลองทดสอบ

โปรเจค หุ่นยนต์หลบสิ่งกีดขวาง Arduino เวอร์ชั่น 1

โปรเจค หุ่นยนต์หลบสิ่งกีดขวาง Arduino เวอร์ชั่น 1

หลังจากที่เราได้ทดลอง การควบคุมรถบังคับด้วย แอนดรอยด์ ไปแล้ว ต่อไปเราจะเรียนรู้ การเขียนโปรแกรม ควบคุมหุ่นยนต์ให้หลบสิ่งกีดขวาง เรียกว่า หุ่นยนต์หลบสิ่งกีดขวาง เวอร์ชั่น 1 ซึ่งทำงานโดยการเคลื่อนที่ไปด้านหน้า เมื่อเซนเซอร์ Ultrasonic เจอสิ่งกีดขวาง จะมีการเคลื่อนที่ถอยหลังแล้วเลี้ยวหลบไปทางซ้าย

หุ่นยนต์หลบสิ่งกีดขวาง อุปกรณ์ที่ต้องใช้ก็คือ

     1. 4WD Smart Robot Car Chassis Kits

     2. Arduino UNO R3 - Made in italy

     3. Arduino Sensor Shield V5.0

     4. เซนเซอร์ Ultrasonic Module HC-SR04

     5. Mounting Bracket for HC-SR04 Ultrasonic Module แบบสั้น

     6. Motor Drive Module L298N

     7. สาย Jumper Female to Male ยาว 20cm.

     8. สาย Jumper Female to Female ยาว 20cm.

     9. รางถ่านแบบ 18650 ใส่ถ่าน 2 ก้อน

     10. แบตเตอรี่ลิเธียม 18650 จำนวน 2 ก้อน

     11. เสารองแผ่นพีซีบีโลหะแบบเหลี่ยม 6 mm

     12. สกรูหัวกลมน็อตตัวเมีย ขนาด 3มม ยาว12มม.

     13. เพาเวอร์สวิตซ์สำหรับเปิดปิด (ON / OFF Rocker Switch)


เริ่มด้วย ต่อสายไฟสำหรับมอเตอร์ ทั้ง 4 ตัว ที่ 4WD smart car chassis ให้มา โดยให้ ขั้วลบ สายไฟสีดำ อยู่ด้านบน เหมือนกันทั้ง 4 ตัว

ประกอบเข้ากับ โครง ชิ้นที่ 1 ของ 4WD smart car chassis ดังรูป

ประกอบ น็อต 6 จุด สำหรับ วาง โครง ชิ้นที่ 2

ประกอบ บอร์ด Motor Drive Module L298N ลงที่ โครง ชิ้นที่ 1

เชื่อมสายไฟ ของ มอเตอร์ ด้านบน สายสีดำ ขั้ว -  ของมอเตอร์ ตัวที่1 เข้า กับมอเตอร์ ตัวที่2 และ สายสีแดง ขั้ว+  ของมอเตอร์ ตัวที่1 เข้า กับมอเตอร์ ตัวที่2 และ เช่นกันทำที่ มอเตอร์ คู่ด้านล่างด้วย เพื่อให้เหมือนมีมอเตอร์ จาก 4 ตัว เหลือ 2 ตัว

จากนั้น ประกอบสายไฟ ของมอเตอร์ ทั้ง 2 ตัว เข้ากับ บอร์ด L298N ดังรูป

ประกอบ โครงชิ้นที่ 2 เข้าที่ ด้านบนของ โครงชิ้นที่ 1 และ ประกอบ บอร์ด Arduino UNO R3 ลงที่โครงชิ้นที่ 2 ซึ่งอยู่ชั้นบน

ประกอบ บอร์ด Arduino Sensor Shield V5.0 ลงที่ บอร์ด Arduino UNO R3

ประกอบ รางถ่านแบบ 18650 และ เชื่อมต่อ วงจรตามรูป

หมายเหตุ : ถ้ามี Jumper อยู่ที่ขา ENA และ ENB ของ บอร์ด L298N  ให้ถอดออก

การเชื่อมต่อระหว่าง บอร์ด Arduino Sensor Shield V5.0 กับ บอร์ด L298N  

.เชื่อมต่อ เซ็นเซอร์ HC-SR04 กับ Sensor Shield V5.0 ดังรูป

ประกอบ HC-SR04  เข้ากับ Mounting Bracket for HC-SR04 Ultrasonic Module แบบสั้น และ ประกอบ Bracket  เข้ากลับ โครงรถ2 ตามรูป

และทดสอบการเชื่อมต่อว่าถูกต้องหรือไม่ อุปกรณ์ใช้งานได้หรือไม่ ตามตัวอย่าง ลิงค์ด้านล่าง

http://robotsiam.blogspot.com/2016/09/hc-sr04-sensor-shield-v50.html


หลังจากนั้นให้ทดสอบเบื้องต้น ว่าการหมุนของล้อถูกต้องหรือไม่ โดย

เปิดโปรแกรม Arduino (IDE) และ Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3



// Motor A pins (enableA = enable motor, pinA1 = forward, pinA2 = backward)
int enableA = 3;
int pinA1 = 6;
int pinA2 = 7;

//Motor B pins (enabledB = enable motor, pinB2 = forward, pinB2 = backward)
int enableB = 5;
int pinB1 = 8;
int pinB2 = 9;

//This lets you run the loop a single time for testing
boolean run = true;

void setup() {
 pinMode(enableA, OUTPUT);
 pinMode(pinA1, OUTPUT);
 pinMode(pinA2, OUTPUT);

 pinMode(enableB, OUTPUT);
 pinMode(pinB1, OUTPUT);
 pinMode(pinB2, OUTPUT);
}
void loop() {
  if (run) {
    delay(2000);
    enableMotors();
    //Go forward
    forward(200);
    //Go backward
    backward(200);
    //Turn left
    turnLeft(400);
    coast(200);
    //Turn right
    turnRight(400);
    coast(200);
    //This stops the loop
    run = false; 
  }
}

//Define high-level H-bridge commands

void enableMotors()
{
 motorAOn();
 motorBOn();
}

void disableMotors()
{
 motorAOff();
 motorBOff();
}

void forward(int time)
{
 motorAForward();
 motorBForward();
 delay(time);
}

void backward(int time)
{
 motorABackward();
 motorBBackward();
 delay(time);
}

void turnLeft(int time)
{
 motorABackward();
 motorBForward();
 delay(time);
}

void turnRight(int time)
{
 motorAForward();
 motorBBackward();
 delay(time);
}

void coast(int time)
{
 motorACoast();
 motorBCoast();
 delay(time);
}

void brake(int time)
{
 motorABrake();
 motorBBrake();
 delay(time);
}
//Define low-level H-bridge commands

//enable motors
void motorAOn()
{
 digitalWrite(enableA, HIGH);
}

void motorBOn()
{
 digitalWrite(enableB, HIGH);
}

 //disable motors
void motorAOff()
{
 digitalWrite(enableB, LOW);
}

void motorBOff()
{
 digitalWrite(enableA, LOW);
}

 //motor A controls
void motorAForward()
{
 digitalWrite(pinA1, HIGH);
 digitalWrite(pinA2, LOW);
}

void motorABackward()
{
 digitalWrite(pinA1, LOW);
 digitalWrite(pinA2, HIGH);
}

//motor B controls
void motorBForward()
{
 digitalWrite(pinB1, HIGH);
 digitalWrite(pinB2, LOW);
}

void motorBBackward()
{
 digitalWrite(pinB1, LOW);
 digitalWrite(pinB2, HIGH);
}

//coasting and braking
void motorACoast()
{
 digitalWrite(pinA1, LOW);
 digitalWrite(pinA2, LOW);
}

void motorABrake()
{
 digitalWrite(pinA1, HIGH);
 digitalWrite(pinA2, HIGH);
}

void motorBCoast()
{
 digitalWrite(pinB1, LOW);
 digitalWrite(pinB2, LOW);
}

void motorBBrake()
{
 digitalWrite(pinB1, HIGH);
 digitalWrite(pinB2, HIGH);
}

ถอดสาย USB ระหว่าง Arduino กับ คอมพิวเตอร์ออก และ หาอุปกรณ์ที่สามารถยกตัวรถ 4WD smart car chassis ขึ้นแล้ว ล้อไม่แตะพื้น เพือทดสอบการหมุนของล้อว่าถูกต้องหรือไม่

ใส่ แบตเตอรี่ลิเธียม 18650 จำนวน 2 ก้อน ไปที่ รางถ่าน และ ตรวจสอบขั้วของแบตเตอรี่ ใส่ถุกต้องหรือไม่

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

ถ้าทุกอย่างถูกต้อง ทดลอง ยกลงวางที่พื้นแล้วทดสอบ อีกครั้ง ถอด แบตเตอรี่ออก (หรือ ปิดเปิด สวิทช์ไฟใหม่) แล้วใส่เข้าไปใหม่ ถ้าทุกอย่างถูกต้อง รถจะเครื่อนไปข้างหน้า-ถอยหลัง แล้ว เลี้ยวซ้าย แล้ว จึงกลับสู่ตำแหน่งเดิม

NewPing.h  คือไลบรารี่ NewPing เป็น ไลบรารี่ ฟังก์ชัน ที่มีผู้พัฒนาเตรียมพร้อมไว้ให้เราแล้ว โดยให้ไปดาวน์โหลด ไลบรารี่ NewPing ได้ที่

  https://bitbucket.org/teckel12/arduino-new-ping/downloads

จากนั้นทดสอบ หุ่นยนต์หลบสิ่งกีดขวาง ได้โดย  Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3

#include <NewPing.h>

//Tell the Arduino where the sensor is hooked up
NewPing sonar(10, 11);

int enableA = 3;
int pinA1 = 6;
int pinA2 = 7;

int enableB = 5;
int pinB1 = 8;
int pinB2 = 9;

long inches;

void setup() {
pinMode(enableA, OUTPUT);
pinMode(pinA1, OUTPUT);
pinMode(pinA2, OUTPUT);

pinMode(enableB, OUTPUT);
pinMode(pinB1, OUTPUT);
pinMode(pinB2, OUTPUT);
}

void loop() {

//Run the motors at slightly less than full power
analogWrite(enableA, 200);
analogWrite(enableB, 200);

//Ping the sensor and determine the distance in inches
inches = sonar.ping_in();

//If the robot detects an obstacle less than four inches away, it will back up, then turn left; if no obstacle is detected, it will go forward
if (inches < 4) {
analogWrite(enableA, 255);
analogWrite(enableB, 255);
backward(600);
coast(200);
turnLeft(600);
coast(200);}
else {
forward(1);

}
}

//Define high-level H-bridge commands

void enableMotors()
{
motorAOn();
motorBOn();
}

void disableMotors()
{
motorAOff();
motorBOff();
}

void forward(int time)
{
motorAForward();
motorBForward();
delay(time);
}

void backward(int time)
{
motorABackward();
motorBBackward();
delay(time);
}

void turnLeft(int time)
{
motorABackward();
motorBForward();
delay(time);
}

void turnRight(int time)
{
motorAForward();
motorBBackward();
delay(time);
}

void coast(int time)
{
motorACoast();
motorBCoast();
delay(time);
}

void brake(int time)
{
motorABrake();
motorBBrake();
delay(time);
}
//Define low-level H-bridge commands

//enable motors
void motorAOn()
{
digitalWrite(enableA, HIGH);
}

void motorBOn()
{
digitalWrite(enableB, HIGH);
}

//disable motors
void motorAOff()
{
digitalWrite(enableB, LOW);
}

void motorBOff()
{
digitalWrite(enableA, LOW);
}

//motor A controls
void motorAForward()
{
digitalWrite(pinA1, HIGH);
digitalWrite(pinA2, LOW);
}

void motorABackward()
{
digitalWrite(pinA1, LOW);
digitalWrite(pinA2, HIGH);
}

//motor B controls
void motorBForward()
{
digitalWrite(pinB1, HIGH);
digitalWrite(pinB2, LOW);
}

void motorBBackward()
{
digitalWrite(pinB1, LOW);
digitalWrite(pinB2, HIGH);
}

//coasting and braking
void motorACoast()
{
digitalWrite(pinA1, LOW);
digitalWrite(pinA2, LOW);
}

void motorABrake()
{
digitalWrite(pinA1, HIGH);
digitalWrite(pinA2, HIGH);
}

void motorBCoast()
{
digitalWrite(pinB1, LOW);
digitalWrite(pinB2, LOW);
}

void motorBBrake()
{
digitalWrite(pinB1, HIGH);
digitalWrite(pinB2, HIGH);
}