ใบงานที่ 7 Measure Distance with Arduino and Ultrasonic Sensor

นางสาวพรรณพิลาศ ศรีสมศักดิ์ 6031280015 1สทค1

 นางสาวรัตนาพร    กลิ่นผล 6031280040     1สทค2

Servo Motor (SG90)  

 Servo เป็นคำศัพท์ที่ใช้กันทั่วไปในระบบควบคุมอัตโนมัติ มาจากภาษาละตินคำว่า Sevus หมายถึง “ทาส” (Slave) ในเชิงความหมายของ Servo Motor ก็คือ Motor ที่เราสามารถสั่งงานหรือตั้งค่า แล้วตัว Motor จะหมุนไปยังตำแหน่งองศาที่เราสั่งได้เองอย่างถูกต้อง โดยใช้การควบคุมแบบป้อนกลับ (Feedback Control) ในบทความนี้จะกล่าวถึง RC Servo Motor ซึ่งนิยมนำมาใช้ในเครื่องเล่นที่บังคับด้วยคลื่นวิทยุ (RC = Radio - Controlled) เช่น เรือบังคับวิทยุ รถบังคับวิทยุ เฮลิคอปเตอร์บังคับวิทยุ เป็นต้น

Servo คืออุปกรณ์มอเตอร์ ที่สามารถควบคุมการหมุนที่แม่นยำ เซอร์โว SG90 มีขนาดเล็กแรงบิด 1.2-1.4 kg/cm

KG/cm สีน้ำตาลเป็นสายกราวด์ สีแดงเป็นไฟเข้า 4.8-7.2V สีส้มเป็นสัญญาณอินพุต  หมุน 0-180องศา

ถ้าทำให้หมุ่น 360 ต่อเนื่่อง องศาให้ใช้ 2.2K ohm

• ขนาด 21.5mmx11.8mmx22.7mm

• น้ำหนัก 9 กรัม

• ความเร็วเมื่อไม่มีโหลด 0.12 วินาที/ุ60องศา (4.8V)

• แรงบิด 1.2-1.4 kg/cm (4.8V)

• ทำงานที่อุณหภูมิ -30-60 องศาเซลเซียส

• เวลาหยุดก่อนรับคำสั่งใหม่ 7 มิลลิวินาที

• ทำงานที่ไฟ 4.8V - 6V

นิยมใช้ร่วมกับ

• Mounting Bracket for Servo SG90

• Switching Power supply แหล่งจ่ายไฟ 5V 5.5A

• Switching Power supply แหล่งจ่ายไฟ 5V 10A

• Switching Power supply แหล่งจ่ายไฟ 5V 20A

HC-SR04

หลักการทำงาน
การต่อใช้งานโมดูล

1. ขา VCC สำหรับต่อแรงดันไฟเลี้ยงไม่เกิน 5V
2. ขา Trig เป็นขาอินพุตรับสัญญาณพัลส์ความกว้าง 10 ไมโครวินาทีเพื่อกระตุ้นการสร้างคลื่นอัลตราโซนิกความถี่ 40KHz ออกสู่อากาศจากตัวส่ง
3. ขา Echo เป็นขาเอาต์พุตสำหรับส่งสัญญาณพัลส์ออกจากโมดูลไปยังไมโครคอนโทรลเลอร์ เพื่อตรวจจับความกว้างของสัญญาณพัลส์และคำนวณเป็นระยะทาง
4. ขา GND สำหรับต่อจุดกราวด์ร่วมแรงดันและสัญญาณ

  HC-SR04 เป็นเซนเซอร์โมดูลสำหรับตรวจจับวัตถุและวัดระยะทางแบบไม่สัมผัส [1-2] โดยใช้คลื่นอัลตราโซนิก ซึ่งเป็นคลื่นเสียงความถี่สูงเกินกว่าการได้ยินของมนุษย์ วัดระยะได้ตั้งแต่ 2 – 400 เซนติเมตร หรือ 1 – 156 นิ้ว สามารถต่อใช้งานกับไมโครคอนโทรลเลอร์ได้ง่าย ใช้พลังงานต่ำ เหมาะกับการนำไปประยุกต์ใช้งานด้านระบบควบคุมอัตโนมัติ หรืองานด้านหุ่นยนต์ หลักการทำงาน จะเหมือนกันกับการตรวจจับวัตถุด้วยเสียงของค้างคาว ตามรูปที่ 1 โดยจะประกอบไปด้วยตัว รับ-ส่ง อัลตราโซนิก ตัวส่งจะส่งคลื่นความถี่ 40 kHz ออกไปในอากาศด้วยความเร็วประมาณ 346 เมตรต่อวินาที และตัวรับจะคอยรับสัญญาณที่สะท้อนกลับจากวัตถุ เมื่อทราบความเร็วในการเคลื่อนที่ของคลื่น, เวลาที่ใช้ในการเดินทางไป-กลับ (t) ก็จะสามารถคำนวณหาระยะห่างของวัตถุ (S) ได้จาก

 S = 346 × 0.5t                                    (1)

รูปที่ 1 หลักการตรวจจับและวัดระยะห่างระหว่างวัตถุด้วยคลื่นเสียง

เพื่อให้การคำนวณหาระยะเป็นไปด้วยความง่าย โมดูลเซนเซอร์นี้จึงได้ประมวลผลให้เรียบร้อยแล้ว และส่งผลลัพธ์ของการคำนวณเป็นสัญญาณพัลส์ที่มีความกว้างสัมพันธ์กับระยะทางที่วัดได้

โมดูลนี้มีจุดต่อใช้งานทั้งหมด 4 จุด การใช้งานบอร์ด STM32F4DISCOVERY การทดลองในเบื้องต้นสามารถต่อวงจรอย่างง่ายได้โดยใช้โปรโตบอร์ดและสายไฟต่อวงจรตามรูปที่ 2 ทั้งนี้ต้องตรวจสอบคุณสมบัติของพอร์ตของไมโครคอนโทรลเลอร์จากดาต้าชีท [3] ว่าสามารถทนระดับแรงดันลอจิก High (5V) ได้

รูปที่ 2 การต่อใช้งานโมดูลอัลตราโซนิก HC-SR04

          ตามคุณลักษณะของเซนเซอร์ จะต้องสร้างสัญญาณพัลส์ความกว้างไม่น้อยกว่า 10 msec ป้อนเข้าที่ขา Trig หลังจากนั้นอีกประมาณ 1.4 msec จึงจะเริ่มมีสัญญาณพัลส์เกิดขึ้นที่ขา Echo มีความกว้างของสัญญาณตั้งแต่ 150 usec – 25 msec ซึ่งถ้าหากกว้างกว่านี้จะถือว่าตรวจไม่พบวัตถุ หลังจากนั้นควรหน่วงเวลาออกไปอีก 10 mS จึงจะส่งสัญญาณ Trig ออกไปอีกรอบ ตามรูปที่ 3

รูปที่ 3 สัญญาณที่ขา Trig และขา Echo ของโมดูลเซนเซอร์อัลตราโซนิก HC-SR04 การตรวจจับความกว้างของสัญญาณใช้โมดูล PWM Capture ซึ่งให้เอาต์พุตออกมาเป็นเวลาในหน่วยวินาที และใช้สมการ (2) หรือ (3) เพื่อคำนวณหาระยะทางระหว่างวัตถุที่ตรวจพบ

ระยะทาง (cm) = ความกว้างของสัญญาณ Echo * 106 /58                         (2)

ระยะทาง (inch) = ความกว้างของสัญญาณ Echo * 106 /148                      (3) 

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

  1. โฟโต้บอร์ด 1 บอร์ด

  2. Servo Motor (SG90)  1 ตัว

  3. บอร์ด Arduino  1 บอร์ด

  4. สายไฟ ผู้ - ผู้

  5. สายอัปโหลด
  6. HC-SR04

รูปวงจร

ดาวน์โหลดรูปวงจร ที่นี่

คำอธิบายวงจร 

เป็นวงจรวัดระยะทางแสดงผล โดยใช้ Servo motor (เข็มชี้ตัวเลข0-7 ) และ Monitor หมุนแสดงผลชี้เป็นเลข 0-7  ตามระยะทางที่วัดได้มีหน่วยของ นิ้ว และ เซนติเมตร โดยที่ Monitor แสดงผล มุมของ Servo (Angle) จะแสดงตามค่า serial monitor

Code

#include <Servo.h>  //Load Servo Library

int trigPin=13; //Sensor Trip pin connected to Arduino pin 13

int echoPin=11;  //Sensor Echo pin connected to Arduino pin 11

int servoControlPin=6; //Servo control line is connected to pin 6

float pingTime;  //time for ping to travel from sensor to target and return

float targetDistance; //Distance to Target in inches

float speedOfSound=776.5; //Speed of sound in miles per hour when temp is 77 degrees.

float servoAngle; //Variable for the value we want to set servo to.

Servo myPointer;  //Create a servo object called myPointer

void setup() {

  // put your setup code here, to run once:

  Serial.begin(9600);

  pinMode(servoControlPin, OUTPUT);

  pinMode(trigPin, OUTPUT);

  pinMode(echoPin, INPUT);

  

  myPointer.attach(servoControlPin);  //Tell arduino where the servo is attached.

}

void loop() {

  // put your main code here, to run repeatedly: 

  

  digitalWrite(trigPin, LOW); //Set trigger pin low

  delayMicroseconds(2000); //Let signal settle

  digitalWrite(trigPin, HIGH); //Set trigPin high

  delayMicroseconds(15); //Delay in high state

  digitalWrite(trigPin, LOW); //ping has now been sent

  delayMicroseconds(10); //Delay in low state

  

  pingTime = pulseIn(echoPin, HIGH);  //pingTime is presented in microceconds

  pingTime=pingTime/1000000; //convert pingTime to seconds by dividing by 1000000 (microseconds in a second)

  pingTime=pingTime/3600; //convert pingtime to hourse by dividing by 3600 (seconds in an hour)

  targetDistance= speedOfSound * pingTime;  //This will be in miles, since speed of sound was miles per hour

  targetDistance=targetDistance/2; //Remember ping travels to target and back from target, so you must divide by 2 for actual target distance.

  targetDistance= targetDistance*63360;    //Convert miles to inches by multipling by 63360 (inches per mile)

  Serial.print("The Distance to Target is: ");

  Serial.print(targetDistance);

  Serial.println(" cm");

  Serial.print("The Distance to Target is: ");

  Serial.print(targetDistance);

  Serial.println(" inches");

  

  servoAngle = (106./7.) * targetDistance + 37; //Calculate Servo Angle from targetDistance

  myPointer.write(servoAngle); //write servoAngle to the servo

  Serial.print("Angle ");

  Serial.println(servoAngle);

  delay(100); //delay tenth of a  second to slow things down a little.

}

ดาวน์โหลดโค้ด Arduino ที่นี่

วิดีโอประกอบ