โปรเจค หุ่นยนต์หลบสิ่งกีดขวาง 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


ทดสอบการเคลื่อนที่ของหุ่นยนต์ Auduino UNO + L298P


โปรเจค นี้จะเป็นการนำ Arduino UNO R3 และ L298P Motor Shield  มาทำเป็น หุ่นยนต์ ซึ่งการใช้ L298P Motor Shield จะทำให้การประกอบหุ่นยนต์ สะดวกและง่ายขึ้น เมื่อเทียบกับการใช้ Motor Driver Module L298N



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



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. สกรูหัวกลม+น็อตตัวเมีย ยาว 12 มม.

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

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



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



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



ใช้ สกรูหัวกลม+น็อตตัวเมีย ยาว 12 มม. ยึด รางถ่านแบบ 18650 และ เชื่อมต่อ สายสีแดง ขั้วบวก เข้าที่ เพาเวอร์สวิตซ์สำหรับเปิดปิด (ON / OFF )
  


ยึด บอร์ด Arduino UNO R3 เข้ากับโครงหุ่นยนต์



เสียบ L298P Motor Shield Board เข้ากับ Arduino UNO R3



ใช้ จั้มเปอร์ ผู้-ผู้ สีแดง เชื่อมต่อ จาก เพาเวอร์สวิตซ์ ด้านที่เหลือ 



ไปยัง ไฟ+ ของ L298P  และ ใช้ จั้มเปอร์ ผู้-ผู้ สีดำ เชื่อมต่อจากสายสีดำของรางถ่านไปยัง GND กราวด์ ของ L298P


ใช้ จั้มเปอร์ ผู้-ผู้ 4 เส้น 4 สี เชื่อมต่อ จาก มอเตอร์ A และ B 


ไปยัง MOTORA และ MOTORฺB ของ L298P ดังรูป



เก็บรัดสายให้เรียบร้อย


เพื่อความสะดวก ในการติดตั้งเซ็นเซอร์ ต่างๆ  จะใช้ ด้านนี้เป็นด้านหน้า ของหุ่นยนต์




2. การอัพโหลดโปรแกรม


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





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


/*

    Robot with Arduino UNO + L298P
    For more details visit:
    https://robotsiam.blogspot.com/2017/12/arduino-uno-l298p.html

*/

/*-------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() {

  //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
}

void loop() {


  Stop(2000);
  Forward(400);
  Stop(200);
  Backward(400);
  Stop(200);
  turnLeft(500);
  Stop(200);
  turnRight(600);


}

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

}




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



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


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

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


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



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


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



3. ทดสอบการเคลื่อนที่ของหุ่นยนต์

ใส่ถ่าน แบบ 18650 จำนวน 2 ก้อน เข้าที่รางถ่าน 
เปิดเพาเวอร์สวิตซ์สำหรับเปิดปิด (ON / OFF ) ถ้าการต่อหุ่นยนต์ถูกต้อง หุ่นยนต์ จะ เดินหน้า-ถอยหลัง-เลี้ยวซ้าย และ เลี้ยวขวา ตาม วีดีโอผลลัพธ์การทำงานของ ทดสอบการเคลื่อนที่ของหุ่นยนต์ UNO + L298P



วีดีโอผลลัพธ์การทำงานของ ทดสอบการเคลื่อนที่ของหุ่นยนต์เบื้องต้น Auduino UNO + L298P