P.I.D - SPEED & POSITION CONTROL

I. TÌM HIỂU VỀ P.I.D

Mục tiêu của bộ điều khiển PID là điều chỉnh giá trị điều khiển ở ngõ ra Ouput sao cho sai lệch Error e(t) = (SP – PV) giữa giá trị đo được của hệ thống PV (Process Variable) với giá trị cài đặt SP (SetPoint) nhỏ nhất có thể (~ 0), đạt được sự ổn định và có đáp ứng nhanh. Các bạn có thể tham khảo chi tiết về P.I.D của Brett Beauregard - tác giả của thư viện PID_v1.

Sơ đồ khối của bộ P.I.D điều khiển động cơ DC Servo Motor cho dự án như sau:

Lấy ví dụ: Ta cài đặt SetPoint tốc độ cho servo motor SP = 2,000 vòng/phút. Hệ thống sẽ phát lệnh cho motor chạy, bộ PID sẽ tính toán dựa vào feedback (hồi tiếp) từ Encoder qua giá trị Process Variable PV, sao cho Error = SP-PV bằng 0 (error ~0). Hay nói cách khác khi ta cài đặt motor chạy 2,000 vòng/ phút, bộ PID sẽ tính toán sao cho motor chạy đúng 2,000 vòng/ phút với sai số nhỏ nhất và đáp ứng nhanh nhất. Trong điều khiển thực tế, giá trị SP này có thể thay đổi liên tục và bộ PID phải có nhiệm vụ thích ứng với thay đổi này.

Cụ thể với DC motor NISCA NF5475 được dùng cho thử nghiệm này, ta có các thông số sau:

• Motor max speed 4,162 rpm (4,162 resolutions per minute): motor chạy tối đa 4,162 vòng/phút.
• Encoder resolution 200 ppr (200 pulses per revolution): motor quay một vòng sẽ tạo ra 200 xung.

1.1. SPEED CONTROL

Như vậy để motor trên chạy với tốc độ cài đặt SP = 2,000 vòng/phút, bộ PID phải điều xung PWM motor hoạt động sao cho Encoder hồi tiếp về xung quanh giá trị PV = 2,000 * 200 =  400,000 xung/phút. Để motor chạy tốc độ tối đa 4,162 vòng/ phút thì bộ PID phải điều khiển sao cho Encoder hồi tiếp về xung quanh giá trị 4,162*200 = 832,400 xung/phút.

Ở đây, thực tế chúng ta điều khiển motor qua biến trở 5Kohm, ứng với 0% (0 rpm) và 100% (4,162 rpm) do vậy ta phải scaling tín hiệu SP và PV sao cho đúng tỉ lệ thì bộ điều khiển PID mới hoạt động đúng. Giá trị tốc độ cài đặt 2,000 vòng/phút ở trên nằm ở khoảng giữa của biến trở 5Kohm.

1.2. POSITION CONTROL

Bài toán đặt ra là làm như thế nào để điều khiển góc quay của motor chính bằng vị trí của biến trở và nó hoạt động đúng khi ta xoay biến trở theo chiều kim đồng hồ hay ngược chiều kim đồng hồ.

Tương tự như trên, motor quay một vòng ứng với một vòng quay của biến trở 5Kohm và sẽ tạo ra 200 xung ở Encoder. Khi biến trở xoay đến vị trí cuối (100%), Arduino sẽ đọc về giá trị là 1,024 và Encoder tạo ra 200 xung. Tỉ lệ giữa giá trị đọc từ biến trở (SetPoint) và xung Encoder (Process Variable) là: 1,024/200 = 5.12.

II. B.O.M

Các thiết bị cần chuẩn bị cho dự án bao gồm:

III. SƠ ĐỒ NGUYÊN LÝ

3.1. SƠ ĐỒ KHỐI CỦA DỰ ÁN

Ở đây lưu ý:

• Tín hiệu Encoder hồi tiếp được đấu nối trực tiếp về Arduino chứ không nối về DC Motor Drive.
• Arduino điều khiển DC motor servo bằng phương pháp điều xung qua biến trở & mạch cầu H - L298N.
• DC motor servo sẽ được Arduino điều khiển ở hay chế độ: SPEED (TỐC ĐỘ) & POSITION (VỊ TRÍ).

3.2. SƠ ĐỒ ĐẤU NỐI

Lưu ý: Đối với DC Servo Motor NISCA NF5475E, Encoder cần phải được cấp nguồn 5VDC và nó có hai kênh A, B.

IV. CHƯƠNG TRÌNH

4.1. THƯ VIỆN PID_v1

• Đầu tiên, các bạn phải import thư viện PID_v1 vào Arduino IDE với tên “PID by Brett Beauregard”. Chi tiết về cách sử dụng thư viện này các bạn tham khảo tại:

http://playground.arduino.cc/Code/PIDLibrary

• Thư viện PinChangeInt cũng phải được import vào Arduino IDE. Tham khảo tại: 

https://github.com/GreyGnome/PinChangeInt

• Chương trình Speed Control và Position Control được tham khảo tại: 

https://github.com/raydike

4.2. SPEED CONTROL

Link Youtube: https://www.youtube.com/watch?v=sM2yTyHvafc&list=PLfrr5s2He43DA7gLnXX5XRed0wyqBtcz-&index=2

Chương trình: 

#include "PinChangeInt.h"
#include <PID_v1.h>                                   // Thanks to Brett Beauregard for his nice PID library
#define encodPinA1      2                             // Quadrature encoder A pin
#define encodPinB1      8                             // Quadrature encoder B pin
#define M1              9                             // PWM outputs to L298N H-Bridge motor driver module
#define M2              10
double kp = 5 , ki = 1 , kd = 0.01 ,input = 0, output = 0, setpoint = 0;   // modify kp, ki and kd for optimal performance
long temp;
volatile long encoderPos = 0;
PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT);  // if motor will only run at full speed try 'REVERSE' instead of 'DIRECT'

void setup() {
  pinMode(encodPinA1, INPUT_PULLUP);                  // quadrature encoder input A
  pinMode(encodPinB1, INPUT_PULLUP);                  // quadrature encoder input B
  attachInterrupt(0, encoder, FALLING);               // update encoder position
  TCCR1B = TCCR1B & 0b11111000 | 1;                   // set 31KHz PWM to prevent motor noise
  myPID.SetMode(AUTOMATIC);
  myPID.SetSampleTime(1);
  myPID.SetOutputLimits(-255, 255);
}
void loop() {
  temp += analogRead(0);                              // increment position target with potentiometer value (speed), potmeter connected to A0
  if (temp < 0) {                                     // in case of overflow
    encoderPos = 0;
    temp = 0;
  }
  setpoint = temp / 500;                              // modify division to fit motor and encoder characteristics
  input = encoderPos ;                                // data from encoder
  myPID.Compute();                                    // calculate new output
  pwmOut(output);                                     // drive L298N H-Bridge module
}
void pwmOut(int out) {                                // to H-Bridge board
  if (out > 0) {
    analogWrite(M1, out);                             // drive motor CW        
    analogWrite(M2, 0);                                              
  }
  else {
    analogWrite(M1, 0);                         
    analogWrite(M2, abs(out));                        // drive motor CCW
  }
}
void encoder()  {                                     // pulse and direction, direct port reading to save cycles
  if (PINB & 0b00000001)    encoderPos++;             // if (digitalRead(encodPinB1)==HIGH)  count ++;
  else                      encoderPos--;             // if (digitalRead(encodPinB1)==LOW)   count --;
}

4.3. POSITION CONTROL

Link Youtube: https://www.youtube.com/watch?v=Whsgfi3JGYM&index=1&list=PLfrr5s2He43DA7gLnXX5XRed0wyqBtcz-

Chương trình:

// PID motor position control.
// Thanks to Brett Beauregard for his nice PID library
#include <PinChangeInt.h>
#include <PID_v1.h>
#define encodPinA1      2                       // Quadrature encoder A pin
#define encodPinB1      8                       // Quadrature encoder B pin
#define M1              9                       // PWM outputs to L298N H-Bridge motor driver module
#define M2              10

double kp = 5 , ki = 1 , kd = 0.01;             // modify for optimal performance
double input = 0, output = 0, setpoint = 0;
long temp;
volatile long encoderPos = 0;
PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT);  // if motor will only run at full speed try 'REVERSE' instead of 'DIRECT'

void setup() {
  pinMode(encodPinA1, INPUT_PULLUP);                  // quadrature encoder input A
  pinMode(encodPinB1, INPUT_PULLUP);                  // quadrature encoder input B
  attachInterrupt(0, encoder, FALLING);               // update encoder position
  TCCR1B = TCCR1B & 0b11111000 | 1;                   // set 31KHz PWM to prevent motor noise
  myPID.SetMode(AUTOMATIC);
  myPID.SetSampleTime(1);
  myPID.SetOutputLimits(-255, 255);
  Serial.begin (115200);                              // for debugging
}

void loop() {
  setpoint = analogRead(0) / 5;                       // modify to fit motor and encoder characteristics, potmeter connected to A0
  input = encoderPos ;                                // data from encoder
  // Serial.println(encoderPos);                      // monitor motor position
  myPID.Compute();                                    // calculate new output
  pwmOut(output);                                     // drive L298N H-Bridge module
}

void pwmOut(int out) {                                // to H-Bridge board
  if (out > 0) {
    analogWrite(M1, out);                             // drive motor CW
    analogWrite(M2, 0);
  }
  else {
    analogWrite(M1, 0);
    analogWrite(M2, abs(out));                        // drive motor CCW
  }
}

void encoder()  {                                     // pulse and direction, direct port reading to save cycles
  if (PINB & 0b00000001)    encoderPos++;             // if(digitalRead(encodPinB1)==HIGH)   count ++;
  else                      encoderPos--;             // if(digitalRead(encodPinB1)==LOW)   count --;
}

V. LỜI KẾT

• Trong điều khiển P.I.D, ngoài các thông số cơ bản P, I, D còn có một thông số cực kỳ quan trọng khác là SampleTime. Trong thư viện PID_v1, giá trị thời gian lấy mẫu SampleTime mặc định là 100 ms.
• Một cách đơn giản nhất để hiểu về P.I.D như sau
  • P: Điều chỉnh tỉ lệ tạo ra tín hiệu điều chỉnh tỉ lệ với sai lệch đầu vào theo thời gian lấy mẫu.
  • I: tích phân của sai lệch theo thời gian lấy mẫu. Điều chỉnh tích phân cho ta biết tổng sai số tức thời theo thời gian hay sai số tích lũy trong quá khứ.
  • D: vi phân của sai lệch theo thời gian lấy mẫu. Điều chỉnh vi phân tạo ra tín hiệu điều chỉnh sao cho tỉ lệ với tốc độ thay đổi sai lệch đầu vào.
• Bộ điều khiển PID được coi là bộ điều khiển lý tưởng đối với các đối tượng có mô hình liên tục. Do vậy, việc trang bị module điều khiển PID bằng phần cứng hay phần mềm luôn được các hãng sản xuất thiết bị tự động trên thế giới tích hợp vào thiết bị của họ.