ควบคุม Servo Motor ภาษาซี

ควบคุม 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 เซอร์โวมอเตอร์


MG996R มี 3 สาย คือ 1. สายสีแดง ต่อเข้ากับไฟเลี้ยง 4.8V – 7.2V 2. สายสีน้ำตาลต่อเข้า กับ GND 3. สายสีส้ม ต่อเข้ากับ สัญญาณควบคุม DC ในตัวอย่างจะเป็นขา PB1


เซอร์โวมอเตอร์ มาพร้อมกับการควบคุมฮาร์ดแวร์อิเล็กทรอนิกส์เพื่อหมุนไปยังมุมใดมุมหนึ่ง โดยทั่วไปสามารถหมุนได้ระหว่าง -90 องศาถึง +90 องศา

ควบคุม Servo Motor ภาษาซี

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

ควบคุม Servo Motor ภาษาซี

 
รายการอุปกรณ์


ขั้นตอนการทํางาน

1 : ทดสอบโปรแกรมแรก กับ ATmega328P


โปรแกรมแรกของ การใช้งานไมโครคอนโทรลเลอร์มักจะเป็น Blink ไฟกะพริบ ซึ่งเป็นหนึ่งในโปรแกรมที่ง่ายที่สุดเท่าที่จะเป็นไปได้ในการเขียนภาษาโปรแกรมต่างๆ เพราะฉะนั้นโดยธรรมเนียมปฏิบัติแล้ว มักจะใช้ในการตรวจสอบว่าเขียนภาษาโปรแกรมได้ถูกต้องหรือระบบมีการประมวลผลที่ถูกต้อง และมักถูกใช้เป็นตัวอย่างที่ง่ายที่สุดในการแสดงผลลัพธ์ของการเขียนโปรแกรม โดยทำตามขั้นตอนลิงค์ด้านล่าง


2: เชื่อมต่อ​ เซอร์โว​มอเตอร์ เข้ากับ วงจร ATmega328P


เชื่อมต่อ สวิตช์กดติดปล่อยดับ 2 ขา เข้ากับ ขา PC0 , PC1 และ PC2 ของ ATmega328P ตามรูปด้านล่าง

ควบคุม Servo Motor ภาษาซี


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

ควบคุม Servo Motor ภาษาซี

ควบคุม Servo Motor ภาษาซี



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 << 00B000000010x01
1 << 10B000000100x02
1 << 20B000001000x04
1 << 30B000010000x08
1 << 40B000100000x10
1 << 50B001000000x20
1 << 60B010000000x40
1 << 70B100000000x80

ดูรายละเอียดเพิ่มเติม : 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 >>>