27. Wio Terminal EP.14 การควบคุมเซอร์โวมอเตอร์

---

 Wio Terminal EP.14 การควบคุมเซอร์โวมอเตอร์ โดยไม่ใช่ไลบรารี่ ใด ๆ

ในการควบคุมเซอร์โว มอเตอร์นั้น สิ่งแรกที่จะต้องรู้คือ เซอร์โว มอเตอร์ ทำงานได้อย่างไร เซอร์โวมอเตอร์มีสาย 3 เส้น เรียงลำดับกัน ดังนี้

S-Signal สายรับสัญญาณควบคุมการหมุนแบบพัลส์ PWM

V-VCC  สายสำหรับต่อกับไฟบวก 5V

G-GND สายสำหรับต่อกับไฟลบ หรือกราว์ด (GND)

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

 คำอธิบายวิธีการคำนวณ

        1. องศาของการหมุน มีค่า จาก 0 - 180 องศา อันนี้อ้างอิงการทำงานของ เซอร์โว มอเตอร์ มาตรฐาน จะพบว่า ตัวเลของศา มีช่วงทั้งหมด 180 ค่า

        2. จะต้องมีการส่งสัญญาณพัลส์ ในลักษณะ ON / OFF หรือ HIGH/LOW ที่มีความกว้างช่วง HIGH จาก 0.5 มิลลิวินาที ถึง 2.5 มิลลิวินาที หรือ  500 ไมโครวินาที ถึง 2500 ไมโครวินาที นั่นเอง จะพบว่าค่าความกว้างของสัญญาณพัลส์ มีช่วงทั้งหมดเป็น 2000

        3. ทำการคำนวณค่าสัมประสิทธิ์ในการแปลงองศากับเวลา โดยนำ 2000 / 180 = 11.1111111111

        4. เมื่อมุมในการหมุน เป็น 0 จะต้องส่งความกว้างพัลส์ เป็น 500 ดังนั้น มุมในการหมุน ใด ๆ จะมีค่าเป็น มุม * 11.1111111111 +500 หรือเขียนโค้ด tOn ได้เป็น 
            int tOn = deg * 11.1111111111 + 500; 

       5. ในทำนองเดียวกัน ช่วงพัลส์ที่เป็น LOW ก็จะคำนวณได้ จากการนำ 2500 - ช่วงเวลาที่เป็นพัลส์ HIGH หรือเขียนโค้ด tOff ได้เป็น

            int tOff = 2500 - tOn;

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

    การเขียนโค้ด ฟังก์ชั่น servo (int deg) ไว้ใช้สำหรับสั่งให้ เซอร์โว มอเตอร์ หมุนไปที่ตำแหน่ง deg

void servo(int deg)

{

  int tOn = deg * 11.1111111111 + 500; // แปลงค่ามุม 0-180 เป็น ค่าเวลา 500 us - 2500 us

  int tOff = 2500 - tOn;

  // Generate pulse to drive servo

  digitalWrite(D2, HIGH); // สั่งให้ขา D2 เป็นลอจิก 1

  delayMicroseconds(tOn); // หน่วงเวลาพัสล์บวก

  digitalWrite(D2, LOW); // สั่งให้ขา D2 เป็นลอจิก 0

  delayMicroseconds(tOff); // หน่วงเวลาพัสล์ลบ

  delay(20);

}

ขั้นตอนการทำงาน
    1. วาดหน้าจอของมิเตอร์
    2. อ่านค่าจากเซ็นเซอร์วัดความเร่ง Acellerator & Gyroscope
    3. แปลงค่าและแสดงผล
    
การต่อสาย
    ต้องใช้สาย Dupont Femle-Female จำนวน 3 เส้น เพื่อใช้ในการต่อจากขั้วสายของ เซอร์โว มอเตอร์ กับ พอร์ต GPIO ด้านหลังของ Wio Terminal โดยต่อสายไฟบวก V ของ เซอร์โว มอเตอร์ กับขั้ว 2 หรือ 4 ต่อสายไฟลบ G ของ เซอร์โว มอเตอร์ กับขั้ว 6 และต่อสายสัญญาณ S ของ เซอร์โว มอเตอร์ กับขั้ว 16 หรือ D2 

---

เขียนโค้ด ใช้ทดสอบ

/*

   plakemphet.blogspot.com

   18/10/2021

   servo drive test using no library for Wio-Terminal

*/

void setup()
{

  pinMode(D2, OUTPUT); // กำหนดขา GPIO-16/D2 เป็นเอาต์พุต

}

void servo(int deg)

{

  int tOn = deg * 11.1111111111 + 500; // แปลงค่ามุม 0-180 เป็น ค่าเวลา 0.5 ms - 2.5 ms

  int tOff = 2500 - tOn;

  // Generate pulse to drive servo

  digitalWrite(D2, HIGH); // สั่งให้ขา D2 เป็นลอจิก 1

  delayMicroseconds(tOn); // หน่วงเวลาพัสล์บวก

  digitalWrite(D2, LOW); // สั่งให้ขา D2 เป็นลอจิก 0

  delayMicroseconds(tOff); // หน่วงเวลาพัสล์ลบ

  delay(20);

}

void loop()

{

  for (int k = 0; k < 181; k++)

    servo(k);

  for (int k = 180; k > 0; k--)

    servo(k);

}

  

---

 ผลที่ได้จากการทำงาน

ผู้เขียนได้ทดสอบการทำงานของโค้ดข้างต้น กับ เซอร์โว มอเตอร์ มาตรฐาน ของ Parallax และ micro servo รุ่น SG90

---