การควบคุม RC servo ด้วย PID Control

เป็นเรื่องที่น่าสนใจและเป็นประโยชน์สำหรับผู้ที่สนใจในการพัฒนาโปรเจ็คทางด้าน อิเล็กทรอนิกส์ และ โรบอทิกส์ สำหรับผู้เริ่มต้น มีข้อแนะนำดังนี้:

 

1. เริ่มจากพื้นฐาน:

   • ความหมายของ RC servo และ ทำไมต้องใช้ PID Control
   • ศึกษาการทำงานของ RC servo รวมถึงการเชื่อมต่อไฟฟ้า 

2. ทฤษฎี PID Control:

   • ความหมายของ P, I, D และ วิธีการทำงานของแต่ละส่วน
   • การปรับ PID constants และวิธีการดูแล้วว่า constants ใดทำให้การควบคุมดีขึ้น

3. การเขียนโค้ด:

   • ตัวอย่างโค้ดที่ง่ายสำหรับควบคุม RC servo ด้วย PID
   • อธิบายการทำงานของโค้ดในแต่ละส่วน

4. ปฏิบัติการ:

   • การเชื่อมต่อ RC servo กับ ESP32 และอุปกรณ์อื่น ๆ
   • วิธีการปรับปรุงและประสิทธิภาพในการควบคุม

5. การทดสอบและการปรับแต่ง:

   • วิธีการทดสอบระบบ เช่น การใช้ฟังก์ชันแตกต่าง ๆ และการปรับค่าต่าง ๆ
   • สาเหตุที่ PID ไม่ทำงานดีในบางเวลา และวิธีการแก้ไข

6. เคสตัวอย่างและโปรเจคที่เกี่ยวข้อง:

   • ตัวอย่างโปรเจคที่สามารถประยุกต์ใช้ PID Control กับ RC servo
   • การพัฒนาและประยุกต์ใช้ในความเป็นจริง

      

ความหมายของ RC Servo

1. RC Servo (Radio Control Servo): เป็นประเภทของมอเตอร์ไฟฟ้าที่สามารถควบคุมตำแหน่งได้อย่างแม่นยำ มักใช้ในระบบวิทยุควบคุม (Radio Control) เช่น ในโมเดลเครื่องบิน, รถยนต์ของเล่น, และโรบอต
2. การทำงาน: RC Servo ประกอบด้วยมอเตอร์ขนาดเล็ก, ชุดเกียร์, และหน่วยควบคุมภายใน สามารถหมุน หรือ เคลื่อนที่ไปยังตำแหน่งที่กำหนดได้โดยแม่นยำตามสัญญาณที่ได้รับ
3. การใช้งาน: ใช้กันอย่างแพร่หลายในงานที่ต้องการการเคลื่อนที่แม่นยำ เช่น ในการควบคุมพวงมาลัย, ปีก, หรือตัวควบคุมการเคลื่อนที่อื่นๆ ในโมเดล หรือโรบอต 

ทำไมต้องใช้ PID Control

1. PID (Proportional-Integral-Derivative) Control: เป็นวิธีการควบคุมที่ใช้ประกอบด้วยสามส่วนหลัก คือ Proportional, Integral, และ Derivative

   • Proportional: ปรับการตอบสนองตามขนาดของความผิดพลาด
   • Integral: ตอบสนองตามความผิดพลาดที่สะสมเป็นเวลานาน
   • Derivative: ปรับการตอบสนองตามอัตราการเปลี่ยนแปลงของความผิดพลาด

2. ความจำเป็นในการใช้กับ RC Servo:

   • ความแม่นยำ: PID Control ช่วยให้ RC Servo สามารถเคลื่อนที่ไปยังตำแหน่งที่ต้องการได้อย่างแม่นยำ โดยลดความผิดพลาดและการเกินเป้าหมาย (overshoot)
   • การปรับตัว: ช่วยให้ RC Servo สามารถปรับตัวตามเงื่อนไขต่างๆ เช่น การเปลี่ยนแปลงของโหลดหรือการต่อต้าน
   • ความเสถียร: PID Control ช่วยให้ RC Servo ทำงานได้อย่างเสถียร ลดการสั่นสะเทือนหรือการเคลื่อนที่ที่ไม่ต้องการ

ด้วยเหตุผลเหล่านี้, PID Control จึงเป็นเทคนิคที่มีความสำคัญและมักถูกใช้ในการควบคุม RC Servo ในหลายๆ แอปพลิเคชัน เพื่อให้ได้การเคลื่อนที่ที่แม่นยำและเสถียร

สมการของ PID Control คือส่วนสำคัญที่ช่วยในการควบคุมและปรับตำแหน่งของ RC Servo ให้แม่นยำ สมการ PID ประกอบด้วยสามส่วนหลัก ได้แก่ Proportional, Integral, และ Derivative ซึ่งทำงานร่วมกันเพื่อปรับการตอบสนองของระบบควบคุม ต่อไปนี้คือรายละเอียดของแต่ละส่วน

 

 

 

ตัวอย่างการใช้งาน PID  โดยใช้ Arduino Library

Single RC Servo Control with PID 

//ESP32 CPU with Arduino IDE

 

#include <ESP32_Servo.h>
#include <PID_v1.h>

Servo throttleServo;
const int throttlePin = 14; // The digital pin where the servo is connected
const int sensorPin = 4; // The digital pin where the sensor is connected

const double setPointRPM = 6800.0; // Store setpoint RPM as double

double measuredRPM = 0;
volatile int pulseCount = 0;
unsigned long lastPulseTime = 0;

// PID parameters
double kp = 1.0; // Proportional constant
double ki = 0.1; // Integral constant
double kd = 0.1; // Derivative constant

double throttleOutput; // Declare throttleOutput here

double actualSetPointRPM = setPointRPM; // Create a double variable for setpoint RPM

PID pid(&measuredRPM, &throttleOutput, &actualSetPointRPM, kp, ki, kd, DIRECT);

const int minServoValue = 0; // Minimum servo value
const int maxServoValue = 180; // Maximum servo value

void setup() {
Serial.begin(115200);
throttleServo.attach(throttlePin);
pinMode(sensorPin, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(sensorPin), handleInterrupt, FALLING);

// Set the output limits for the PID controller
pid.SetOutputLimits(minServoValue, maxServoValue);

// Set the mode of the PID controller to AUTOMATIC
pid.SetMode(AUTOMATIC);
}

void loop() {
unsigned long currentTime = millis();
if (currentTime - lastPulseTime >= 1000) {
measuredRPM = pulseCount * (60000.0 / (currentTime - lastPulseTime));
lastPulseTime = currentTime;
pulseCount = 0;

pid.Compute(); // Compute the PID output

throttleServo.write(throttleOutput);

Serial.print("Measured RPM: ");
Serial.print(measuredRPM);
Serial.print("\tThrottle Output: ");
Serial.println(throttleOutput);
printPIDValues(kp, ki, kd); // Print PID gains for debugging
}
}

void handleInterrupt() {
pulseCount++;
}

void printPIDValues(double Kp, double Ki, double Kd) {
Serial.print("P: "); Serial.print(Kp);
Serial.print("\tI: "); Serial.print(Ki);
Serial.print("\tD: "); Serial.println(Kd);
}

 

 

 

Multi RC Servo Control with PID 

 

//ESP32 CPU with Arduino IDE

#include <ESP32_Servo.h>
#include <PID_v1.h>

struct ServoControl {
Servo servo;
int servoPin;
int sensorPin;
double setPointRPM;
double kp;
double ki;
double kd;

// Runtime variables
PID* pid;
double measuredRPM = 0;
volatile int pulseCount = 0;
unsigned long lastPulseTime = 0;
double throttleOutput;

double input;
double output;
double setpoint;

ServoControl(int sPin, int sensor, double sp, double p, double i, double d)
: servoPin(sPin), sensorPin(sensor), setPointRPM(sp), kp(p), ki(i), kd(d) {}
};

ServoControl controls[] = {
ServoControl(14, 4, 6800.0, 1.0, 0.1, 0.1),
ServoControl(15, 5, 6800.0, 1.2, 0.05, 0.2),
ServoControl(16, 17, 6800.0, 1.2, 0.05, 0.2)
};

void IRAM_ATTR handleInterrupt0() { controls[0].pulseCount++; }
void IRAM_ATTR handleInterrupt1() { controls[1].pulseCount++; }
void IRAM_ATTR handleInterrupt2() { controls[2].pulseCount++; }

typedef void (*ISR)(void);
ISR interruptHandlers[] = {
handleInterrupt0,
handleInterrupt1,
handleInterrupt2 // Fixed to correct interrupt handler
};

void initServo(ServoControl &control, int index) {
control.servo.attach(control.servoPin);
pinMode(control.sensorPin, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(control.sensorPin), interruptHandlers[index], FALLING);

control.input = control.measuredRPM;
control.output = control.throttleOutput;
control.setpoint = control.setPointRPM;

control.pid = new PID(&control.input, &control.output, &control.setpoint, control.kp, control.ki, control.kd, DIRECT);
control.pid->SetOutputLimits(0, 180);
control.pid->SetMode(AUTOMATIC);
}

void setup() {
Serial.begin(115200);
for (int i = 0; i < sizeof(controls)/sizeof(controls[0]); i++) {
initServo(controls[i], i);
}
}

void adjustPIDViaSerial() {
if (Serial.available()) {
String input = Serial.readStringUntil('\n');
int servoID;
char parameter;
double value;
sscanf(input.c_str(), "%d,%c,%lf", &servoID, &parameter, &value);

if(servoID >= 0 && servoID < sizeof(controls)/sizeof(controls[0])) {
switch (parameter) {
case 'P': controls[servoID].kp = value; break;
case 'I': controls[servoID].ki = value; break;
case 'D': controls[servoID].kd = value; break;
}

controls[servoID].pid->SetTunings(controls[servoID].kp, controls[servoID].ki, controls[servoID].kd);
Serial.println("PID values updated!");
} else {
Serial.println("Invalid servo ID!");
}
}
}

void loop() {
adjustPIDViaSerial();

for (int i = 0; i < sizeof(controls)/sizeof(controls[0]); i++) {
ServoControl& control = controls[i];
unsigned long currentTime = millis();
if (currentTime - control.lastPulseTime >= 1000) {
control.measuredRPM = control.pulseCount * (60000.0 / (currentTime - control.lastPulseTime));
control.lastPulseTime = currentTime;
control.pulseCount = 0;

control.input = control.measuredRPM;
control.pid->Compute();

control.servo.write(control.output);

Serial.print("Servo #");
Serial.print(i);
Serial.print(" - Measured RPM: ");
Serial.print(control.measuredRPM);
Serial.print("\tThrottle Output: ");
Serial.println(control.output);
}
}
}

 

---