DC Motor Control Speed Kit : 2 Read Speed Motor With Sensor Encoder

ความเดิมตอนที่แล้ว ….. https://makerasia.com/dc-motor-control-speed-kit-1
สามารถซื้อชุดฝึกการเรียนรู้ควบคุมความเร็วรอบด้วย DC Motor Speed Control Kit (พร้อมคอร์สเรียนออนไลน์ฟรี) และรายละเอียดได้ที่ https://www.aiiotshop.com/product/61

ความเดิมตอนที่แล้ว… จาก DC Motor Control Speed Kit : 1 เราได้ศึกษาวิธีการกลับทิศทางการหมุน และปรับความเร็วรอบของมอเตอร์ด้วย PWM อย่างง่ายกันแล้วใช่ไหมครับ ซึ่งเราสามารถปรับความเร็วรอบของ DC Motor ให้หมุนเร็วหรือหมุนช้าได้แล้ววว แต่เราไม่รู้ว่าความเร็วรอบเป็นเท่าไหร่ใช่ไหมครับ ในบทความนี้ผมจะอธิบายการอ่านความเร็วรอบ DC Motor ด้วย Sensor Encoder โดยที่เราจะใช้ไมโครคอนโทรลเลอร์ ESP32 ในการอ่านจำนวน Pulse ของ Sensor Encoder กัน

สำหรับการมุมองศาการหมุน หรือการอ่านความเร็วรอบ เราหลีกเลี่ยงไม่ได้เลยที่เราจะต้องใช้ Sensor Encoder อย่างแน่นอน ซึ่งถ้าเราสามารถใช้อ่านองศาการหมุน หรือความเร็วรอบจาก Sensor Encoder ได้ ….แน่นอนครับ ถ้าเราอยากจะทำสิ่งที่ยากขึ้นไป เช่น Balancing Robot Control Speed Car เป็นต้นไป เราก็สามารถที่จำนำความรู้จากการบทความนี้ไปประยุกต์ใช้ได้อย่างแน่นอน

เรามาทำความรู้จักทำ Sensor Encoder กันก่อน ….. Sensor Encoder เป็นเซนเซอร์ที่ใช้อ่านระยะทางการเคลื่อน การอ่านความเร็วรอบ องศาการหมุน เป็นต้น ซึ่งชนิดของเซนเซอร์ก็จะแตกต่างกันออกไปต่างลักษณะของงานที่ใช้ ซึ่งในการอ่านความเร็วรอบของมอเตอร์เราจะใช้ Sensor Encoder ที่ลักษณะเป็นจานหมุน สามารถแบ่งออกได้เป็น 2 ชนิดดังรูปภาพที่ 1 1) Absolute Rotary Encoder 2) Incremental Rotary Encoder ภายในจานหมุนจะมีช่องว่างเล็กๆ หลายๆ เพื่อให้ แสงรอดผ่านช่องว่างได้ โดยที่เราจะเอาไว้นับเป็นจำนวน Plus ของ Sensor Encoder นั้นเอง *** ในบทความนี้ผมจะพูดถึงแต่ Incremental Rotary Encoder นะครับ แต่การใช้งานทั้ง 2 ชนิดนี้เหมือนกันนะครับ แตกต่างกันที่ Absolute Rotary Encoder สามารถรู้ได้ว่าองศาของจานหมุนได้ในขณะเริ่มต้นว่าจานหมุนมีองศาที่เท่าไหร่ เช่น หากไฟฟ้าดับกระทันหันเมื่อไฟฟ้ากลับมาติดเราก็สามารถรู้ตำแหน่งองศาได้ทันที่ แต่สำหรับ Incremental Rotary Encoder ไม่สามารถรู้ได้ว่าขณะเริ่มต้นองศาของจานหมุนเป็นเท่าไหร่ เป็นต้น ***

Incremental Encoder ส่วนใหญ่จะเป็นเซนเซอร์ที่ใช้สำหรับวัดระยะการเคลื่อนที่ เชิงมุม โดยความละเอียดในการอ่านค่าขึ้นอยู่กับจำนวน Pulse Per  Revolution (PPR) ยิ่งค่ามากความละเอียดในการอ่านค่ามุมก็จะมากขึ้นตามไปด้วย

สำหรับมอเตอร์ที่เราใช้ในชุดคิทจะมีเซนเซอร์ Incremental Encoder 2 สัญญาณ Output ที่แสดงอยู่ในรูปด้านบนมีค่าความละเอียดอยู่ที่ 96 PPR
หมายถึงเมื่อ Encoder หมุน 1 รอบจะสร้าง Pulse output จำนวน 96 ครั้ง / 1 ช่องสัญญาณ ดังรูปภาพที่ 2

Output ของ Incremental Encoder อาจจะมี 1, 2 หรือ 3 ช่องสัญญาณก็ได้ ถ้า
มี 1 สัญญาณ Output (A) จะทำให้เราทราบระยะที่ Encoder หมุน แต่ไม่รู้ทิศทางของการหมุน
มี 2 สัญญาณ Output (A, B)จะทำให้เราทราบระยะที่ Encoder หมุน และทราบทิศทางของการหมุน
มี 3 สัญญาณ Output (A, B, Z(index))จะทำให้เราทราบระยะที่ Encoder หมุน, ทราบทิศทางของการหมุน, และทราบ Absolute angle ด้วย

เนื่องจาก Incremental Encoder ของเรามี 2 สัญญาณ Output (A, B) ทำมุมห่างกัน 90 องศา ดังนั้นจึงทำให้เราทราบถึงทิศทางการหมุนของมอเตอร์ได้นั้นเอง ว่าจะหมุนตามเข็ม หรือทวนเข็ม เรามาดูหลักการอ่านทิศทางการหมุนกัน…. ก่อนอื่นเลยสำหรับ Encoder ทีมี 2 ช่องสัญญาน Output ตำแหน่งติดตั้งของเซนเซอร์แสงจะห่างกัน 1/2 ของความกว้างของช่องแถบโลหะ เพื่อให้ phase ของสัญญาน Output ห่างกัน 90 องศา ซึ่งการติดตั้งแบบนี้ทำให้เราสามารถทราบทิศทางของการหมุน

จากรูปภาพที่ 3 เมื่อสัญญาณ A เปลี่ยนแปลงสถานะขอบสัญญาณ ถ้าสัญญาณ Pulse A ไม่เท่ากับ Pulse B ก็จะทำให้เราทราบถึงทิศทางการหมุนไปตามเข็มนาฬิกาดังรูปภาพที่ 4

จากรูปภาพที่ 3 เมื่อสัญญาณ A เปลี่ยนแปลงสถานะขอบสัญญาณ ถ้าสัญญาณ Pulse A เท่ากับ Pulse B ก็จะทำให้เราทราบถึงทิศทางการหมุนไปทวนเข็มนาฬิกาดังรูปภาพที่ 5

… เพียงเท่านี้เราก็สามารถทราบถึงทิศทางการหมุนของ DC Motor ได้

โดยปรกติการค่าอ่านมุมจาก Incremental Encoder มีโหมดการอ่านอยู่ 3 โหมด คือ X1 , X2 , X4

โหมด X1

สำหรับการอ่านโหมด X1  สามารถอ่านได้จากการเปลี่ยนแปลงของสัญญาณ Output A หรือ Output B ก็ได้ สามารถอ่านได้จากการเปลี่ยนของสัญญาณ Output rising หรือ falling (ทำให้เราทราบองศาที่ Encoder หมุน แต่ไม่รู้ทิศทางของการหมุน)

โหมด X2

สำหรับการอ่านโหมด X2  สามารถอ่านได้จากการเปลี่ยนแปลงของสัญญาณ Output A จากการเปลี่ยนแปลงสัญญาณ Output (rising หรือ falling) พร้อมกับอ่านสัญญาณ Output B (HIGH หรือ LOW) (ทำให้เราทราบองศาที่ Encoder หมุน และทราบทิศทางของการหมุน)

โหมด X4

สำหรับการอ่านโหมด X4  สามารถอ่านได้จากการเปลี่ยนแปลงของสัญญาณ Output A และ Output B จากการเปลี่ยนแปลงสัญญาณ Output rising และ falling (ทำให้เราทราบระยะที่ Encoderหมุน และทราบทิศทางของการหมุน เพิ่มความละเอียดในการอ่านสัญญาณ)

เรามาลองคำนวณความละเอียดในการอ่านมุมของแต่ละโหมดกัน… โดยใช้จำนวน Pulse Per  Revolution ของเซนเซอร์ของเราคือ 96 PPR

จากการคำนวณที่ผ่านมาโหมดในการอ่าน Incremental Encoder จะมีความละเอียดแตกต่างกัน ในโหมด X4 มีความละเอียดสูงสุด ซึ่งก็ทำให้เราอ่านค่าได้ละเอียดมากยิ่งขึ้น

สำหรับ ESP32 ที่เขียนด้วย Arduino IDE เพื่อน ๆ อาจจะใช้ การอ่าน Encoder โดนใช้ External Interrupt Input ก็ได้ แต่ ESP32 มี Hardware Counter Module ที่ขื่อ Pulse Counter(PCNT) ที่ใช้สำหรับอ่านค่าจาก Encoder อยุ่แล้ว ทำให้ CPU ไม่ต้อง interrupt ทุกครั้งที่สัญญาณ Output ของ Encoder มีการเปลี่ยนแปลง

สามารถ Download >> ESP32Encoder library >> https://github.com/madhephaestus/ESP32Encoder

ศึกษาเพิ่มเติ่มได้ที่ >> ESP32: Reading Incremental Encoder with Pulse Counter(PCNT)

เมื่อเพื่อนๆ ติดตั้ง ESP32Encoder library เสร็จเรียบร้อยแล้วสามารถ Code Example ด้านล่างนี้ ไปอ่านมุมของ Incremental Encoder ได้เลย…

#include <ESP32Encoder.h>

ESP32Encoder encoder;

const int encoderA_pin = 26;
const int encoderB_pin = 25;
const int encoder_CPR = 96;

float count_to_angle(int32_t cnt)
{
  float angle = (float)cnt * 360.0f / ((float)encoder_CPR*4);
  return angle;
}

void setup()
{
  // put your setup code here, to run once:
  pinMode(encoderA_pin, INPUT_PULLUP);
  pinMode(encoderB_pin, INPUT_PULLUP);

  encoder.setCount(0);
  encoder.attachFullQuad(encoderA_pin, encoderB_pin);
  Serial.begin(115200);
}

void loop()
{
  // put your main code here, to run repeatedly:
  delay(10);
   Serial.println(count_to_angle(encoder.getCount()));
//  Serial.println(encoder.getCount());
if (count_to_angle(encoder.getCount()) > 360) encoder.setCount(0);
if (count_to_angle(encoder.getCount()) < -360) encoder.setCount(0);
}

ต่อไป…..เมื่อเราสามารถอ่านมุมของ Sensor Encoder ได้แล้ว ดังนั้นเราก็สามารถอ่านความเร็วรอบของมอเตอร์ได้อย่างแน่นอน

สำหรับการอ่านความเร็วรอบ (รอบต่อนาที) ของมอเตอร์จากการสมการดังนี้

rpm คือ รอบ/นาที
Counter คือ จำนวน Counter ที่อ่านได้จากการหมุน
mode คือ โหมดที่เราใช้ในการอ่าน X1 = 1 , X2 = 2 , X4 =4
sampling time คือ เวลาที่เราใช้ในการอ่านจำนวน Counter
60 คือ เปลี่ยนจากวินาทีให้เป็นนาที

สำหรับการอ่านความเร็วรอบ (รอบต่อนาที) ของมอเตอร์ เพื่อนๆสามารถนำ Code Example ด้านล่างนี้ ไปทำการอ่านความเร็วรอบของมอเตอร์ได้เลยนะครับ

#include <ESP32Encoder.h>
#include <Arduino.h>
#include <math.h>

ESP32Encoder encoder;

const int vr1_pin = 13;

const int MotorA_pin = 22;
const int MotorB_pin = 21;

const int encoderA_pin = 26;
const int encoderB_pin = 25;

const int freq = 15000;
const int resolution = 12;

uint32_t time_now = 0;
uint32_t time_prve1 = 0;
uint32_t time_prve2 = 0;


float alpha = 1;
float omega = 0;
int counter = 0;
float Raw_SensorValue = 0;

float count_to_rpm (float counter, float dt , float rotary , float x)
{
  float tmp = ((counter * 10.00f) / (x * dt * rotary));
//  return tmp * 0.1f * 2 * 3.14159f;
    return tmp * 0.1f * 60.00f;
}

float cal_coff(float fc, float d_T)
{
  float alpha = 0;
  alpha = (2.0f * M_PI * d_T * fc) / ((2.0f * M_PI * d_T * fc) + 1);
  return alpha;
}

float lpf(float alpha, float x_input, float y_output)
{
  return y_output + alpha * (x_input - y_output);
}

void setup()
{

  pinMode(MotorA_pin, OUTPUT);
  pinMode(MotorB_pin, OUTPUT);
  Serial.begin(115200);

  pinMode(vr1_pin, INPUT);

  pinMode(encoderA_pin, INPUT_PULLDOWN);
  pinMode(encoderB_pin, INPUT_PULLDOWN);

  // PWM channel ,PWM signal frequency , set the signal’s duty cycle resolution
  ledcSetup(0, freq, resolution);
  ledcSetup(1, freq, resolution);

  ledcAttachPin(MotorA_pin, 1);
  ledcAttachPin(MotorB_pin, 0);

  encoder.setCount(0);
  encoder.attachFullQuad(encoderA_pin, encoderB_pin);

  Serial.print(" Fc = 10Hz alpha=");
  Serial.println(cal_coff(10, 0.005));
  alpha = cal_coff(10, 0.005);

  delay(5000);
}

void loop()
{
  time_now = millis();

  if (time_now - time_prve1 >= 5)
  {
    time_prve1 = time_now;

    int speed_motor = analogRead(vr1_pin);

    Serial.print("omega ");
    Serial.println(omega, 2);
    

    if (speed_motor > 0)
    {
      ledcWrite(1, 0);                          // 0-4095
      ledcWrite(0, constrain(speed_motor, 0, 4095)); // 0-4095
    }
    else
    {
      ledcWrite(0, 0);                                // 0-4095
      ledcWrite(1, constrain(fabs(speed_motor), 0, 4095)); // 0-4095
    }

  }
  if (time_now - time_prve2 >= 5)
  {
    time_prve2 = time_now;

    counter = encoder.getCount();

    Raw_SensorValue = lpf(alpha, counter , Raw_SensorValue);

    omega =  count_to_rpm(Raw_SensorValue, 0.005, 96, 4); //caculation

    encoder.setCount(0);

  }
}

สามารถซื้อชุดฝึกการเรียนรู้ควบคุมความเร็วรอบด้วย DC Motor Speed Control Kit (พร้อมคอร์สเรียนออนไลน์ฟรี) และรายละเอียดได้ที่ https://www.aiiotshop.com/product/61