นางสาวพรรณพิลาศ ศรีสมศักดิ์ 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
หลักการทำงาน
การต่อใช้งานโมดูล
การต่อใช้งานโมดูล
- ขา VCC สำหรับต่อแรงดันไฟเลี้ยงไม่เกิน 5V
- ขา Trig เป็นขาอินพุตรับสัญญาณพัลส์ความกว้าง 10 ไมโครวินาทีเพื่อกระตุ้นการสร้างคลื่นอัลตราโซนิกความถี่ 40KHz ออกสู่อากาศจากตัวส่ง
- ขา Echo เป็นขาเอาต์พุตสำหรับส่งสัญญาณพัลส์ออกจากโมดูลไปยังไมโครคอนโทรลเลอร์ เพื่อตรวจจับความกว้างของสัญญาณพัลส์และคำนวณเป็นระยะทาง
- ขา 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. HC-SR04
2. Servo Motor (SG90) 1 ตัว
3. บอร์ด Arduino 1 บอร์ด
4. สายไฟ ผู้ - ผู้
5. สายไฟ ผู้ - เมีย
6. สายอัปโหลด
7. วัตถุ
รูปวงจร
วงจร Arduino Radar System เป็นวงจรการทำ Arduino radar โดยใช้ servo และ hc-sr04 เป็นตัววัดระยะว่ามีสิ่งของอยู่ในระยะที่เรดาร์อยู่หรือไม่ โดยแปรแกรมเรดาร์จะใช้โปรแกรม processing 3
Code
- ใช้กับโปรแกรม Arduion
// Includes the Servo library
#include <Servo.h>.
// Defines Tirg and Echo pins of the Ultrasonic Sensor
const int trigPin = 10;
const int echoPin = 11;
// Variables for the duration and the distance
long duration;
int distance;
Servo myServo; // Creates a servo object for controlling the servo motor
void setup() {
pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
pinMode(echoPin, INPUT); // Sets the echoPin as an Input
Serial.begin(9600);
myServo.attach(12); // Defines on which pin is the servo motor attached
}
void loop() {
// rotates the servo motor from 15 to 165 degrees
for(int i=15;i<=165;i++){
myServo.write(i);
delay(30);
distance = calculateDistance();// Calls a function for calculating the distance measured by the Ultrasonic sensor for each degree
Serial.print(i); // Sends the current degree into the Serial Port
Serial.print(","); // Sends addition character right next to the previous value needed later in the Processing IDE for indexing
Serial.print(distance); // Sends the distance value into the Serial Port
Serial.print("."); // Sends addition character right next to the previous value needed later in the Processing IDE for indexing
}
// Repeats the previous lines from 165 to 15 degrees
for(int i=165;i>15;i--){
myServo.write(i);
delay(30);
distance = calculateDistance();
Serial.print(i);
Serial.print(",");
Serial.print(distance);
Serial.print(".");
}
}
// Function for calculating the distance measured by the Ultrasonic sensor
int calculateDistance(){
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
// Sets the trigPin on HIGH state for 10 micro seconds
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH); // Reads the echoPin, returns the sound wave travel time in microseconds
distance= duration*0.034/2;
return distance;
}
ดาวน์โหลดโค้ด Arduino ที่นี่
- ใช้กับโปรแกรม processing 3 (Radar)
import processing.serial.*; // imports library for serial communication
import java.awt.event.KeyEvent; // imports library for reading the data from the serial port
import java.io.IOException;
Serial myPort; // defines Object Serial
// defubes variables
String angle="";
String distance="";
String data="";
String noObject;
float pixsDistance;
int iAngle, iDistance;
int index1=0;
int index2=0;
PFont orcFont;
void setup() {
size (1200, 700); // ***CHANGE THIS TO YOUR SCREEN RESOLUTION***
smooth();
myPort = new Serial(this,"COM5", 9600); // starts the serial communication
myPort.bufferUntil('.'); // reads the data from the serial port up to the character '.'. So actually it reads this: angle,distance.
}
void draw() {
fill(98,245,31);
// simulating motion blur and slow fade of the moving line
noStroke();
fill(0,4);
rect(0, 0, width, height-height*0.065);
fill(98,245,31); // green color
// calls the functions for drawing the radar
drawRadar();
drawLine();
drawObject();
drawText();
}
void serialEvent (Serial myPort) { // starts reading data from the Serial Port
// reads the data from the Serial Port up to the character '.' and puts it into the String variable "data".
data = myPort.readStringUntil('.');
data = data.substring(0,data.length()-1);
index1 = data.indexOf(","); // find the character ',' and puts it into the variable "index1"
angle= data.substring(0, index1); // read the data from position "0" to position of the variable index1 or thats the value of the angle the Arduino Board sent into the Serial Port
distance= data.substring(index1+1, data.length()); // read the data from position "index1" to the end of the data pr thats the value of the distance
// converts the String variables into Integer
iAngle = int(angle);
iDistance = int(distance);
}
void drawRadar() {
pushMatrix();
translate(width/2,height-height*0.074); // moves the starting coordinats to new location
noFill();
strokeWeight(2);
stroke(98,245,31);
// draws the arc lines
arc(0,0,(width-width*0.0625),(width-width*0.0625),PI,TWO_PI);
arc(0,0,(width-width*0.27),(width-width*0.27),PI,TWO_PI);
arc(0,0,(width-width*0.479),(width-width*0.479),PI,TWO_PI);
arc(0,0,(width-width*0.687),(width-width*0.687),PI,TWO_PI);
// draws the angle lines
line(-width/2,0,width/2,0);
line(0,0,(-width/2)*cos(radians(30)),(-width/2)*sin(radians(30)));
line(0,0,(-width/2)*cos(radians(60)),(-width/2)*sin(radians(60)));
line(0,0,(-width/2)*cos(radians(90)),(-width/2)*sin(radians(90)));
line(0,0,(-width/2)*cos(radians(120)),(-width/2)*sin(radians(120)));
line(0,0,(-width/2)*cos(radians(150)),(-width/2)*sin(radians(150)));
line((-width/2)*cos(radians(30)),0,width/2,0);
popMatrix();
}
void drawObject() {
pushMatrix();
translate(width/2,height-height*0.074); // moves the starting coordinats to new location
strokeWeight(9);
stroke(255,10,10); // red color
pixsDistance = iDistance*((height-height*0.1666)*0.025); // covers the distance from the sensor from cm to pixels
// limiting the range to 40 cms
if(iDistance<40){
// draws the object according to the angle and the distance
line(pixsDistance*cos(radians(iAngle)),-pixsDistance*sin(radians(iAngle)),(width-width*0.505)*cos(radians(iAngle)),-(width-width*0.505)*sin(radians(iAngle)));
}
popMatrix();
}
void drawLine() {
pushMatrix();
strokeWeight(9);
stroke(30,250,60);
translate(width/2,height-height*0.074); // moves the starting coordinats to new location
line(0,0,(height-height*0.12)*cos(radians(iAngle)),-(height-height*0.12)*sin(radians(iAngle))); // draws the line according to the angle
popMatrix();
}
void drawText() { // draws the texts on the screen
pushMatrix();
if(iDistance>40) {
noObject = "Out of Range";
}
else {
noObject = "In Range";
}
fill(0,0,0);
noStroke();
rect(0, height-height*0.0648, width, height);
fill(98,245,31);
textSize(25);
text("10cm",width-width*0.3854,height-height*0.0833);
text("20cm",width-width*0.281,height-height*0.0833);
text("30cm",width-width*0.177,height-height*0.0833);
text("40cm",width-width*0.0729,height-height*0.0833);
textSize(40);
text(" VIRAL SCIENCE ", width-width*0.875, height-height*0.0277);
text("Angle: " + iAngle +" ฐ", width-width*0.48, height-height*0.0277);
text("Distance: ", width-width*0.26, height-height*0.0277);
if(iDistance<40) {
text(" " + iDistance +" cm", width-width*0.225, height-height*0.0277);
}
textSize(25);
fill(98,245,60);
translate((width-width*0.4994)+width/2*cos(radians(30)),(height-height*0.0907)-width/2*sin(radians(30)));
rotate(-radians(-60));
text("30ฐ",0,0);
resetMatrix();
translate((width-width*0.503)+width/2*cos(radians(60)),(height-height*0.0888)-width/2*sin(radians(60)));
rotate(-radians(-30));
text("60ฐ",0,0);
resetMatrix();
translate((width-width*0.507)+width/2*cos(radians(90)),(height-height*0.0833)-width/2*sin(radians(90)));
rotate(radians(0));
text("90ฐ",0,0);
resetMatrix();
translate(width-width*0.513+width/2*cos(radians(120)),(height-height*0.07129)-width/2*sin(radians(120)));
rotate(radians(-30));
text("120ฐ",0,0);
resetMatrix();
translate((width-width*0.5104)+width/2*cos(radians(150)),(height-height*0.0574)-width/2*sin(radians(150)));
rotate(radians(-60));
text("150ฐ",0,0);
popMatrix();
}
ดาวน์โหลดโค้ด Radar ที่นี่
