นางสาวจริญญา แสงวงศ์ 1 สทค 2 รหัส 6031280026
นางสาวจิรนันท์ จานศิลา 1 สทค 2 รหัส 6031280028
Measure Distance with Arduino and Ultrasonic Sensor
SG90 Servo Motor มาตรฐาน
Servo เป็นคำศัพท์ที่ใช้กันทั่วไปในระบบควบคุมอัตโนมัติ มาจากภาษาละตินคำว่า Sevus หมายถึง “ทาส” (Slave) ในเชิงความหมายของ Servo Motor ก็คือ Motor ที่เราสามารถสั่งงานหรือตั้งค่า แล้วตัว Motor จะหมุนไปยังตำแหน่งองศาที่เราสั่งได้เองอย่างถูกต้อง โดยใช้การควบคุมแบบป้อนกลับ (Feedback Control)
Servo คืออุปกรณ์มอเตอร์ ที่สามารถควบคุมการหมุนที่แม่นยำ เซอร์โว SG90 มีขนาดเล็กแรงบิด 1.2-1.4 kg/cm KG/cm สีน้ำตาลเป็นสายกราวด์ สีแดงเป็นไฟเข้า 4.8-7.2 V สีส้มเป็นสัญญาณอินพุต หมุน 0-180องศา ถ้าทำให้หมุน 360 ต่อเนื่อง องศาให้ใช้ 2.2 K ohm Tower Pro SG90 Mini Micro Servo
Servo motor รุ่นนี้เหมาะสำหรับการใช้งานกับ RC คอปเตอร์ หรือ งานที่ต้องการมอเตอร์น้ำหนักเบา (เฟืองเป็นพลาสติด) ให้ทอร์กที่ 1.8 kg-cm ที่แรงดัน 4.8 V
Feedback Control คือ ระบบควบคุมที่มีการวัดค่าเอาต์พุตของระบบนำมาเปรียบเทียบกับค่าอินพุตเพื่อควบคุมและปรับแต่งให้ค่าเอาต์พุตของระบบให้มีค่า เท่ากับ หรือ ใกล้เคียงกับค่าอินพุต
Feedback Control คือ ระบบควบคุมที่มีการวัดค่าเอาต์พุตของระบบนำมาเปรียบเทียบกับค่าอินพุตเพื่อควบคุมและปรับแต่งให้ค่าเอาต์พุตของระบบให้มีค่า เท่ากับ หรือ ใกล้เคียงกับค่าอินพุต
- ขนาด 21.5 mm x 11.8 mm x 22.7 mm
- น้ำหนัก 9 กรัม
- ความเร็วเมื่อไม่มีโหลด 0.12 วินาที/60องศา (4.8 V)
- แรงบิด 1.2-1.4 kg/cm (4.8 V)
- ทำงานที่อุณหภูมิ -30-60 องศาเซลเซียส
- เวลาหยุดก่อนรับคำสั่งใหม่ 7 มิลลิวินาที
- ทำงานที่ไฟ 4.8 V - 6 V
ส่วนประกอบภายนอก RC Servo Motor
- Case ตัวถัง หรือ กรอบของตัว Servo Motor
- Mounting Tab ส่วนจับยึดตัว Servo กับชิ้นงาน
- Output Shaft เพลาส่งกำลัง
- Servo Horns ส่วนเชื่อมต่อกับ Output shaft เพื่อสร้างกลไกล
- Cable สายเชื่อมต่อเพื่อ จ่ายไฟฟ้า และ ควบคุม Servo Motor จะประกอบด้วยสายไฟ 3 เส้น และ ใน RC Servo Motor จะมีสีของสายแตกต่างกันไปดังนี้
o สายสีแดง คือ ไฟเลี้ยง (4.8-6V)
o สายสีดำ หรือ น้ำตาล คือ กราวด์
o สายสีเหลือง (ส้ม ขาว หรือฟ้า) คือ สายส่งสัญญาณพัลซ์ควบคุม (3-5V)
- Connector จุดเชื่อมต่อสายไฟ
ส่วนประกอบภายใน RC Servo Motor
- Motor เป็นส่วนของตัวมอเตอร์
- Gear Train หรือ Gearbox เป็นชุดเกียร์ทดแรง
- Position Sensor เป็นเซ็นเซอร์ตรวจจับตำแหน่งเพื่อหาค่าองศาในการหมุน
- Electronic Control System เป็นส่วนที่ควบคุมและประมวลผล
Servo Motor Block Diagram
หลักการทำงานของ Servo Motor
เมื่อจ่ายสัญญาณพัลซ์เข้ามายัง Servo Motor ส่วนวงจรควบคุม (Electronic Control System) ภายใน Servo จะทำการอ่านและประมวลผลค่าความกว้างของสัญญาณพัลซ์ที่ส่งเข้ามาเพื่อแปลค่าเป็นตำแหน่งองศาที่ต้องการให้ Motor หมุนเคลื่อนที่ไปยังตำแหน่งนั้น แล้วส่งคำสั่งไปทำการควบคุมให้ Motor หมุนไปยังตำแหน่งที่ต้องการ โดยมี Position Sensor เป็นตัวเซ็นเซอร์คอยวัดค่ามุมที่ Motor กำลังหมุน เป็น Feedback กลับมาให้วงจรควบคุมเปรียบเทียบกับค่าอินพุตเพื่อควบคุมให้ได้ตำแหน่งที่ต้องการอย่างถูกต้องแม่นยำ
นิยมใช้ร่วมกับ
- Mounting Bracket for Servo SG90
- Switching Power supply แหล่งจ่ายไฟ 5 V 5.5 A
- Switching Power supply แหล่งจ่ายไฟ 5 V 10 A
- Switching Power supply แหล่งจ่ายไฟ 5 V 20 A
ตัวอย่างการใช้งาน
- Servo -> Arduino
- สายสีน้ำตาล -> GND
- สายสีแดง ->5V
- สายสีส้ม -> Pin 8
HC-SR04
หลักการทำงาน
HC-SR04 เป็นเซนเซอร์โมดูลสำหรับตรวจจับวัตถุและวัดระยะทางแบบไม่สัมผัส [1-2] โดยใช้คลื่นอัลตราโซนิก ซึ่งเป็นคลื่นเสียงความถี่สูงเกินกว่าการได้ยินของมนุษย์ วัดระยะได้ตั้งแต่ 2 – 400 เซนติเมตร หรือ 1 – 156 นิ้ว สามารถต่อใช้งานกับไมโครคอนโทรลเลอร์ได้ง่าย ใช้พลังงานต่ำ เหมาะกับการนำไปประยุกต์ใช้งานด้านระบบควบคุมอัตโนมัติ หรืองานด้านหุ่นยนต์ หลักการทำงาน จะเหมือนกันกับการตรวจจับวัตถุด้วยเสียงของค้างคาว ตามรูปที่ 1 โดยจะประกอบไปด้วยตัว รับ-ส่ง อัลตราโซนิก ตัวส่งจะส่งคลื่นความถี่ 40 kHz ออกไปในอากาศด้วยความเร็วประมาณ 346 เมตรต่อวินาที และตัวรับจะคอยรับสัญญาณที่สะท้อนกลับจากวัตถุ เมื่อทราบความเร็วในการเคลื่อนที่ของคลื่น, เวลาที่ใช้ในการเดินทางไป-กลับ (t) ก็จะสามารถคำนวณหาระยะห่างของวัตถุ (S) ได้จาก S = 346 × 0.5t
รุ่น HC-SR04 เป็นรุ่นที่ถูกนิยมใช้งานมากที่สุด การสื่อสารกับไมโครคอนโทรลเลอร์จะใช้วิธีทริกสัญญาณ ขาใช้งานจะมี 4 ขา คือขา VCC Trig Echo และ GND ในรุ่นนี้รองรับแรงดันไฟเลี้ยงที่ 5V กรณีที่แรงดันไฟเลี้ยงน้อยกว่า 5V โมดูลจะไม่สามารถทำงานได้ตามปกติ
รูปที่ 1 หลักการตรวจจับและวัดระยะห่างระหว่างวัตถุด้วยคลื่นเสียง
เพื่อให้การคำนวณหาระยะเป็นไปด้วยความง่าย โมดูลเซนเซอร์นี้จึงได้ประมวลผลให้เรียบร้อยแล้ว และส่งผลลัพธ์ของการคำนวณเป็นสัญญาณพัลส์ที่มีความกว้างสัมพันธ์กับระยะทางที่วัดได้
การต่อใช้งานโมดูล
โมดูลนี้มีจุดต่อใช้งานทั้งหมด 4 จุด การใช้งานบอร์ด Arduino การทดลองในเบื้องต้นสามารถต่อวงจรอย่างง่ายได้โดยใช้โปรโตบอร์ดและสายไฟต่อวงจรตามรูปที่ 2 ทั้งนี้ต้องตรวจสอบคุณสมบัติของพอร์ตของไมโครคอนโทรลเลอร์จากดาต้าชีท [3] ว่าสามารถทนระดับแรงดันลอจิก High (5V) ได้
- ขา VCC สำหรับต่อแรงดันไฟเลี้ยงไม่เกิน 5V
- ขา Trig เป็นขาอินพุตรับสัญญาณพัลส์ความกว้าง 10 ไมโครวินาทีเพื่อกระตุ้นการสร้างคลื่นอัลตราโซนิกความถี่ 40KHz ออกสู่อากาศจากตัวส่ง
- ขา Echo เป็นขาเอาต์พุตสำหรับส่งสัญญาณพัลส์ออกจากโมดูลไปยังไมโครคอนโทรลเลอร์ เพื่อตรวจจับความกว้างของสัญญาณพัลส์และคำนวณเป็นระยะทาง
- ขา GND สำหรับต่อจุดกราวด์ร่วมแรงดันและสัญญาณ
รูปที่ 2 การต่อใช้งานโมดูลอัลตราโซนิก HC-SR04
ในการสื่อสารข้อมูลกับโมดูล HC-SR04 ใช้ขาสัญญาณ 2 ขา คือ Trigger และ Echo โดยขา Trigger มีไว้สำหรับสั่งงานให้โมดูล HC-SR04 ส่งคลื่นอัลตร้าโซนิกออกไปข้างหน้า เมื่อคลื่นอัลตร้าโซนิกสะท้อนกลับมาจากวัตถุเป้าหมาย จะส่งสัญญาณพัลส์ออกมาทางขา Echo โดยสัญญาณนี้จะมีความกว้างที่สัมพันธ์กับระยะทางที่วัดได้ ดังนั้น ไมโครคอนโทรลเลอร์จะต้องส่งสัญญาณพัลส์ที่มีความกว้างพัลส์อย่างน้อย 10 ไมโครวินาทีไปยังขา Trigger ของโมดูล HC-SR04 แล้วรอรับสัญญาณพัลส์ที่ส่งกลับมาทางขา Echo เพื่อวัดความกว้างของสัญญาณพัลส์
ตามคุณลักษณะของเซนเซอร์ จะต้องสร้างสัญญาณพัลส์ความกว้างไม่น้อยกว่า 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
ระยะทาง (inch) = ความกว้างของสัญญาณ Echo * 106 /148
อ้างอิงข้อมูลจาก : http://www.myarduino.net/product/29/sg90-servo-motor-มาตรฐาน
https://www.arduitronics.com/product/283/tower-pro-sg90-mini-micro-servo
http://www.thaieasyelec.com/article-wiki/review-product-article/บทความตัวอย่างการควบคุม-rc-servo-motor-ด้วย-arduino.html
http://aimagin.com/blog/อัลตราโซนิก-hc-sr04/?lang=th
https://www.arduitronics.com/product/283/tower-pro-sg90-mini-micro-servo
http://www.thaieasyelec.com/article-wiki/review-product-article/บทความตัวอย่างการควบคุม-rc-servo-motor-ด้วย-arduino.html
http://aimagin.com/blog/อัลตราโซนิก-hc-sr04/?lang=th
อุปกรณ์ที่ใช้
1.บอร์ด Arduino 1 ตัว
2.สาย USB 1 เส้น
3.โฟโต้บอร์ด 1 อัน
4.Servo Motor (SG90) 1 ตัว
5.HC-SR04 1 ตัว
3.โฟโต้บอร์ด 1 อัน
4.Servo Motor (SG90) 1 ตัว
5.HC-SR04 1 ตัว
6.สายไฟผู้-ผู้ 8 เส้น
7.โปรแกรม Arduino
8.PC / NoteBook
8.PC / NoteBook
รูปการต่อวงจร (Fritzing)
คลิก Download เพื่อดาวน์โหลดรูปไปแก้เพิ่มเติม แต่ทั้งนี้ต้องมีโปรแกรม Fritzing อยู่ในเครื่องคอม ฯ ของเราก่อนนะคะ หากไม่มีสามารถโหลดได้ที่ DownloadProgramFritzing
Code และคำอธิบายการทำงานของวงจรในโปรแกรม Arduino
การควบคุม Servo Motor (SG90) กับเซนเซอร์วัดระยะทาง HC-SR04 ด้วยโปรแกรม Arduino ให้ Servo
motor (SG90) กับเซนเซอร์วัดระยะทาง HC-SR04 ให้ทำงานดังนี้
1.ใช้เซนเซอร์วัดระยะทาง HC-SR04 วัดระยะทาง โดยให้แสดงผลบนจอ Serial Monitor เป็นนิ้วและเป็นเซนติเมตร และเมื่อ...
- เซนเซอร์วัดระยะทาง HC-SR04 วัดระยะทางได้ 1 นิ้ว ให้ Servo motor (SG90) ชี้ที่เลข 1
- เซนเซอร์วัดระยะทาง HC-SR04 วัดระยะทางได้ 2 นิ้ว ให้ Servo motor (SG90) ชี้ที่เลข 2
- เซนเซอร์วัดระยะทาง HC-SR04 วัดระยะทางได้ 3 นิ้ว ให้ Servo motor (SG90) ชี้ที่เลข 3
- เซนเซอร์วัดระยะทาง HC-SR04 วัดระยะทางได้ 4 นิ้ว ให้ Servo motor (SG90) ชี้ที่เลข 4
- เซนเซอร์วัดระยะทาง HC-SR04 วัดระยะทางได้ 5 นิ้ว ให้ Servo motor (SG90) ชี้ที่เลข 5
- เซนเซอร์วัดระยะทาง HC-SR04 วัดระยะทางได้ 6 นิ้ว ให้ Servo motor (SG90) ชี้ที่เลข 6
- เซนเซอร์วัดระยะทาง HC-SR04 วัดระยะทางได้ 7 นิ้ว ให้ Servo motor (SG90) ชี้ที่เลข 7
2.ให้Serial Monitor แสดงผลมุมของ Servo (Angle)
#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*2.54);
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.
}
คลิก DownloadCode เพื่อนำไปแก้ไขเพิ่มเติม แต่ทั้งนี้ต้องมีโปรแกรม Arduino อยู่ในเครื่องคอม ฯ
ของเราก่อนนะคะ หากไม่มีสามารถโหลดได้ที่ DownloadArduino
