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


การทำงานหุ่นยนต์หลบสิ่งกีดขวางของเวอร์ชั่นนี้  คือ เมื่อ หุ่นยนต์พบสิ่งกีดขวาง จะถอยหลัง แล้ว เลี้ยวหลบไปทางซ้ายเสมอ หุ่นยนต์หลบสิ่งกีดขวาง Arduino 2WD

อุปกรณ์ที่ต้องใช้ก็คือ


   
  1. 2WD Smart Robot Car Chassis Kits

     2. Arduino UNO R3 - Made in italy

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

     4. Mounting Bracket for HC-SR04 Ultrasonic Module แบบยาว

     5. Motor Drive Module L298N

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

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

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

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

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


 เริ่มต้นด้วยการ ประกอบ 2WD Smart Car Robot Chassis Kits


เชื่อมต่อ รางถ่าน 18650 กับ Motor Drive Module L298N
และ Motor Drive Module L298N กับ Arduino UNO R3

ภาพต่อวงจรทั้งหมดตามรูป





*** VCC ของ Arduino UNO R3 คือ 5V ***


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





เชื่อมต่อ Ultrasonic Module HC-SR04 กับ 
Arduino UNO R3 ดังรูป



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

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

เรียนรู้ วิธีเพิ่มไลบรารี่ NewPing และ การทำงานของ Ultrasonic Module HC-SR04 ได้ที่

https://robotsiam.blogspot.com/2016/09/ultrasonic-module-hc-sr04.html


ทดสอบการทำงานของ HC-SR04 โดย เปิดโปรแกรม Arduino (IDE) และ Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3


#include <NewPing.h>


NewPing sonar(10, 11);

long inches;

void setup() {

Serial.begin(9600);
}

void loop() {
delay(50);

inches = sonar.ping_in();

Serial.print(inches);
Serial.print(" in.");
Serial.print("\n");

}



เมื่อ Upload เสร็จ ให้เปิด Serial Monitor ขึ้นมา โดยไปที่  Tools -> Serial Monitor




ทดสอบการทำงาน เซนเซอร์ HC-SR04 กับ Arduino UNO R3 โดย ทดลอง เอามือ หรือ วัตถุอื่นๆ เคลื่อนไหว ขึ้นลง หน้าจุดรับสัญญาณ อัลตร้าโซนิค HC-SR04
 


ที่ Serial Monitor จะแสดงค่า ระยะความห่างของมือเรากับ HC-SR04 หน่วยเป็น นิ้ว (inch) ตามที่เราเครื่อนไหว แสดงว่า โมดูลอัลตร้าโซนิค HC-SR04 ของเรา นั้นพร้อมใช้งานแล้วครับ



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


เปิดโปรแกรม 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 กับ คอมพิวเตอร์ออก ใส่ แบตเตอรี่ลิเธียม 18650 จำนวน 2 ก้อน ไปที่ รางถ่าน และ ตรวจสอบขั้วของแบตเตอรี่ ใส่ถุกต้องหรือไม่  และ หาอุปกรณ์ที่สามารถยกตัวรถ 2WD smart car chassis ขึ้นแล้ว หรือใช้มือยกไว้ โดยล้อไม่แตะพื้น เพือทดสอบการหมุนของล้อว่าถูกต้องหรือไม่





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

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

 



หลังจากนั้นทดสอบ หุ่นยนต์หลบสิ่งกีดขวาง Arduino 2WD ได้โดย  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(400);
coast(200);
turnLeft(400);
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);
}



ทดลอง ยกลงวางที่พื้นแล้ว หาสิ่งกีดขวาง ทดสอบ อีกครั้ง จากรถจะกลายเป็น "หุ่นยนต์หลบสิ่งกีดขวาง"

หมายเหตุ : หากต้องการเพิ่มการแสดงผลการทำงาน ของ 
"หุ่นยนต์หลบสิ่งกีดขวาง" ได้ดังนี้ 

อุปกรณ์ที่ต้องเพิ่มเติมก็คือ
 

1. หลอดไฟ LED สีเขียว จำนวน 1 ตัว
2. หลอดไฟ LED สีแดง จำนวน 1 ตัว
3. หลอดไฟ LED สีเหลือง จำนวน 1 ตัว
4. รีซิสเตอร์ 220 Ohm 1/4 Watt 5%  จำนวน 3 ตัว

ต่อวงจรเพิ่มดังนี้
 








เปิดโปรแกรม Arduino (IDE) และ 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);

  pinMode(2, OUTPUT);

  pinMode(12, OUTPUT);
  pinMode(13, 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 < 5) {
    analogWrite(enableA, 255);
    analogWrite(enableB, 255);
    digitalWrite(2, LOW);
    digitalWrite(12, LOW);
    digitalWrite(13, HIGH);
    backward(400);
    coast(200);
    turnLeft(400);
    coast(200);

  }

  else if (inches >= 5  and inches < 30 ) {
    forward(1);
    digitalWrite(2, LOW);
    digitalWrite(12, HIGH);
    digitalWrite(13, LOW);

  }

  else {
    forward(1);
    digitalWrite(2, HIGH);
    digitalWrite(12, LOW);
    digitalWrite(13, LOW);
  }
}

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

}

การแสดงผลของ LED คือ

1. เมื่อไม่มีสิ่งกีดขวาง หรือ
มีสิ่งกีดขวางในระยะมากกว่า 30 นิ้ว LED สีเขียวจะติด
2. เมื่อมีสิ่งกีดขวางในระยะ 5 - 30 นิ้ว LED สีเหลืองจะติด
3. เมื่อมีสิ่งกีดขวางในระยะ น้อยกว่า 5 นิ้ว LED สีแดงจะติด

วีดีโอผลลัพธ์การทำงานของ 
โปรเจค หุ่นยนต์หลบสิ่งกีดขวาง Arduino 2WD