ว่าไงน้อง!
ไอเดียหลักของโปรเจคนี้คือการควบคุมความเร็วรอบของมอเตอร์ DC ด้วย PID controller ครับพี่น้อง บางคนอาจคิดว่า "ไม่เห็นมีอะไร มอเตอร์ DC ก็แค่ปรับแรงดันก็เปลี่ยนความเร็วได้แล้วนิ" จริงครับพี่! ถ้าอยากให้เร็วขึ้นก็แค่ปั๊มแรงดันเข้าไป
แต่! ปัญหามันอยู่ที่ว่า แรงดันไม่ได้กำหนดความเร็วตายตัว หรอกนะตัวดี สมมติมอเตอร์ในรถไฟฟ้าได้แรงดันคงที่ ความเร็วก็จะคงที่... ตราบใดที่มันไม่เจอทางขึ้นเขา! พอเจอทางชัน แรงดันก็ต้องปรับตามเพื่อรักษาความเร็วให้คงที่ ใครเคยปั่นจักรยานก็เข้าใจดี
ถ้าอยากควบคุมความเร็วมอเตอร์ DC สิ่งแรกที่ต้องทำคือ "วัดความเร็วจริง" กันก่อน หน่วยที่ใช้คือรอบต่อนาที หรือที่เรียกกันว่า "rpm" หลังจากนั้นเราก็ตั้ง "ความเร็วเป้าหมาย" หรือ "Setpoint" ได้เลย
ทีนี้ปัญหาคือ... ทำยังไงให้ความเร็วจริง (rpm) วิ่งเข้าใกล้ความเร็วเป้าหมาย (Setpoint) ให้ได้? เรารู้ว่าเพิ่มแรงดัน -> ความเร็วเพิ่ม ลดแรงดัน -> ความเร็วลด งั้นลองคิดแบบนี้ดู:
- ถ้า
rpm < Setpoint-> ต้องเพิ่มแรงดัน - ถ้า
rpm > Setpoint-> ต้องลดแรงดัน
เขียนเป็นสมการคณิตศาสตร์แบบชิวๆ ได้ว่า:
แรงดัน = P * (Setpoint - rpm)
หรือ
แรงดัน = P * error (โดย error = Setpoint - rpm)
เจ้านี่แหละที่เรียกว่า P controller เพราะค่าควบคุม (แรงดัน) เป็นสัดส่วนโดยตรง (Proportional) กับ error
แต่ PID controller จริงๆ มันเจ๋งกว่านั้น! มันคำนึงถึง Integral (I) และ Derivative (D) ของ error ด้วย
- Integral (I): คือการรวม error ตลอดเวลา (คิดเป็นพื้นที่ใต้กราฟ error) ส่วนนี้ช่วยจัดการกรณีที่ error น้อยๆ แต่สะสมนานๆ จนระบบไม่ไปถึงจุดที่ต้องการ
- Derivative (D): คืออัตราการเปลี่ยนแปลงของ error (ความชันของกราฟ error) ส่วนนี้เหมือนเพิ่มแดมป์ให้ระบบ ช่วยลดการแกว่ง (oscillation) ถ้าระบบเรามันชอบสั่น
สมการ PID เต็มๆ จึงเป็นแบบนี้:
แรงดัน = P * error + I * Integral(error) + D * Derivative(error)
อยากรู้ลึกกว่านี้เกี่ยวกับ PID controller ลองไปหาอ่านเพิ่มเติมได้นะน้อง (พี่ว่าในวิกิพีเดียอธิบายได้ละเอียดดี)
เอาล่ะ! กลับมาที่ตัวอย่างของเรา ในส่วนต่อไปพี่จะพาน้องไปดูว่า:
- ชุดทดลอง ประกอบยังไง
- เซนเซอร์วัดความเร็ว ทำงานยังไง
- เขียนโค้ด PID controller บน Arduino ยังไง
เตรียมตัวให้พร้อม แล้วไปลุยกันต่อเลย! สู้งานนะน้อง!
การประกอบชุดทดสอบ (Test Rig)
ชิ้นส่วนโครงทั้งหมดเป็นชิ้นงานพิมพ์ 3D นะน้อง สามารถดาวน์โหลดไฟล์ได้ที่นี่:
(ลิงก์ Thingiverse ถูกลบตามกฎ)
สำหรับอุปกรณ์อิเล็กทรอนิกส์ พี่แนะนำให้หาชุด Arduino Starter Kit มาใช้ จะได้ครบเครื่องเลย อย่างนี้:
(ลิงก์ Amazon ถูกลบตามกฎ)
ถ้าไม่มีเครื่องพิมพ์ 3D ก็ไม่เป็นไรน้อง จัดโครงจากไม้หรือวัสดุอื่นที่หาได้เลยก็ได้ ข้อมูลการพิมพ์ชิ้นส่วนต่างๆ มีเขียนไว้บน Thingiverse หมดแล้ว ส่วนขั้นตอนการประกอบพี่มีรูปให้ดูตามด้านล่างนี้เลย
การเดินสาย (Wiring)
โค้ด (Code)
เพื่อให้โค้ดรันได้ เราต้องมีไลบรารีเสริม 2 ตัวนะ
สำหรับ PID controller:
(ลิงก์ GitHub ถูกลบตามกฎ)
สำหรับจอ LCD:
(ลิงก์ Arduino Libraries ถูกลบตามกฎ)
ถ้ายังไม่เคยติดตั้งไลบรารีเสริมมาก่อน ลองหาวิธีติดตั้งจากแหล่งข้อมูลทั่วไปเกี่ยวกับ Arduino Libraries ได้เลย
รายละเอียดโปรแกรมทั้งหมด:
//////////PID///////////
#include <PID_v1.h>
//////////Display///////////
#include <LiquidCrystal_I2C.h>
//////////PID///////////
//Define variable
double Setpoint = 0, Input = 0, Output = 0;
//Define the PID named myPID
PID myPID(&Input, &Output, &Setpoint, 1, 1, 0, DIRECT);
//////////Display///////////
LiquidCrystal_I2C lcd(0x27, 20, 4); // set the LCD address to 0x27 for a 16 chars and 2 line display
unsigned long t3 = 0;
unsigned long update_time = 200;
/////Sensor////
int LDR_Pin = A0;
unsigned long t1, t2;
unsigned long braketime = 300000;
unsigned long dt = 0;
unsigned long rpm = 0;
int wait_time = 2;
/////Driver////
//channels
const int pwm = 9;
const int in_1 = 5;
const int in_2 = 6;
const int led_pin = 13;
//////Poti/////
int Pot_pin = A2;
void setup() {
lcd.init(); // initialize the lcd
lcd.backlight();
myPID.SetMode(AUTOMATIC); //turn the PID on
Serial.begin(9600);
pinMode(pwm, OUTPUT); //define channels
pinMode(in_1, OUTPUT);
pinMode(in_2, OUTPUT);
pinMode(led_pin, OUTPUT);
digitalWrite(led_pin, HIGH); //turn led on
}
void loop() {
//Define the Setpoint
//Setpoint = (sin(3.0 * millis() / (1000.0 * 2 * 3.14)) + 2.0 ) * 500.0;
//Setpoint = 0;
Setpoint = analogRead(Pot_pin) * 5.0;
////////measure and calculate rpm////////
/////////////////////////////////////////
//detecting the time dt which passes between two holes
//1. starting at an unknown state and wait until the photoresistor indicates that the light barrier is blocked
t2 = micros(); //t2 indicates the starting time of the loop
while (analogRead(LDR_Pin) < 850) { //The loop runs until the LDR signal is lower than the threshold of 850
if (micros() - t2 > braketime) { //If the loop runs longer than braketime e.g. the rotor did not move the loop stopped
break;
}
}
delay(wait_time); //this delay time is needed due to the noise in the signal
//2. wait until the next hole appears and save this time to the variable t1
t2 = micros();
while (analogRead(LDR_Pin) > 850) { //The loop runs until the LDR signal is higher than the threshold of 850
if (micros() - t2 > braketime) {
break;
}
}
t1 = micros(); //save the time when the first hole appear to t1
delay(wait_time); //this delay time is needed due to the noise in the signal
//3. wait until the hole disappears
t2 = micros();
while (analogRead(LDR_Pin) < 850) { //The loop runs until the LDR signal is lower than the trashhold of 850
if (micros() - t2 > braketime) {
break;
}
}
delay(wait_time); //ดีเลย์นี่ต้องมีนะน้อง ไม่งั้นสัญญาณรบกวนเล่นงาน
//4. รอจนรูถัดไปโผล่มา
t2 = micros();
while (analogRead(LDR_Pin) > 850) { //ลูปนี้จะวิ่งจนกว่า LDR จะอ่านค่าได้สูงกว่า 850 (ค่าแสงทะลุรูมาแล้ว)
if (micros() - t2 > braketime) {
break;
}
}
dt = micros() - t1; //คำนวณเวลาระหว่างรูสองรู
delay(wait_time);
rpm = (1.0 / (dt / 60000000.0)) / 2.0; //คำนวณ RPM เอาเลยวัยรุ่น
//ใช้ PID controller จัดการ
Input = rpm;
myPID.Compute();
digitalWrite(in_1, HIGH); //เซ็ตทิศทางการหมุนมอเตอร์
digitalWrite(in_2, LOW);
analogWrite(pwm, Output); //ส่งค่าที่ PID คำนวณได้ไปให้มอเตอร์
Serial.print(rpm - Setpoint);
Serial.print(",");
Serial.print(rpm);
Serial.print(",");
Serial.print(Setpoint);
Serial.print(",");
Serial.println(Output);
//โชว์ผลลัพธ์บน LCD
if (millis() - t3 > update_time)
{
t3 = millis();
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("set rpm");
lcd.setCursor(8, 0);
lcd.print(int(Setpoint));
lcd.setCursor(13, 0);
lcd.print("U/m");
lcd.setCursor(0, 1);
lcd.print("act rpm");
lcd.setCursor(8, 1);
lcd.print(int(rpm));
lcd.setCursor(13, 1);
lcd.print("U/m");
}
}
เซนเซอร์นี้ทำงานยังไง:
เซนเซอร์วัดความเร็วหรือรอบต่อนาที (RPM) ตัวนี้ ออกแบบมาเป็นแบบ Light Barrier หรือกำแพงแสงนั่นเอง หลักการคือวาง LED ไว้ตรงข้ามกับตัวต้านทานไวแสง (LDR) เพื่อให้ LDR รับแสงจาก LED ได้ ระหว่างกลางสองตัวนี้ เราก็วางโรเตอร์ (ใบพัด) ไว้ โรเตอร์ของเรามีรูสองรู ทำให้แสงผ่านไปถึง LDR ได้สองครั้งต่อการหมุนหนึ่งรอบ
ถ้าเราต่อวงจร LED และ LDR ตามแผนภาพ แล้วสั่งพล็อตค่าจากช่อง A0 ด้วยคำสั่ง
Serial.println(analogRead(A0));
เราจะเห็นกราฟแบบนี้ใน Plotter:
ทุกครั้งที่รูของโรเตอร์หมุนมาทับทางแสง ค่าที่อ่านได้จะกระโดดขึ้นไปประมาณ 930 และเมื่อแสงถูกบังโดยตัวโรเตอร์ ค่าจะตกลงมาต่ำกว่า 800 เวลาระหว่างยอดกราฟสองยอด (peak) ก็คือเวลา "dt" ที่โรเตอร์ใช้หมุนไปครึ่งรอบนั่นเอง
ดังนั้น ถ้าเรารู้ค่าเวลา dt เป็นไมโครวินาที เราก็คำนวณ RPM ได้จากสูตร: rpm = (60 * 10^6)/(2 * dt)
เพื่อให้ได้ค่าเวลาระหว่างสองยอดกราฟมา โค้ดของเราแบ่งการทำงานเป็น 4 ขั้นตอน:
ขั้นตอนที่ 1: รอจนกว่าเราจะอยู่ ระหว่าง ยอดกราฟสองยอด ทำได้ด้วยลูป while ที่เช็คเงื่อนไขว่า ค่าจากช่อง A0 ต่ำกว่า 850 (แสงถูกบัง) เพื่อป้องกันกรณีโรเตอร์หยุดนิ่งแล้วลูปวิ่งไม่หยุด เราจึงใส่เงื่อนไขเบรกลูปถ้ามันรันนานเกิน "braketime" ที่กำหนดไว้
ขั้นตอนที่ 2: โปรแกรมจะรอจนกว่ายอดกราฟถัดไปจะปรากฏ (แสงเริ่มทะลุรู) แล้วบันทึกเวลานั้นเป็น t1
ขั้นตอนที่ 3: โปรแกรมรอจนกว่ายอดกราฟนั้นจะหายไป (แสงถูกบังอีกครั้ง)
ขั้นตอนที่ 4: รอจนกว่ายอดกราฟถัดไปจะปรากฏอีกครั้ง แล้วคำนวณเวลาปัจจุบันลบด้วย t1 ออกมาเป็นค่า "dt" ที่ต้องการ
อย่าเพิ่งงงว่าทำไมมี delay() เยอะแยะในลูปหลักนะน้อง ปัญหาคือถ้าเราเช็คค่า LDR แค่ครั้งเดียวในลูปหลัก มันอาจจะเช็คช้าเกินจนพลาดการตรวจจับยอดกราฟไปได้เลย ทางเลือกอื่นคือใช้ Interrupt นั่นเองจ้า
////////measure and calculate rpm////////
/////////////////////////////////////////
//detecting the time dt which passes between two holes
//1. เริ่มจากสถานะไม่รู้จัก และรอจนกว่า LDR จะบอกว่ากำแพงแสงถูกบัง
t2 = micros(); //t2 คือเวลาเริ่มต้นลูป
while (analogRead(LDR_Pin) < 850) { //ลูปจะวิ่งจนกว่า LDR จะอ่านค่าได้ต่ำกว่า 850 (แสงถูกบัง)
if (micros() - t2 > braketime) { //ถ้าลูปวิ่งนานกว่า braketime (เช่น โรเตอร์ไม่ขยับ) ให้หยุดลูป
break;
}
}
delay(wait_time); //ดีเลย์นี่ต้องมีนะน้อง ไม่งั้นสัญญาณรบกวนเล่นงาน
//2. รอจนรูถัดไปโผล่มา แล้วเซฟเวลานี้เก็บไว้ในตัวแปร t1
t2 = micros();
while (analogRead(LDR_Pin) > 850) { //ลูปนี้จะรันจนกว่าสัญญาณจาก LDR จะสูงกว่าเกณฑ์ 850
if (micros() - t2 > braketime) {
break;
}
}
t1 = micros(); //เซฟเวลาตอนที่รูแรกโผล่มา เก็บไว้ใน t1
delay(wait_time); //ต้องดีเลย์หน่อยนะ น้อง ไม่งั้นสัญญาณรบกวนจะเล่นงาน
//3. รอจนกว่ารูจะหายไป
t2 = micros();
while (analogRead(LDR_Pin) < 850) { //ลูปนี้จะรันจนกว่าสัญญาณจาก LDR จะต่ำกว่าเกณฑ์ 850
if (micros() - t2 > braketime) {
break;
}
}
delay(wait_time); //ดีเลย์อีกนิด ชัวร์ไว้ก่อน เดี๋ยวสัญญาณรบกวนมากวน
//4. รอจนรูถัดไปโผล่มา
t2 = micros();
while (analogRead(LDR_Pin) > 850) { //ลูปนี้จะรันจนกว่าสัญญาณจาก LDR จะสูงกว่าเกณฑ์ 850
if (micros() - t2 > braketime) {
break;
}
}
dt = micros() - t1; //คำนวณเวลาระหว่างรูสองรู
delay(wait_time);
rpm = (1.0 / (dt / 60000000.0)) / 2.0; //คำนวณค่า RPM ออกมาเลยจ้า
### วิธีที่ PID Controller ถูกนำมาใช้:
ด้วยการ
`#include <PID_v1.h>`
เราก็ได้ include library ของ PID controller มาใช้แล้ว ตัว library นี้เจ๋งมาก อยากรู้ลึกๆ ลองไปหาอ่านเพิ่มเติมได้นะ
ในการกำหนดค่า PID controller ของเรา เราเขียนโค้ดสองบรรทัดนี้ก่อนส่วน `setup` loop
double Setpoint = 0, Input = 0, Output = 0; PID myPID(&Input, &Output, &Setpoint, 1, 1, 0, DIRECT);
บรรทัดแรก กำหนดตัวแปร 3 ตัว `Setpoint` ก็คือความเร็วเป้าหมายที่เราตั้งไว้ `Input` คือความเร็วที่วัดได้จริง และ `Output` คือค่าแรงดันหรือตัวแปรควบคุม
บรรทัดที่สอง คือการเริ่มต้นใช้งาน PID controller และตั้งชื่อมันว่า `myPID` พารามิเตอร์ 3 ตัวแรกคือที่อยู่ของตัวแปร 3 ตัวที่เรากำหนดไว้ก่อนหน้า ถ้าเราเปลี่ยนค่าตัวแปรพวกนี้ทีหลัง ค่าใน PID controller ก็จะอัพเดทตามอัตโนมัติ
พารามิเตอร์ 3 ตัวถัดไปคือค่า P, I และ D ของ PID controller ตอนนี้เราตั้งเป็น P=1, I=1, และ D=0 น้องลองปรับค่าเล่นๆ ดูได้นะ หาค่าที่เหมาะกับงานเรา
ในส่วน `setup` loop เราต้องเปิดการทำงานของ PID controller ด้วยคำสั่ง
`myPID.SetMode(AUTOMATIC);`
สุดท้ายใน `main` loop เราเขียนโค้ดแบบนี้:
Input = rpm; myPID.Compute(); digitalWrite(in_1, HIGH); digitalWrite(in_2, LOW); analogWrite(pwm, Output);
เริ่มแรก เราเก็บค่า `rpm` ที่วัดได้ ไว้ใน `Input` จากนั้นคำนวณค่า `Output` ด้วย `myPID.Compute();` ตอนนี้ `Output` บอกเราว่าต้องจ่ายแรงดันให้มอเตอร์ DC เท่าไหร่
ในการจ่ายแรงดันให้มอเตอร์ DC เราใช้ H-Bridge (L293D) ตัวนี้มันควบคุมมอเตอร์ DC ได้สองตัว แต่เราใช้แค่ตัวเดียว ถ้าเราต่อสายตามรูปที่เห็นตอนต้น ช่อง `in_1=5` และ `in_2=6` จะกำหนดทิศทางการหมุน ส่วนช่อง `pwm=9` กำหนดแรงดัน ด้วยคำสั่ง
`analogWrite(pwm, Output);`
ค่า `Output` ที่คำนวณได้ก็จะถูกส่งไปให้มอเตอร์ DC ทำงานตามนั้น
อยากรู้เรื่อง H-Bridge L293D เพิ่มเติม ลองหาอ่านดูได้ มีคนอธิบายไว้เยอะแยะ
### วิธีกำหนดความเร็วเป้าหมาย (Target Speed):
เราจะใช้ตัวต้านทานปรับค่าได้ (Rotary Potentiometer) ในการกำหนดความเร็วเป้าหมาย (ถ้าอยากรู้ว่ามันทำงานยังไง ลองไปหาอ่านเพิ่มเติมเรื่อง potentiometer ดูนะน้อง). สำหรับโปรเจคนี้ เราใช้โค้ดแค่บรรทัดเดียวก็พอแล้วว่ะ!
`Setpoint = analogRead(Pot_pin) * 5.0;`
เราอ่านค่าจากพินอนาล็อก `Pot_pin=A3` แล้วคูณด้วย 5 เพื่อเก็บเป็นค่า "Setpoint" ซึ่งก็คือความเร็วเป้าหมายที่เราตั้งไว้ให้ PID Controller ไงล่ะ
### วิธีแสดงผลให้ดูเท่ๆ:
เราจะใช้จอ LCD ในการโชว์ผลลัพธ์ จอจะแสดงทั้งความเร็วเป้าหมาย (Target Speed) และความเร็วจริง (Actual Speed) โดยใช้ไลบรารี่ช่วย (ลองไปดูตัวอย่างในไลบรารี่ LiquidCrystal_I2C เองนะ จะได้เข้าใจ)
นอกจากนี้ เรายังใช้ Serial Plotter ใน Arduino IDE เพื่อดูกราฟผลลัพธ์แบบสวยๆ อีกด้วย ตามโค้ดด้านล่างเลย:
Serial.print(rpm - Setpoint); Serial.print(","); Serial.print(rpm); Serial.print(","); Serial.print(Setpoint); Serial.print(","); Serial.println(Output);
<figure><img src=https://projects.arduinocontent.cc/abf1e7b9-b594-4573-a1a0-0c26be4e309d.png /><figcaption> </figcaption></figure>
รูปด้านบนแสดงความเร็วเป้าหมาย (Setpoint) เป็นสีเขียว ส่วนความเร็วจริง (r