[TH] ESP-IDF Ep.10 : Control the Servo Motor with LEDC.

jarutex ESP-IDF, esp32, esp32s2, Thai

บทความนี้กล่าวถึงการใช้งานโมดูลเซอร์โวมอเตอร์ด้วยการใช้ GPIO ของ ESP32 ที่นำออกสัญญาณดิจิทัลแบบ PWM หรือ Pulse Width Modulation หรือ LEDC (LED Control) ซึ่งทำให้สามารถสร้างคลื่นความถี่ หรือปรับสัดส่วนของสถานะ 1 และ 0 ใน 1 ลูกคลื่น ที่มีความถี่ 50Hz โดยใช้บอร์ดทดลองดังภาพที่ 1

ภาพที่ 1 การต่อใช้งานประกอบตัวอย่างการใช้งาน LEDC

โครงสร้างของโครงงาน

โครงสร้างของโครงงานของ ESP-IDF เป็นดังภาพที่ 2 คือ ในไดเร็กทอรีหรือโฟลเดอร์ของโครงงานจะมีไฟล์ CMakeList.txt และ sdkconfig กับไดเร็กทอรีชื่อ main สำหรับเก็บรหัสต้นฉบับของโครงงาน โดยในไดเร็กทอรีดังกล่าวมีไฟล์ภาษา C และ CMakeLists.txt

ภาพที่ 2 โครงสร้างของโครงงาน

จากโครงสร้างในภาพที่ 2 ต้องสร้างโค้ดของไฟล์ CMakeLists.txt ดังนี้ ซึ่งเนื้อหาในโค้ดได้กำหนดรุ่นขั้นต่ำของโปรแกรม cmake และกำหนดค่าการใช้งานของ cmake เบื้องต้นตามจ้นฉบับที่มากับ ESP-IDF พร้อมทั้งตั้งชื่อโครงงานเป็น ep10

cmake_minimum_required(VERSION 3.5)
include($ENV{IDF_PATH}/tools/cmake/project.cmake)
project(ep10)

สิ่งที่เขียนในไฟล์ main/CMakeLists.txt เป็นดังต่อไปนี้ เพื่อกำหนดรายการไฟล์ที่จะต้องคอมไพล์ ซึ่งกำหนดไว้เป็น main.c และกำหนดไดเร็กทอรีที่เก็บไฟล์ส่วนหัวเอาไว้เป็นค่าว่างซึ่งหมายถึงที่เดียวกับ main.c หรือในไดเร็กทอรี main

idf_component_register(SRCS "main.c"
                    INCLUDE_DIRS "")

เมื่อสร้างโครงสร้างได้เหมือนดังภาพที่ 2 ให้สั่งเลือก target ของระบบเป็น ESP32 ดังนี้

idf.py set-target esp32

ส่วน sdkconfig เกิดจากการเรียกใช้คำสั่งต่อไปนี้ idf.py menuconfig

idf.py menuconfig

จากหน้าจอกำหนดการตั้งค่าให้เข้าไปที่ Component Config –> FreeRTOS และกำหนด Tick rate (Hz) เป็น 1000 ดังภาพที่ 3 หลังจากนั้นบันทึกและออกจากการตั้งค่า

FreeOS Tick Rate settings
ภาพที่ 3 ตั้งค่า Tick rate (Hz)

ที่มักจะลืมกันคือตั้งค่าขนาดความจุของหน่วยความจำรอม (Flash size) ดังภาพที่ 4 ให้ตรงกับขนาดที่ติดตั้งบนบอร์ด จากเสนู Serial flasher config ซึ่งในบทความใช้เป็น 4MB หลังจากนั้นกด S และ Q เพื่อบันทึกและออกจากการตั้งค่า

Serial Flasher size settings.
ภาพที่ 4 กำหนดขนาด flash size เป็น 4MB

เซอร์โวมอเตอร์

เซอร์โวมอเตอร์เป็นมอเตอร์ไฟฟ้ากระแสตรงขนาดเล็ก ในชุดประกอบด้วยมอเตอร์ไฟฟ้ากระแสตรงและชุดเฟืองขับ ทำให้ใช้กระแสไม่มากแต่ได้แรงขับ (Torque) สูง และมีน้ำหนักเบา โดยการควบคุมมอเตอร์ไฟฟ้าประเภทนี้สามารถสั่งให้มอเตอร์หมุนไปยังองศาทางซ้ายสุด หรือขวาสุดของมอเตอร์ ซึ่งแต่ละตัวมีค่าไม่เท่ากัน ดังนั้น เราจึงสามารถสั่งให้มอเตอร์หมุนไปที่ 0 องศา 90 องศา และ 180 องศาได้ดังภาพที่ 5, 6 และ 7 ตามลำดับ

ภาพที่ 5 ตัวอย่างการหมุนไปที่ 0 องศา
ภาพที่ 6 ตัวอย่างการหมุนไปที่ 90 องศา
ภาพที่ 7 ตัวอย่างการหมุนไปที่ 180 องศา

การเชื่อมต่อสายจากเซอร์โวมอเตอร์เข้ากับบอร์ดทดลองในภาพที่ 1 กระทำดังนี้

  • สายสีดำของมอเตอร์ ต่อเข้า GND ของระบบ
  • สายสีแดงของมอเตอร์ต่อเข้ากับ 3V3 ของบอร์ด ESP32
  • สายสีส้มซึ่งมีหน้าที่ส่งสัญญาณของมอเตอร์ให้ต่อเข้ากับขา 22 ของบอร์ด ESP32

การใช้ LEDC หรือ PWM กับเซอร์โวมอเตอร์์ต้องกำหนดให้ความถี่ของคลื่นเท่ากัย 50Hz จึงต้องกำหนดค่าคอนฟิกดังนี้

  ledc_timer_config_t ledc_timer = {
        .speed_mode       = LEDC_LOW_SPEED_MODE,
        .timer_num        = LEDC_TIMER_0,
        .duty_resolution  = LEDC_TIMER_13_BIT,
        .freq_hz          = 50,  
        .clk_cfg          = LEDC_AUTO_CLK
  };

และกำหนดค่า duty ในคำสั่ง ledc_set_duty() และสั่งให้ทำงานด้วยคำสั่ง ledc_update_duty() ดังนี้

  ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, ค่าดิวตี้);
  ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0);

ส่วนการคำนวณค่า duty ซึ่งเป็นค่าตัวเลขจำนวนเต็มที่ใช้แทนค่าร้อยละของเป็นสถานะแรงดัน 1 ต่อ 1 ลูกคลื่น โดยมีค่าขนาด 13 บิต (ตามที่กำหนดด้วย duty_resolution) หรือตัวเลขในช่วง 0 ถึง 213 (หรือตัวเลขฐานสิบ 8,191)

การเคลื่อนที่ของมอเตอร์โดยปกติจะถูกล็อกให้สามารถหมุนไปทางซ้ายสุดและขวาสุดได้ ซึ่งแต่ละตัวมีค่าแตกต่างกัน แต่สามารถแก้ไขให้สามารถหมุนได้ 360 องศาได้เช่นกัน แต่อย่างไรก็ดี ในบทความนี้ขอกล่าวถึงการควบคุมมอเตอร์ที่หมุนแบบปกติ จึงได้ว่า การสั่งงานจึงมีการสั่งด้วยค่าดิวตี้ที่น้อยสุดทำให้หมุนไปทางซ้ายสุดและมากสุดไปทางขวาสุด ด้วยเหตุนี้ในโปรแกรมจึงกำหนดให้มีค่าคงที่ 3 ตัว คือ ค่าที่ 0 องศา 180 องศา และ90 องศา (ตรงกลางระหว่าง 0 กับ 180 องศา) เอาไว้ดังนี้

  • ServoMsMin ค่าน้อยสุดที่ทำให้หมุนไปที่ 0 องศา
  • ServoMsMax ค่ามากสุดที่ทำให้หมุนไปที่ 180 องศา
  • ServoMsAvg ((ServoMsMax-ServoMsMin)/2.0)

ด้วยต้องกำหนดให้มีความถี่ในการส่งสัญญาณเป็น 50Hz ทำให้แต่ละลูกคลื่นมีความยาวหรือคาบเท่ากับค่าดังนี้

T = 1/f

= 1/50

ดังนั้น จึงได้ว่า T มีค่าเป็น 20ms

เมื่อเราทราบค่า ServoMsMin, ServoMsAvg และ ServoMsMax ซึ่งมีค่าเป็นมิลลิวินาที โดยมองทั้ง 3 ค่าเป็น t1 หรือช่วงเวลาที่มีสถานะเป็น 1 จะได้ว่า

T = t0+t1

t0 = T-t1

และด้วย T ที่ 0% มีค่าเป็น 0 และ 100% มีค่าเป็น 8191 เราจึงได้ว่า แต่ละ 1% มีค่าเท่ากับ 81.91 หน้าที่ของเราคือต้องหาว่า t1 คิดเป็นร้อยละเท่าไรของ T ดังนี้

ร้อยละของ t1 = 100*t1/20

จากการหาค่าจึงคำนวณค่า duty ได้ดังนี้

int duty = (int)(100.0*(ค่าของt1/20.0)*81.91);

กรณีที่ต้องการใช้งาน GPIO แบบอื่น ๆ สามารถเข้าไปอ่านบทความต่าง ๆ ดังต่อไปนี้เพิ่มเติม

  1. นำออกข้อมูลสัญญาณดิจิทัล
  2. นำเข้าสัญญาณดิจิทัล
  3. นำเข้าสัญญาณแอนาล็อก
  4. นำออกสัญญาณแอนาล็อก (ตอน 2)
  5. การใช้ PWM หรือ LEDC

ตัวอย่างโปรแกรม

ตัวอย่างโปรแกรมสร้างคลื่นความถี่ 50Hz เพื่อส่งให้เซอร์โวมอเตอร์ที่เชื่อมต่อกับขา GPIO22 โดยทำการส่งค่าดิวตี้สำหรับสั่งให้ขยับที่ 0 องศา, 90 องศา, 180 องศา และ 90 องศาไปให้ และหยุด 2 วินาทีในทุกครั้งที่ส่ง พร้อมทั้งวนรอบไปเรื่อย ๆ ส่งผลให้มอเตอร์หมุนไปที่ 0, 90, 180 และ 90 องศาวนไป พร้อมแสดงผลลัพธ์ดังภาพที่ 8 ซึ่งโค้ดทั้งหมดเขียนได้ดังนี้

#include <stdio.h>
#include <time.h>
#include <string.h>
#include <math.h>
#include <sdkconfig.h>
#include <driver/gpio.h>
#include <driver/ledc.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>

#define pinServo 22
#define ServoMsMin 0.06
#define ServoMsMax 2.1
#define ServoMsAvg ((ServoMsMax-ServoMsMin)/2.0)

void servoDeg0() {
  ledc_timer_config_t ledc_timer = {
        .speed_mode       = LEDC_LOW_SPEED_MODE,
        .timer_num        = LEDC_TIMER_0,
        .duty_resolution  = LEDC_TIMER_13_BIT,
        .freq_hz          = 50,  
        .clk_cfg          = LEDC_AUTO_CLK
  };
  ledc_timer_config(&ledc_timer);
  ledc_channel_config_t ledc_channel = {
        .speed_mode     = LEDC_LOW_SPEED_MODE,
        .channel        = LEDC_CHANNEL_0,
        .timer_sel      = LEDC_TIMER_0,
        .intr_type      = LEDC_INTR_DISABLE,
        .gpio_num       = pinServo,
        .duty           = 0,
        .hpoint         = 0
  };
  ledc_channel_config(&ledc_channel);  
  int duty = (int)(100.0*(ServoMsMin/20.0)*81.91);
  printf("%fms, duty = %f%% -> %d\n",ServoMsMin, 100.0*(ServoMsMin/20.0), duty);
  ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, duty);
  ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0);  
  vTaskDelay( 2000/portTICK_PERIOD_MS ); 
  ledc_stop(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, 0);
}

void servoDeg90() {
  ledc_timer_config_t ledc_timer = {
        .speed_mode       = LEDC_LOW_SPEED_MODE,
        .timer_num        = LEDC_TIMER_0,
        .duty_resolution  = LEDC_TIMER_13_BIT,
        .freq_hz          = 50,  
        .clk_cfg          = LEDC_AUTO_CLK
  };
  ledc_timer_config(&ledc_timer);
  ledc_channel_config_t ledc_channel = {
        .speed_mode     = LEDC_LOW_SPEED_MODE,
        .channel        = LEDC_CHANNEL_0,
        .timer_sel      = LEDC_TIMER_0,
        .intr_type      = LEDC_INTR_DISABLE,
        .gpio_num       = pinServo,
        .duty           = 0,
        .hpoint         = 0
  };
  ledc_channel_config(&ledc_channel);  
  int duty = (int)(100.0*(ServoMsAvg/20.0)*81.91);
  printf("%fms, duty = %f%% -> %d\n",ServoMsAvg, 100.0*(ServoMsAvg/20.0), duty);
  ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, duty);
  ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0);  
  vTaskDelay( 2000/portTICK_PERIOD_MS ); 
  ledc_stop(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, 0);
}

void servoDeg180() {
  ledc_timer_config_t ledc_timer = {
        .speed_mode       = LEDC_LOW_SPEED_MODE,
        .timer_num        = LEDC_TIMER_0,
        .duty_resolution  = LEDC_TIMER_13_BIT,
        .freq_hz          = 50,  
        .clk_cfg          = LEDC_AUTO_CLK
  };
  ledc_timer_config(&ledc_timer);
  ledc_channel_config_t ledc_channel = {
        .speed_mode     = LEDC_LOW_SPEED_MODE,
        .channel        = LEDC_CHANNEL_0,
        .timer_sel      = LEDC_TIMER_0,
        .intr_type      = LEDC_INTR_DISABLE,
        .gpio_num       = pinServo,
        .duty           = 0, 
        .hpoint         = 0
  };
  ledc_channel_config(&ledc_channel);  
  int duty = (int)(100.0*(ServoMsMax/20.0)*81.91);
  printf("%fms, duty = %f%% -> %d\n",ServoMsMax, 100.0*(ServoMsMax/20.0), duty);
  ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, duty);
  ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0);  
  vTaskDelay( 2000/portTICK_PERIOD_MS ); 
  ledc_stop(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, 0);
}

void manualServo() {
  printf("  0 degree:");
  servoDeg0();
  printf(" 90 degree:");
  servoDeg90();
  printf("180 degree:");
  servoDeg180();
  printf(" 90 degree:");
  servoDeg90();
}

void app_main(void)
{ 
  printf("EP10 : LEDC&Servo Motor\n");
  while (1) {
    manualServo();
  }
}

คอมไพล์และอัพโหลด

ทำการคอมไพล์ หลังจากนั้น flash ลงชิพ และเข้าโปรแกรม Serial Monitor สั่งงานดังนี้

idf.py -p /dev/ttyUSB0 build flash monitor

ตัวอย่างผลลัพธ์ของโปรแกรมเป็นดังภาพที่ 8

ภาพที่ 8 ผลลัพธ์จากโปรแกรม ep10

สรุป

จากบทความนี้จะพบว่า การการใช้ PWM หรือ LEDC นั้นประกอบด้วย 3 ขั้นตอน คือ

  1. ตั้งค่าตัวตั้งเวลา ด้วย ledc_timer_config()
  2. ตั้งค่าช่องสัญญาณ ledc_channel_config()
  3. สั่งงาน ledc_set_duty(), ledc_update_duty() และ ledc_stop()

แต่อย่างไรก็ดี การนำตัวอย่างนี้ไปใช้จะต้องทำการเปลี่ยนแปลงค่า ServoMsMin และ ServoMsMax ให้เหมาะสมเพื่อให้มอเตอร์หมุนไปยังตำแหน่งที่ถูกต้องดังภาพตัวอย่างที่ 5, 6 และ 7 ซึ่งจากบทความทั้ง 2 ตอนจะพบว่า เราสามารถใช้ LEDC หรือ PWM ควบคุมได้ทั้งการหรี่/เร่งความสว่างของหลอด การส่งคลื่นความถี่ไปที่ลำโพง และควบคุมทิศทางของเซอร์โวมอเตอร์ด้วยการใช้คำสั่งที่เหมือนกัน แต่ต้องประยุกต์วิธีที่แตกต่างกันตามอุปกรณ์ที่ต่อพ่วงด้วย ดังนั้น การฝึกเขียนและทำความเข้าใจกับอุปกรณ์จึงเป้นสิ่งสำคัญในการนำไปใช้จริง และสุดท้ายขอให้สนุกกับการเขียนโปรแกรมครับ

ท่านใดต้องการพูดคุยสามารถคอมเมนท์ได้เลยครับ

แหล่งอ้างอิง

  1. WiKiPedia : Pulse-width modulation
  2. ESP-IDF : LEDC

(C) 2020-2021, โดย อ.ดนัย เจษฎาฐิติกุล/อ.จารุต บุศราทิจ
ปรับปรุงเมื่อ 2021-10-16, 2021-12-29