Code โปรเจค หุ่นยนต์หลบสิ่งกีดขวาง Arduino UNO + HY-SRF05

โปรเจค นี้จะเป็นการนำ Arduino UNO R3 + L298P Motor Shield และ เซ็นเซอร์วัดระยะทาง HY-SRF05 มาทำเป็น หุ่นยนต์หลบสิ่งกีดขวาง โดยให้หุ่นยนต์ตัดสินใจโดยอัตโนมัติ ด้วยการวัดระยะทาง จาก สิ่งกีดขวาง และ เมื่อมีสิ่งกีดขวางอยู่ในระยะที่กำหนดไว้ ในตัวอย่างคือ น้อยกว่า 20 เซ็นติเมตร ให้หุ่นยนต์ หยุด - ถอยหลัง แล้ว เลี้ยวซ้าย หลบสิ่งกีดขวางนั้นเสมอ โดยในขณะที่ทำงาน เมื่อมีสิ่งกีดขวางอยู่ในระยะน้อยกว่า 20 เซ็นติเมตร ให้ส่งเสียงเตือนจาก Buzzer ออดไฟฟ้า ของ L298P ออกมาด้วย

อุปกรณ์ที่ใช้

1. 2WD Smart Car Robot Chassis Kits

2. Arduino UNO R3 - Made in italy

3. L298P Motor Shield Board

4. Jumper (M2M) cable 20cm Male to Male

5. Jumper (F2M) cable 30cm Female to Male

6. สกรูหัวกลม+น็อตตัวเมีย ยาว 12 มม.

7. รางถ่าน 18650 แบบ 2 ก้อน

8. ถ่านชาร์จ 18650 Panasonic NCR18650B 3.7v  จำนวน 2 ก้อน

9. HY-SRF05 SRF05 Ultrasonic Distance Sensor

10. Mounting Bracket for HC-SR04 แบบสั้น

1. ประกอบหุ่นยนต์

เริ่มต้นด้วยการ ประกอบ หุ่นยนต์ Arduino UNO + L298P และ ทดสอบการเคลื่อนที่ของหุ่นยนต์ ตามลิงค์ บทความด้านล่าง


https://robotsiam.blogspot.com/2017/12/arduino-uno-l298p.html

2. ประกอบ HY-SRF05 Ultrasonic

เซ็นเซอร์วัดระยะทาง HY-SRF05

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


หลักการทำงานของ HY-SRF05

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


การทำงานเริ่มจากเมื่อทริกสัญญาณเข้าที่ขา Trig ให้เป็น HIGH จะทำให้โมดูลเริ่มวัดระยะ แล้วส่งค่าที่วัดได้ออกมาเป็นความกว้างพัลส์ที่ขา Echo นำค่าเวลาความกว้างพัลส์ที่ส่งมาจากขา Echo เมื่อนำมาหาร 29 / 2 จะได้ค่าระยะออกมาเป็นเซ็นติเมตร โดยสามารถวัดระยะได้ตั้งแต่ 2 เซ็นติเมตร ไปจนถึง 4.5 เมตร ความผิดพลาดขึ้นอยู่กับระยะทางที่วัด

ใช้ จั้มเปอร์ ผู้-เมีย 30cm เชื่อมต่อ HY-SRF05 เข้ากับ L298P ตามรูปด้านล่าง 

SRF05 <-> L298P

VCC <-> 5V

GND <-> Gnd

Trig <-> D7

Echo <-> D6

ใช้สาย USB เชื่อมต่อระหว่าง คอมพิวเตอร์ กับ Arduino UNO R3

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

const unsigned int TRIG_PIN=7;

const unsigned int ECHO_PIN=6;

const unsigned int BAUD_RATE=9600;



void setup() {

  pinMode(TRIG_PIN, OUTPUT);

  pinMode(ECHO_PIN, INPUT);

  Serial.begin(BAUD_RATE);

}



void loop() {

  digitalWrite(TRIG_PIN, LOW);

  delayMicroseconds(2);

  digitalWrite(TRIG_PIN, HIGH);

  delayMicroseconds(10);

  digitalWrite(TRIG_PIN, LOW);

  

 const unsigned long duration= pulseIn(ECHO_PIN, HIGH);

 int distance= duration/29/2;

 if(duration==0){

   Serial.println("Warning: no pulse from sensor");

   } 

  else{

      Serial.print("distance to nearest object:");

      Serial.println(distance);

      Serial.println(" cm");

  }

 delay(100);


 }




ไปที่ Tools > Board เลือกเป็น Arduino/Genuino UNO

ไปที่ Tools > Port แล้วเลือกพอร์ตที่ปรากฏ (กรณีใช้เครื่องคอมพิวเตอร์ที่มี COM Port มากกว่าหนึ่ง  ให้เลือกตัวอื่นที่ไม่ใช่ COM1)

ในตัวอย่างเลือกเป็น "COM14"

(ถ้ายังว่าง หรือ เป็น COM1 ให้ตรวจสอบการติดตั้งไดร์เวอร์ การต่อสาย USB ของ Arduino UNO)

(ถ้าไม่พบให้ทำการติดตั้งไดร์เวอร์ก่อน) 
การติดตั้งโปรแกรม Arduino (IDE) และ การติดตั้งไดร์เวอร์

กดปุ่ม  เพื่ออัพโหลด

หากสามารถอัพโหลดโปรแกรมลงบอร์ดได้สำเร็จ จะแสดงคำว่า Done uploading. ที่แถบด้านล่าง

เปิดสวิตช์ไฟ จากรางถ่านเข้าสู่หุ่นยนต์

แล้วไปที่ Tools -> Serial Monitor

เลือก Both NL & CR และ เลือก 9600 baud

Serial Monitor จะแสดง ระยะทางที่ห่าง จาก สิ่งกีดขวาง มีหน่วยเป็น cm (เซนติเมตร)


แสดงว่า การเชื่อมต่อ SRF05 ของเรานั้นถูกต้อง และ พร้อมใช้งานแล้ว

3. ทดสอบการทำงาน

Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3




/*

    Obstacle Avoiding Robot with Arduino UNO + HY-SRF05
    For more details visit:
    https://robotsiam.blogspot.com/2017/12/arduino-uno-hy-srf05.html

*/

/*-------definning SRF05------*/

const unsigned int TRIG_PIN = 7;
const unsigned int ECHO_PIN = 6;
const unsigned int BAUD_RATE = 9600;

/*-------definning Outputs------*/

int MA1 = 12;     // Motor A1
int MA2 =  3;     // Motor A2
int PWM_A =  10;   // Speed Motor A

int MB1 =  13;     // Motor B1
int MB2 =  8;     // Motor B2
int PWM_B =  11;  // Speed Motor B

int SPEED = 200;  // Speed PWM สามารถปรับความเร็วได้ถึง 0 - 255

void setup() {

  pinMode(4, OUTPUT); //Buzzer

  //Setup Channel A
  pinMode(12, OUTPUT); //Motor A1
  pinMode(3, OUTPUT); //Motor A2
  pinMode(10, OUTPUT); //Speed PWM Motor A

  //Setup Channel B
  pinMode(13, OUTPUT);  //Motor B1
  pinMode(8, OUTPUT);  //Motor B2
  pinMode(11, OUTPUT); //Speed PWM Motor B

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  Serial.begin(BAUD_RATE);

}

void loop() {

  digitalWrite(TRIG_PIN, LOW);

  delayMicroseconds(2);

  digitalWrite(TRIG_PIN, HIGH);

  delayMicroseconds(10);

  digitalWrite(TRIG_PIN, LOW);



  const unsigned long duration = pulseIn(ECHO_PIN, HIGH);

  int distance = duration / 29 / 2;

  if ((distance > 1) && (distance < 20))  {

    digitalWrite(4, HIGH);
    Stop(500);
    digitalWrite(4, LOW);
    Backward(400);
    turnLeft(400);
    Serial.print("distance to nearest object:");
    Serial.println(distance);
    Serial.println(" cm");

  }

  else {

    digitalWrite(4, LOW);
    Forward(10);
    Serial.print("distance to nearest object:");
    Serial.println(distance);
    Serial.println(" cm");

  }

  delay(100);

}

void Backward(int time)
{
  digitalWrite(MA1, LOW);
  digitalWrite(MA2, HIGH);
  analogWrite(PWM_A, SPEED);

  digitalWrite(MB1, HIGH);
  digitalWrite(MB2, LOW);
  analogWrite(PWM_B, SPEED);

  delay(time);
}

void Forward (int time)
{
  digitalWrite(MA1, HIGH);
  digitalWrite(MA2, LOW);
  analogWrite(PWM_A, SPEED);

  digitalWrite(MB1, LOW);
  digitalWrite(MB2, HIGH);
  analogWrite(PWM_B, SPEED);

  delay(time);
}

void turnLeft(int time)
{
  digitalWrite(MA1, HIGH);
  digitalWrite(MA2, LOW);
  analogWrite(PWM_A, SPEED);

  digitalWrite(MB1, LOW);
  digitalWrite(MB2, LOW);
  analogWrite(PWM_B, 0);

  delay(time);
}

void turnRight(int time)
{
  digitalWrite(MA1, LOW);
  digitalWrite(MA2, LOW);
  analogWrite(PWM_A, 0);

  digitalWrite(MB1, LOW);
  digitalWrite(MB2, HIGH);
  analogWrite(PWM_B, SPEED);

  delay(time);
}

void Stop(int time)
{
  digitalWrite(MA1, LOW);
  digitalWrite(MA2, LOW);
  analogWrite(PWM_A, 0);

  digitalWrite(MB1, LOW);
  digitalWrite(MB2, LOW);
  analogWrite(PWM_B, 0);

  delay(time);

}




วีดีโอผลลัพธ์ หุ่นยนต์หลบสิ่งกีดขวาง Arduino UNO + HY-SRF05