ควบคุม Servo Motor ภาษาซี
เซอร์โวมอเตอร์ (Servo Motor) เป็นการรวมมอเตอร์ไฟฟ้ากระแสตรง (DC Motor) เข้ากับวงจรควบคุม โดยความแตกต่างที่สำคัญของเซอร์โวมอเตอร์กับมอเตอร์แบบอื่น ๆ คือเซอร์โวมอเตอร์จะรู้ตำแหน่งที่ตัวเองอยู่ และสั่งเปลี่ยนตำแหน่งโดยการเปลี่ยนองศาได้ นิยมใช้งานในเครื่องบินบังคับ เรือบังคับ โดยใช้กำหนดทิศทางของหางเสือเป็นองศา
การทํางานเพียงตัว Servo Motor เพียงอย่างเดียวนั้นไม่สามารถทํางานได้ การที่จะให้ Servo Motor จะควบคุมลักษณะที่กล่าวมาข้างต้นนั้นต้องมี ไมโครคอนโทรลเลอ มีหน้าที่รับคําสั่งจากผู้ใช้งานว่าต้องการให้ Servo Motor นั้นเคลื่อนที่ด้วยความเร็วเท่าไหร่และระยะทาง ใกล้หรือไกลแค่ไหน หน้าที่ตรงจุดนี้จะเป็น ไมโครคอนโทรลเลอร์ จะเป็นตัวกําหนดให้กับตัว Servo Motor
ปกติแล้วเซอร์โวมอเตอร์ที่ยังไม่ได้รับการปรับแต่งใดๆ เซอร์โวมอเตอร์มาตรฐานจะมีมุมในการหมุนอยู่ระหว่าง 90 ถึง 180 องศา แล้วแต่ ผู้ผลิต แต่ที่นิยมมากที่สุดคือ 0 ถึง 180 องศา และในบางรุ่นของบางผู้ผลิตจะสามารถดัดแปลง ให้หมุนได้ครบ 360 องศาด้วย
ในบทความนี้เลือกใช้ MG996R ซึ่งเป็นเซอร์โวมอเตอร์ เฟืองทดเป็นแบบโลหะ หมุนได้ 0-180 องศา ให้แรงบิดสูงถึง 15 kg ใช้ไฟเลี้ยง 4.8V – 7.2V

MG996R มี 3 สาย คือ 1. สายสีแดง ต่อเข้ากับไฟเลี้ยง 4.8V – 7.2V 2. สายสีน้ำตาลต่อเข้า กับ GND 3. สายสีส้ม ต่อเข้ากับ สัญญาณควบคุม DC ในตัวอย่างจะเป็นขา PB1
เซอร์โวมอเตอร์ มาพร้อมกับการควบคุมฮาร์ดแวร์อิเล็กทรอนิกส์เพื่อหมุนไปยังมุมใดมุมหนึ่ง โดยทั่วไปสามารถหมุนได้ระหว่าง -90 องศาถึง +90 องศา

สัญญาณควบคุมเป็นคลื่นสี่เหลี่ยมที่มีแอมพลิจูด 5 V. โดยทั่วไปจะรับความถี่ 50 Hz การวางตำแหน่งมุมทำโดย High time จะแตกต่างกันไปตั้งแต่ 1 ms ถึง 2 ms

รายการอุปกรณ์
- 1. ชุดคิทต่อวงจร Minimum ATmega328P Circuit
- 2. Breadboard 700 Points SYB-120
- 3. Jumper (M2M) cable 10cm Male to Male
- 4. FT232RL FTDI USB To TTL Serial Converter Module
- 5. USB Male to Mini USB B 5pin Data Cable
- 6. Push Button Switch สวิตช์กดติดปล่อยดับ 2 ขา จำนวน 3 ตัว
- 7. เซอร์โวมอเตอร์ MG996R
ขั้นตอนการทํางาน
1 : ทดสอบโปรแกรมแรก กับ ATmega328P
โปรแกรมแรกของ การใช้งานไมโครคอนโทรลเลอร์มักจะเป็น Blink ไฟกะพริบ ซึ่งเป็นหนึ่งในโปรแกรมที่ง่ายที่สุดเท่าที่จะเป็นไปได้ในการเขียนภาษาโปรแกรมต่างๆ เพราะฉะนั้นโดยธรรมเนียมปฏิบัติแล้ว มักจะใช้ในการตรวจสอบว่าเขียนภาษาโปรแกรมได้ถูกต้องหรือระบบมีการประมวลผลที่ถูกต้อง และมักถูกใช้เป็นตัวอย่างที่ง่ายที่สุดในการแสดงผลลัพธ์ของการเขียนโปรแกรม โดยทำตามขั้นตอนลิงค์ด้านล่าง
2: เชื่อมต่อ เซอร์โวมอเตอร์ เข้ากับ วงจร ATmega328P
เชื่อมต่อ สวิตช์กดติดปล่อยดับ 2 ขา เข้ากับ ขา PC0 , PC1 และ PC2 ของ ATmega328P ตามรูปด้านล่าง

เชื่อมต่อ เซอร์โวมอเตอร์ เข้ากับ ATmega328P คือ 1. สายสีแดง ต่อเข้า VCC 2. สายสีน้ำตาลต่อเข้า กับ GND 3. สายสีส้ม ต่อเข้ากับ ขา PB1



3: เขียนโค้ด ควบคุมมอเตอร์ Servo Motor แบบ ไม่ใช้ PWM
ตัวอย่าง การเขียนโค้ดแบบ ใช้ระบบเลขฐานสิบหก โดยไม่ใช้ค่า PWM เป้าหมายคือ ถ้าสวิตช์ ตัวที่ 1 ถูก กด ให้เซอร์โวมอเตอร์ หมุนไปที่ – 90 องศา , ถ้า สวิตช์ ตัวที่ 2 ถูก กด ให้เซอร์โวมอเตอร์ หมุนไปที่ 0 องศา และ ถ้า สวิตช์ ตัวที่ 3 ถูก กด ให้เซอร์โวมอเตอร์ หมุนไปที่ 90 องศา
#define F_CPU 16000000UL
#include <avr/io.h>
#include "util/delay.h"
#define rotate__90 (PINC & 0x01)
#define rotate_0 (PINC & 0x02)
#define rotate_90 (PINC & 0x04)
void servoRotate(short angle)
{
switch(angle)
{
case -90:
PORTB = 0x02;
_delay_us(650);
PORTB = 0x00;
_delay_us(2000);
break;
case 0:
PORTB = 0x02;
_delay_us(1500);
PORTB = 0x00;
_delay_us(2000);
break;
case 90:
PORTB = 0x02;
_delay_us(2350);
PORTB = 0x00;
_delay_us(2000);
break;
default:
break;
}
}
int main(void)
{
DDRB = 0x02;
DDRC = 0xF8;
PORTC = 0x07;
while (1)
{
if(rotate__90 == 0) servoRotate(-90);
else if(rotate_0 == 0) servoRotate(0);
else if(rotate_90 == 0) servoRotate(90);
}
}
อธิบายโค้ด
ตารางเทียบการเขียนโค้ด เลื่อนบิตไปทางซ้าย | ใช้ระบบเลขฐานสอง | ใช้ระบบเลขฐานสิบหก
เลื่อนบิตไปทางซ้าย | ใช้ระบบเลขฐานสอง | ใช้ระบบเลขฐานสิบหก |
1 << 0 | 0B00000001 | 0x01 |
1 << 1 | 0B00000010 | 0x02 |
1 << 2 | 0B00000100 | 0x04 |
1 << 3 | 0B00001000 | 0x08 |
1 << 4 | 0B00010000 | 0x10 |
1 << 5 | 0B00100000 | 0x20 |
1 << 6 | 0B01000000 | 0x40 |
1 << 7 | 0B10000000 | 0x80 |
ดูรายละเอียดเพิ่มเติม : ASCII Character Table
#define คือ ค่าคงที่ และ ให้ชื่อว่า F_CPU กำหนดค่าให้เป็น 16000000 คือการกำหนดให้นำค่า 16MHz ไปไปคำนวณร่วมกับฟังก์ชั่น delay.h ในการหน่วงเวลาเพราะ วงจร Minimum ATmega328P Circuit ที่เราเชื่อมต่อนั้น ใช้คริสตัล 16Mhz เป็นแหล่งสัญญาณนาฬิกา
#define F_CPU 16000000UL
#include คือการเรียกใช้ไฟล์ภาษาซี จากไลบรารี ซึ่งอยู่ในโฟลเดอร์ avr และ ชื่อไฟล์ io.h สำหรับใช้งานเกี่ยวกับ ฟังก์ชั่น ควบคุมอินพุท/เอาท์พุท (เช่น PORTB, DDRB)
#include <avr/io.h>
#include คือการเรียกใช้ไฟล์ภาษาซี จากไลบรารี ซึ่งอยู่ในโฟลเดอร์ util และ ชื่อไฟล์ delay.h สำหรับใช้งานเกี่ยวกับ ฟังก์ชั่น การหน่วงเวลา
#include "util/delay.h"
4: ทดสอบการทํางาน
ถ้าสวิตช์ตัวที่ 1 ถูก กด ให้เซอร์โวมอเตอร์ หมุนไปที่ – 90 องศา , ถ้า สวิตช์ ตัวที่ 2 ถูก กด ให้เซอร์โวมอเตอร์ หมุนไปที่ 0 องศา และ ถ้า สวิตช์ ตัวที่ 3 ถูก กด ให้เซอร์โวมอเตอร์ หมุนไปที่ 90 องศา
5 : เขียนโค้ด ควบคุมมอเตอร์ Servo Motor แบบใช้ PWM
ตัวอย่าง การเขียนโค้ด แบบเลื่อนบิตไปทางซ้ายมือ โดยใช้ค่า PWM เป้าหมายคือ เมื่อเริ่มทำงาน ให้เซอร์โวมอเตอร์ หมุนไปที่ – 90 องศา แล้วค่อยๆหมุนไปตามเข็มนาฬิกา ไปยัง 0 องศา แล้วหมุนต่อไปยัง 90 องศา แล้วหยุด 5000 มิลลิเซก แล้วจึงหมุนกลับ ทวนเข็มนาฬิกา กลับไปยัง – 90 องศา ลักษณะการทำงาน วนลูป โดยที่ไม่ต้องกดสวิตช์ ใดๆ
#define F_CPU 16000000UL
#include <avr/io.h>
#include <util/delay.h>
int main(void) {
DDRB |= 1 << PINB1; // Set pin PB1 on ATmega328P to output
/* 1. Set Fast PWM mode 14: set WGM11, WGM12, WGM13 to 1 */
/* 3. Set pre-scaler of 8 */
/* 4. Set Fast PWM non-inverting mode */
TCCR1A |= (1 << WGM11) | (1 << COM1A1);
TCCR1B |= (1 << WGM12) | (1 << WGM13) | (1 << CS11);
/* 2. Set ICR1 register: PWM period */
ICR1 = 39999;
/* Offset for correction */
int offset = 800;
/* 5. Set duty cycle */
while(1) {
OCR1A = 3999 + offset;
_delay_ms(5000);
OCR1A = 1999 - offset;
_delay_ms(5000);
}
return 0;
}
<<< C7: ควบคุม DC Motor ด้วย L298N บทความก่อนหน้า | บทความต่อไป C9: ควบคุม Stepper Motor >>>