ควบคุมความเร็วและทิศทางของมอเตอร์กระแสตรงด้วย PID Controller และ PWM Outputs

เกริ่นนำ

เกือบทุกโปรเจคที่มีอยู่บนโลกนี้ เจ้าของโปรเจคมักอยากควบคุมทั้งความเร็ว และ ทิศทางของมอเตอร์ไปพร้อมกัน แต่ส่วนใหญ่ชอบส่งสัญญาณ PWM ตรงไปที่มอเตอร์กระแสตรงเลยซะงั้น แม้จะผ่านวงจรควบคุมมอเตอร์ก็ตาม แต่วิธีนี้มักจะล้มเหลวเสมอถ้าอยากได้ความเร็วเป๊ะๆ ตามที่ตั้งใจไว้ เพราะมีฝาแฝดคู่หนึ่งชื่อว่า "แรงเสียดทาน (Friction)" กับ "ความเฉื่อย (Inertia)" นั่นเอง

(ห้ามไปโทษฝาแฝดพวกนี้นะเว้ย! ไม่ว่าพี่จะทำอะไร กับอะไร หรือไม่มีอะไร พวกมันก็จะปรากฏตัวขึ้นมาช่วย "ควบคุม" สถานการณ์ให้อยู่หมัดเสมอ ความเฉื่อยทำให้สิ่งต่างๆ "คิดก่อนทำ" ส่วนแรงเสียดทานคอยจำกัดความเร่งและความเร็วของมัน และ "พลัง" จะกลายเป็น "ศูนย์" ถ้ามันไม่ถูกควบคุมให้อยู่มือ)

Speed Control by Reflecting Setpoint directly to PWM-Output

ดังนั้น ถ้าน้องลองควบคุมความเร็วมอเตอร์โดยส่งค่าที่ป้อนเข้าไป (Input) เป็นสัญญาณ PWM ตรงไปที่เอาต์พุต ความเร็วจริงจะไม่มีทางตรงกับจุดที่ตั้งไว้ (Set Point) แน่นอน และจะมีข้อผิดพลาด (Error) เกิดขึ้นอย่างเห็นได้ชัด ดังในภาพด้านบน นี่แหละที่เราต้องหาวิธีอื่น ซึ่งมันมีชื่อว่า "PID control"

PID Controller

PID control คืออะไร? ลองนึกภาพตอนขับรถดู: เวลาจะออกตัวจากจุดหยุดนิ่ง พี่ต้องเหยียบคันเร่งมากกว่าตอนขับ巡航 ความเร็วคงที่ พอขับด้วยความเร็ว (เกือบ) คงที่แล้ว ก็ไม่ต้องเหยียบคันเร่งมาก แค่คอยชดเชยความเร็วที่หายไปเมื่อจำเป็นเท่านั้น แถมยังต้องปล่อยคันเร่งนิดหน่อยถ้าเร่งเกินความต้องการ นี่แหละคือวิธี "ขับรถอย่างมีประสิทธิภาพ"

PID controller ก็ทำงานแบบเดียวกันเป๊ะ: ตัวควบคุมอ่านค่าความแตกต่าง "สัญญาณข้อผิดพลาด (Error Signal, e)" ระหว่างจุดที่ตั้งไว้ (Set Point) กับค่าจริงที่ได้ออกมา (Actual Output) มันมีองค์ประกอบ 3 ส่วน คือ "สัดส่วน (Proportional)", "อินทิกรัล (Integral)" และ "อนุพันธ์ (Derivative)" ชื่อของตัวควบคุมก็มาจากตัวแรกของแต่ละคำนั่นเอง ส่วนสัดส่วนกำหนดความชัน (ความเร่ง) ของสัญญาณควบคุมตามข้อผิดพลาดจริง ส่วนอินทิกรัลจะรวมข้อผิดพลาดที่เกิดขึ้นตามเวลาเพื่อลดข้อผิดพลาดสุดท้ายให้เหลือน้อยที่สุด ส่วนอนุพันธ์จะคอยจับตาดูความเร่งของสัญญาณข้อผิดพลาด และใส่ "การปรับแต่ง" เข้าไป พี่จะไม่ลงรายละเอียดลึกๆ ตรงนี้ ถ้าสนใจก็ไปหาอ่านเพิ่มเติมในเน็ตเองนะจ๊ะ

ในโปรแกรม Arduino ของพี่ PID controller ถูกเขียนเป็นฟังก์ชันดังนี้:

float controllerPID(float _E,  float _Eprev, float _dT,
                    float _Kp, float _Ki,    float _Kd)
{
  float P, I, D;
  /*
  Base Formula:
  U = _Kp * ( _E + 0.5*(1/_Ki)*(_E+_Eprev)*_dT + _Kd*(_E-_Eprev)/_dT );
  */
  P = _Kp * _E; /* Proportional Component */
  I = _Kp * 0.5 * _Ki * (_E+_Eprev) * _dT; /* Integral Component */
  D = _Kp * _Kd * (_E-_Eprev) / _dT; /* Derivative Component */
  return (P+I+D);
}

จากนั้นค่าสุดท้ายที่จะส่งออกไป (Final Output) ก็หาได้ง่ายๆ โดยนำค่าปัจจุบันบวกกับผลลัพธ์จาก PID controller ดูส่วนของโปรแกรมหลักนี้ ที่มีการคำนวณสัญญาณข้อผิดพลาดและผลลัพธ์จาก PID Controller:

/* Error Signal, PID Controller Output and
   Final Output (PWM) to Motor */
E = RPMset - RPM;
float cPID = controllerPID(E, Eprev, dT, Kp, Ki, Kd);
if ( RPMset == 0 ) OutputRPM = 0;
else OutputRPM = OutputRPM + cPID;
if ( OutputRPM < _minRPM ) OutputRPM = _minRPM;

PID-Control (initial)

PID-Control (tuned)

วงจรจ่ายไฟให้มอเตอร์กระแสตรง

แน่นอนว่าไม่แนะนำให้ขับมอเตอร์กระแสตรงตรงจากขาเอาต์พุตของ Arduino หรือบอร์ดควบคุมอื่นๆ โดยตรง มอเตอร์กระแสตรงต้องการกระแสไฟจำนวนมากเมื่อเทียบกับที่ขาเอาต์พุตของบอร์ดควบคุมจ่ายให้ไม่ไหว ดังนั้นเราต้องใช้ขดลวดรีเลย์ แต่ปัญหาอีกอย่างก็ตามมา: รีเลย์มีชิ้นส่วนกลไกที่อาจเสียหายได้ในระยะกลางหรือยาว เราต้องใช้อุปกรณ์อีกชนิดที่นี่ นั่นคือ ทรานซิสเตอร์

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

หลังจากทำขั้นตอนนั้นเสร็จ พี่ก็ตัดสินใจใช้ทรานซิสเตอร์แบบ PNP BC307A จำนวนสี่ตัว วางแบบ "บริดจ์" เพื่อกำหนดทิศทางของกระแสที่ไหลผ่านขดลวดมอเตอร์ (จริงๆ แล้วชุด NPN BC337 น่าจะทำงานได้ดีกว่าเพราะทนกระแสคอลเลกเตอร์ที่สูงกว่าได้มาก แต่ตอนนั้นพี่ไม่มีมันน่ะ)

เนื่องจากกระแสของมอเตอร์จะต้องไหลผ่านทาง Emitter-Collector ของทรานซิสเตอร์ มันจึงจำเป็นต้องใช้ทรานซิสเตอร์ที่มีค่าสัมประสิทธิ์อัตราขยายกระแสตรง (hfe) ประมาณเท่ากัน เพื่อตรวจสอบเรื่องนี้ น้องสามารถใช้วงจรด้านล่างนี้ แล้วคัดเลือกทรานซิสเตอร์ที่ให้ค่ากระแสบนแอมป์มิเตอร์อ่านได้ใกล้เคียงกัน สำหรับการออกแบบวงจรเบื้องต้นนี้ น้องต้องพิจารณาตามนี้:

หาค่า "Base-Emitter On-Voltage" (VBEon) ของทรานซิสเตอร์ มันคือแรงดันขั้นต่ำที่ต้องป้อนให้ขา Base เพื่อสวิตช์ทรานซิสเตอร์ให้เปิด
หาค่า "DC Current Gain" (hfe) โดยทั่วไปของทรานซิสเตอร์ ที่กระแสคอลเลกเตอร์ใกล้เคียงกับกระแสมอเตอร์ โดยทั่วไปมันคืออัตราส่วนระหว่าง Collector Current (IC) กับ Base Current (IB), hfe = IC / IB
หาค่า "Maximum Continuous Collector Current" ของทรานซิสเตอร์ (ICmax) กระแสตรงของมอเตอร์ห้ามเกินค่านี้เป็นอันขาด (ในแง่ของค่าสัมบูรณ์) พี่ใช้ BC307 ได้เพราะมอเตอร์ที่พี่ใช้ต้องการแค่ 70 mA ในขณะที่ทรานซิสเตอร์มี ICmax(abs) = 100 mA

Circuit to Determine Equivalent Transistors

Sample Maximum Ratings for a PNP Bipolar Junction Transistor

Sample On-Characteristics for a PNP Bipolar Junction Transistor

ทีนี้ น้องก็สามารถหาค่าความต้านทานที่จะต่อเข้ากับขา Base ได้แล้ว: เริ่มแรก น้องต้องพิจารณาขีดจำกัดของเอาต์พุตจากบอร์ดคอนโทรลเลอร์ และพยายามให้กระแส Base น้อยที่สุดเท่าที่จะเป็นไปได้ (ดังนั้นแนะนำให้เลือกทรานซิสเตอร์ที่มีค่า DC Current Gain สูงที่สุดเท่าที่หาได้) เอาแรงดันที่ออกจากบอร์ดคอนโทรลเลอร์มาเป็น "Trigger Voltage" (VT) แล้วหา Required Base Current (IBreq) โดยการนำ Motor Current (IM) มาหารด้วย DC Current Gain (hfe) ของทรานซิสเตอร์: IBreq = IM / hfe

จากนั้นหาแรงดันที่จะตกคร่อมตัวต้านทาน (VR) โดยการเอา Trigger Voltage ลบด้วย Base-Emitter On-Voltage (VBEon): VR = VT - VBEon

สุดท้าย นำแรงดันที่จะตกคร่อม ตัวต้านทาน (VR) มาหารด้วย Required Base Current (IBreq) เพื่อหาค่า ความต้านทาน (R): R = VR / IBreq

[ สูตรรวม: R = ( VT - VBEon ) * hfe / IM ]

ในกรณีของพี่:

Motor Current: IM = 70 mA
BC307A Parameters: ICmax = 100 mA, hfe = 140 (พี่วัดได้ประมาณนี้), VBEon = 0.62 V
Trigger Voltage: VT = 3.3 V (เอาต์พุต PWM จาก Arduino Due)
R = 5360 โอห์ม (พี่เลยตัดสินใจใช้ 4900 โอห์ม ซึ่งทำจาก 2K2 ต่ออนุกรมกับ 2K7 เพื่อให้แน่ใจว่าครอบคลุมรอบการทำงานเต็มที่ และวงจรดึงกระแสจากเอาต์พุต PWM แค่ ~0.6 mA - ถือว่าเหมาะสมแล้ว)

ลอจิกขั้นสูงสำหรับการควบคุมมอเตอร์

โปรเจกต์นี้ถือเป็นคลาสมาสเตอร์สำหรับการขับโหลดแบบอินดักทีฟอย่างปลอดภัยและมีประสิทธิภาพโดยใช้ Arduino

เทคนิคไดโอดฟลายแบ็ก: องค์ประกอบสำคัญสำหรับปกป้องวงจรของน้องก็คือ ไดโอดฟลายแบ็ก เมื่อปิดมอเตอร์กระทันหัน ขดลวดเหนี่ยวนำของมันจะสร้างแรงดันสไปก์สูงที่เรียกว่า "Back-EMF" สไปก์นี้สามารถทำลาย Arduino หรือทรานซิสเตอร์ได้ง่ายๆ การวางไดโอดเช่น 1N4007 ข้ามขั้วมอเตอร์ (แคโทดต่อกับขั้วบวก, แอโนดต่อกับขั้วลบ) จะสร้างเส้นทางปลอดภัยให้กระแสนี้สลายไป ช่วยปกป้องอุปกรณ์อิเล็กทรอนิกส์ของน้อง
การเชี่ยวชาญ H-Bridge: ในขณะที่โปรเจกต์นี้ใช้บริดจ์ทรานซิสเตอร์แบบแยกส่วน อีกทางเลือกที่พบได้บ่อยและแข็งแรงคือ H-bridge แบบชิป เช่น L298N H-bridge ช่วยให้น้องควบคุมทั้งความเร็ว (ผ่าน PWM) และทิศทาง โดยการสลับการไหลของกระแสผ่านมอเตอร์ทางอิเล็กทรอนิกส์ ทำให้สามารถทำงานเดินหน้าและถอยหลังได้โดยไม่ต้องใช้สวิตช์กล

การกลับทิศทางและข้อควรระวังสำคัญ

การกลับทิศทางของมอเตอร์ DC แค่กลับทิศทางการไหลของกระแสก็พอแล้ว ในการทำเช่นนั้น เราสามารถสร้างวงจรบริดจ์ด้วยชุดทรานซิสเตอร์สี่ตัวได้ง่ายๆ ตามแผนภาพ; PWM Output#2 จะเปิด T1A และ T1B ในขณะที่ PWM Output#3 จะเปิด T2A และ T2B ดังนั้นกระแสที่ไหลผ่านมอเตอร์จึงเปลี่ยนทิศทางไป

แต่ตรงนี้เราต้องคิดถึงอีกเรื่องนึงนะน้อง: ตอนมอเตอร์เพิ่งสตาร์ท มันจะดูดกระแสพุ่งสูงกว่ากระแสที่ระบุบนแผ่นสเปค (ที่โรงงานให้มา) มาก กระแสสตาร์ทอาจจะสูงถึงประมาณ 130% ของกระแสปกติสำหรับมอเตอร์ขนาดเล็ก และจะยิ่งสูงขึ้นถ้ามอเตอร์ใหญ่ขึ้น

ดังนั้น ถ้าน้องจ่ายไฟให้มอเตอร์ตรงๆ จากแหล่งจ่าย แล้วรีบสั่งย้อนทางทันทีตอนมันยังวิ่งอยู่ มอเตอร์จะดูดกระแสสูงปรี๊ดเพราะมันยังไม่หยุดสนิท สุดท้ายอาจจะทำแหล่งจ่ายระเบิดหรือทำให้ขดลวดมอเตอร์ไหม้ได้เลยนะตัวนี้! เรื่องนี้อาจจะไม่ค่อยสำคัญสำหรับมอเตอร์จิ๋วๆ แต่จะสำคัญมากถ้าน้องเล่นกับมอเตอร์กำลังสูงขึ้น

แต่ถ้าน้องขับมอเตอร์ผ่านทรานซิสเตอร์หรือชุดทรานซิสเตอร์ (เช่น แบบดาร์ลิงตัน) ปัญหานี้จะไม่เกิดเพราะทรานซิสเตอร์มันจำกัดกระแสให้อยู่แล้ว

ยังไงก็ตาม พี่ได้เขียนโค้ดเล็กๆ ไว้แบบนี้: เมื่อมีการเปลี่ยนทิศทางตอนมอเตอร์กำลังวิ่ง โปรแกรมจะสั่งให้สัญญาณ PWM ทั้งสองช่องเป็นศูนย์ก่อน แล้วค่อยรอให้มอเตอร์หยุดสนิท ค่อยไปทำหน้าที่ต่อ แล้วค่อยคืนการควบคุมให้โปรแกรมหลัก

if ( Direction != prevDirection )
 {
  /* Killing both of the PWM outputs to motor */
  analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0);
  /* Wait until motor speed decreases */
  do
   { RPM = 60*(float)readFrequency(_chSpeedRead,4)/_DiscSlots;   }
  while ( RPM > _minRPM );
 }

Direction Changing

ทริคเล็กๆ น้อยๆ ในซอฟต์แวร์

เพิ่มความเร็วแบบค่อยเป็นค่อยไป (Acceleration Ramping): เทคนิคง่ายๆ แต่ได้ผลคือการเขียนโค้ดให้ค่า PWM Duty Cycle ค่อยๆ เพิ่มขึ้นจาก 0 ไปถึงค่าที่ต้องการภายในเวลาที่กำหนด (เช่น จาก 0 ถึง 255 ในเวลา 1 วินาที) แทนที่จะกระโดดเปลี่ยนค่าในทันที วิธีนี้ช่วยป้องกัน "กระแสไหลเข้าสูง (inrush current)" ตอนมอเตอร์หยุดนิ่งแล้วได้รับไฟกะทันหัน ซึ่งอาจทำให้ Arduino รีเซ็ตเพราะไฟตกหรือฟิวส์ขาดได้

การอ่านความเร็ว

ในโปรเจคนี้พี่ใช้เซ็นเซอร์วัดความเร็วราคาถูก HC-020K มันส่งพัลส์ออกมาที่ระดับแรงดันไฟเลี้ยง ซึ่งใน datasheet บอกว่าใช้ 5V แต่อาร์ดุยโน่ Due ของพี่รับแรงดัน 5V ไม่ได้ พี่เลยจ่ายไฟเลี้ยงให้มันจากขา 3.3V ของ Due ตรงๆ ปรากฏว่าใช้ได้เวิร์ค! และฟังก์ชันด้านล่างนี้เขียนไว้เพื่ออ่านความถี่จากเอาต์พุตของ HC-020K

int readFrequency(int _DI_FrequencyCounter_Pin, float _ReadingSpeed)
{
 pinMode(_DI_FrequencyCounter_Pin,INPUT);
 byte _DigitalRead, _DigitalRead_Previous = 0;
 unsigned long _Time = 0, _Time_Init;
 float _Frequency = 0;
 if ( (_ReadingSpeed<=0) || (_ReadingSpeed>10) ) return (-1);
    else
      {
        _Time_Init = micros();
        do
         {
          _DigitalRead = digitalRead(_DI_FrequencyCounter_Pin);
          if ( (_DigitalRead_Previous==1) && (_DigitalRead==0) ) _Frequency++;
          _DigitalRead_Previous = _DigitalRead;
          _Time = micros();
         }
        while ( _Time < (_Time_Init + (1000000/_ReadingSpeed)) );
      }
 return (_ReadingSpeed * _Frequency);
}

จำไว้ว่าล้อของ HC-020K มีช่อง 20 ช่อง ดังนั้นความถี่ที่อ่านได้ต้องหารด้วย 20 เพื่อให้ได้ความเร็วรอบต่อวินาที แล้วค่อยคูณด้วย 60 เพื่อให้ได้ค่า RPM

RPM = 60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots;

แต่งหน้ากราฟิกส์สวยๆ

เพื่อแสดงค่าอินพุตและผลลัพธ์ พี่ใช้จอ Makeblock Me TFT LCD และเขียนฟังก์ชัน CommandToTFT() ไว้ส่งคำสั่งไปให้มัน จริงๆ เหตุผลที่มีฟังก์ชันนี้แค่เพื่อให้เปลี่ยนจุดเชื่อมต่อ serial ในโปรแกรมแค่บรรทัดเดียวตอนจำเป็น

ฟังก์ชัน Cartesian_Setup(), Cartesian_ClearPlotAreas() และ Cartesian_Line() ถูกเขียนขึ้นเพื่อเตรียมพื้นที่พล็อตกราฟ ล้างพื้นที่พล็อตเมื่อกราฟถึงจุดสิ้นสุดของแกนนอน (ซึ่งในที่นี้คือ "เวลา") และวาดกราฟ ตามลำดับ ถ้าน้องสนใจเรื่องฟังก์ชันกราฟิกส์พวกนี้ ลองไปดูคู่มือของ Makeblock Me TFT LCD เพิ่มเติมนะ เพราะพี่จะไม่ลงรายละเอียดตรงนี้แล้ว มันออกนอกสโคปของบล็อกนี้แล้วจ้า

จบละจ้า

รุ่นพี่แยกโค้ดโปรแกรมแบบมีและไม่มีฟังก์ชันกราฟิกไว้ให้แล้วนะน้อง จะได้เลือกดูได้ว่าโค้ดส่วนควบคุมความเร็วและทิศทาง (Speed and Direction Control Implementation) ทำงานยังไงแบบล้วนๆ หรือจะดูแบบมีกราฟิกประกอบก็ได้ นอกจากนี้ในโค้ดก็มีคำอธิบายเพิ่มเติมสำหรับฟังก์ชันต่างๆ ที่เขียนไว้ให้ด้วย ไปไล่อ่านกันได้เลย

สุดท้ายนี้ เรื่องนึงที่ต้องรู้ไว้คือ โดยปกติแล้วเราไม่สามารถลดความเร็วของมอเตอร์กระแสตรง (DC motor) ให้ต่ำกว่า 10-20% ของความเร็วพิกัดได้ แม้จะไม่มีโหลดเลยก็ตาม แต่ถ้าใช้การควบคุมแบบ PID กับมอเตอร์ที่กำลังทำงานอยู่แล้วและไม่มีโหลด เราก็อาจจะดึงความเร็วลงไปได้ต่ำเกือบถึง 5% เลยทีเดียว รู้ไว้ใช้ว่านะตัวนี้

แก้ไขเพิ่มเติม (25 ก.พ. 2018): ถ้าน้องอยากใช้ทรานซิสเตอร์แบบ NPN แทน PNP ต้องระวังเรื่องทิศทางกระแสให้ดีด้วย เพราะสองตัวนี้ไหลสวนทางกัน เวลาเปิด (trigger) กระแสใน PNP จะไหลจาก Emitter ไป Collector ส่วน NPN จะไหลจาก Collector ไป Emitter ดังนั้นการต่อขั้วจึงต่างกัน: PNP ต่อเป็น E(+) C(-) ส่วน NPN ต้องต่อเป็น C(+) E(-) จำไว้ให้แม่น ไม่งั้นช็อตแน่!

Setup

ข้อมูล Frontmatter ดั้งเดิม