package com.arcrobotics.ftclib.controller

package com.arcrobotics.ftclib.controller

Trong FTCLib, có các bộ điều khiển (controllers) giúp cải thiện chuyển động của các cơ cấu trong FTC. Điều này bao gồm điều khiển PID (PID control) và điều khiển feedforward (feedforward control). Để tìm hiểu về cơ sở lý thuyết của điều khiển PID, chúng tôi khuyến nghị đọc trang này trên gm0.

PID Control

Bài viết sau của Noah trên FTC Discord giải thích PID control một cách rõ ràng nhất.

Bạn sẽ nghe thấy thuật ngữ PID Controller được sử dụng rất nhiều (đôi khi có thêm chữ F) trong robotics. Nó tương đối đơn giản và khá hiệu quả cho nhiều tác vụ đơn giản. Một PID controller là một dạng điều khiển vòng kín (closed loop control). Điều này về cơ bản có nghĩa là bạn thay đổi một đầu vào của một “plant” dựa trên phản hồi (feedback).

Khái niệm này áp dụng cho rất nhiều loại hành động, nhưng ở đây chúng ta sẽ xem xét điều khiển PID vận tốc (velocity PID control) vì đó là nội dung liên quan đến trò chơi của năm nay.

Giả sử bạn có một động cơ goBILDA 3:1 1620RPM dùng để quay một bánh đà (flywheel). Bạn muốn bánh đà này quay với vận tốc không đổi để đảm bảo sự nhất quán giữa các lần bắn. Vì vậy, bạn gọi
motor.setPower(0.5)
điều này gửi 50% của 12V tới động cơ. Động cơ nhận được tín hiệu 6V (thực tế không hoàn toàn đúng vì PWM, nhưng đó là một chủ đề khác).

Động cơ sẽ quay ở 810 RPM đúng không? Vì đó là 50% của 1620 RPM. Nhưng rất có thể là không. Các động cơ có sai số khoảng ±10% giữa từng chiếc. Đường cong điện áp – mô-men (voltage-torque curve) không tuyến tính. Hoặc có một lực cản tác động lên động cơ (chẳng hạn như quán tính của bánh đà), khiến nó cần thêm công suất để đạt được vận tốc đó.

Vậy làm thế nào để đảm bảo rằng động cơ thực sự quay chính xác ở 810 RPM? Hầu hết các động cơ FTC đều có encoder tích hợp. Điều này cho phép chúng ta đo vận tốc của trục đầu ra. Khi encoder đã được kết nối, ta biết rằng động cơ chưa quay nhanh như mong muốn. Nhưng làm thế nào để hiệu chỉnh điều đó? Bạn gắn một PID Controller vào.

PID controller là một bộ điều khiển khá cơ bản (mặc dù phương trình của nó trông có vẻ đáng sợ khi bạn tra cứu) và về cơ bản nó phản ứng dựa trên sai lệch (error) giữa vận tốc đo được và vận tốc mong muốn, rồi tăng hoặc giảm công suất dựa trên sai lệch đó. Bạn chỉ cần kiểm tra vận tốc ở mỗi vòng lặp, đưa giá trị đó vào bộ điều khiển, và nó sẽ trả về công suất cần thiết để đặt động cơ đạt tới vận tốc mong muốn.

Video sau đây giải thích rất tốt ý nghĩa của từng hệ số (gain):Simple Examples of PID Control

Lớp cơ sở của chúng tôi cho sơ đồ điều khiển PID trong FTCLib là PIDFController. Lớp này thực hiện các phép tính cho PIDF, bao gồm các thành phần tỷ lệ (proportional), tích phân (integral), vi phân (derivative) và feedforward. Thành phần F bổ sung là một hệ số khuếch đại thêm, dùng để tạo độ lệch (offset) cho các mục đích như giữ vị trí, chống lại trọng lực/khối lượng, hoặc vượt qua ma sát.

Using the PIDFController Class

Constructing a PIDFController

Để sử dụng chức năng điều khiển PIDF của FTCLib, người dùng trước tiên phải tạo một đối tượng PIDFController với các hệ số mong muốn:

// Creates a PIDFController with gains kP, kI, kD, and kF
PIDFController pidf = new PIDFController(kP, kI, kD, kF);
/*
 * Here are the constructors for the other controllers
 */
PIDController pid = new PIDController(kP, kI, kD);
PDController pd = new PDController(kP, kD);
PController p = new PController(kP);

// Creates a PIDFController with gains kP, kI, kD, and kF
PIDFController pidf = new PIDFController(kP, kI, kD, kF);
/*
 * Here are the constructors for the other controllers
 */
PIDController pid = new PIDController(kP, kI, kD);
PDController pd = new PDController(kP, kD);
PController p = new PController(kP);

Bạn cũng có thể truyền thêm hai tham số: setpoint và previous value. Giá trị mặc định của chúng là 0.

Bạn cũng có thể thay đổi các hệ số (gain constants) ngay cả sau khi đã tạo đối tượng controller.

// set our gains to some value
pidf.setP(0.37);
pidf.setI(0.05);
pidf.setD(1.02);
// get our gain constants
kP = pidf.getP();
kI = pidf.getI();
kD = pidf.getD();
// set all gains
pidf.setPIDF(kP, KI, kD, 0.7);
// get all gain coefficients
double[] coeffs = pidf.getCoefficients();
kP = coeffs[0];
kI = coeffs[1];
kD = coeffs[2];
kF = coeffs[3];

// set our gains to some value
pidf.setP(0.37);
pidf.setI(0.05);
pidf.setD(1.02);
// get our gain constants
kP = pidf.getP();
kI = pidf.getI();
kD = pidf.getD();
// set all gains
pidf.setPIDF(kP, KI, kD, 0.7);
// get all gain coefficients
double[] coeffs = pidf.getCoefficients();
kP = coeffs[0];
kI = coeffs[1];
kD = coeffs[2];
kF = coeffs[3];

Các hệ số này phải được tinh chỉnh (tuned). Một ví dụ về việc tinh chỉnh PID có thể được tìm thấy trên trang này của Learn Road Runner.

Using the Feedback Loop Output

Phương thức calculate() nên được gọi ở mỗi lần lặp của vòng điều khiển. Bộ điều khiển sử dụng dấu thời gian (timestamps) để tính khoảng chênh lệch thời gian giữa mỗi lần gọi phương thức, điều này có nghĩa là nó tự điều chỉnh theo thời gian vòng lặp. Bạn có thể lấy thời gian chu kỳ của vòng lặp hiện tại bằng cách gọi getPeriod().

Việc sử dụng PIDFController đã tạo rất đơn giản: gọi phương thức calculate() từ vòng lặp chính.

// Calculates the output of the PIDF algorithm based on sensor
// readings. Requires both the measured value
// and the desired setpoint
double output = pidf.calculate(
    motor.getCurrentPosition(), setpoint
);
/*
 * A sample control loop for a motor
 */
PController pController = new PController(kP);
// We set the setpoint here.
// Now we don't have to declare the setpoint
// in our calculate() method arguments.
pController.setSetPoint(1200);
// perform the control loop
/*
 * The loop checks to see if the controller has reached
 * the desired setpoint within a specified tolerance
 * range
 */
while (!pController.atSetPoint()) {
  double output = pController.calculate(
    m_motor.getCurrentPosition()  // the measured value
  );
  m_motor.setVelocity(output);
}
m_motor.stopMotor(); // stop the motor
// NOTE: motors have internal PID control

// Calculates the output of the PIDF algorithm based on sensor
// readings. Requires both the measured value
// and the desired setpoint
double output = pidf.calculate(
    motor.getCurrentPosition(), setpoint
);
/*
 * A sample control loop for a motor
 */
PController pController = new PController(kP);
// We set the setpoint here.
// Now we don't have to declare the setpoint
// in our calculate() method arguments.
pController.setSetPoint(1200);
// perform the control loop
/*
 * The loop checks to see if the controller has reached
 * the desired setpoint within a specified tolerance
 * range
 */
while (!pController.atSetPoint()) {
  double output = pController.calculate(
    m_motor.getCurrentPosition()  // the measured value
  );
  m_motor.setVelocity(output);
}
m_motor.stopMotor(); // stop the motor
// NOTE: motors have internal PID control

Checking Errors

Các phương thức getPositionError() và getVelocityError() được đặt tên với giả định rằng vòng lặp đang điều khiển vị trí (position). Đối với một vòng lặp điều khiển vận tốc (velocity), các phương thức này lần lượt trả về sai số vận tốc (velocity error) và sai số gia tốc (acceleration error).

Sai số hiện tại của biến quá trình được đo được trả về bởi hàm getPositionError(), trong khi đạo hàm của nó được trả về bởi hàm getVelocityError().

Specifying and Checking Tolerances

Nếu chỉ chỉ định dung sai vị trí (position tolerance), thì dung sai vận tốc (velocity tolerance) mặc định sẽ là vô hạn.

Như đã nói ở trên, “position” ở đây ám chỉ giá trị đo của biến quá trình, và “velocity” là đạo hàm của nó — do đó, đối với một vòng lặp vận tốc, chúng thực chất lần lượt là vận tốc và gia tốc.

Đôi khi, việc biết liệu bộ điều khiển đã bám theo setpoint trong một dung sai nhất định hay chưa là rất hữu ích — ví dụ, để xác định xem một lệnh có nên kết thúc hay không, hoặc (khi đang theo dõi một motion profile) để xác định xem chuyển động có bị cản trở và cần lập kế hoạch lại hay không.

Để làm điều này, trước tiên ta phải chỉ định dung sai bằng phương thức setTolerance(), sau đó có thể kiểm tra bằng phương thức atSetPoint().

// Sets the error tolerance to 5, and the error derivative
// tolerance to 10 per second
pidf.setTolerance(5, 10);
// Returns true if the error is less than 5 units, and the
// error derivative is less than 10 units
pidf.atSetPoint();

// Sets the error tolerance to 5, and the error derivative
// tolerance to 10 per second
pidf.setTolerance(5, 10);
// Returns true if the error is less than 5 units, and the
// error derivative is less than 10 units
pidf.atSetPoint();

Resetting the Controller

Đôi khi, việc xóa trạng thái nội bộ (quan trọng nhất là bộ tích lũy tích phân – integral accumulator) của một PIDFController là cần thiết, vì trạng thái đó có thể không còn hợp lệ (ví dụ: khi PIDFController bị vô hiệu hóa rồi được bật lại). Điều này có thể thực hiện bằng cách gọi phương thức reset().

Feedforward Control

Cho đến nay, chúng ta đã sử dụng điều khiển phản hồi (feedback control) để bám theo giá trị tham chiếu (reference tracking). Mặc dù hiệu quả, nhưng đây là một biện pháp mang tính phản ứng; hệ thống sẽ không bắt đầu tác động điều khiển cho đến khi nó đã bị chậm so với mục tiêu.

Nếu chúng ta có thể cho bộ điều khiển biết trước chuyển động mong muốn và đầu vào cần thiết, hệ thống có thể phản ứng nhanh hơn và bộ điều khiển phản hồi sẽ phải làm ít việc hơn. Một bộ điều khiển đưa thông tin đi trước vào plant như vậy được gọi là feedforward controller.

Feedforward controller đưa thông tin về động lực học của hệ thống (system dynamics) (giống như một mô hình toán học) hoặc chuyển động dự định. Feedforward xử lý những phần của hành động điều khiển mà ta đã biết chắc cần phải áp dụng để hệ thống bám theo tham chiếu, sau đó feedback sẽ bù cho những gì ta không biết hoặc không thể biết về hành vi của hệ thống tại thời điểm chạy.

Có hai loại feedforward:

• Feedforward dựa trên mô hình (model-based feedforward)

• Feedforward cho động lực học chưa được mô hình hóa (feedforward for unmodeled dynamics)

Loại thứ nhất giải một mô hình toán học của hệ thống để tìm đầu vào cần thiết nhằm đạt được vận tốc và gia tốc mong muốn. Loại thứ hai bù trực tiếp cho các lực hoặc hành vi chưa được mô hình hóa để bộ điều khiển feedback không phải xử lý. Cả hai loại đều có thể giúp đơn giản hóa bộ điều khiển feedback.

FTCLib cung cấp nhiều lớp để giúp người dùng triển khai feedforward control chính xác cho các cơ cấu của họ. Trên nhiều phương diện, một feedforward chính xác còn quan trọng hơn feedback đối với việc điều khiển hiệu quả một cơ cấu. Vì hầu hết các cơ cấu FTC tuân theo các phương trình hệ thống đã được hiểu rõ, nên việc bắt đầu với một feedforward chính xác vừa dễ dàng vừa mang lại lợi ích rất lớn cho điều khiển chính xác và ổn định.

Hiện tại, FTCLib cung cấp ba lớp hỗ trợ feedforward sau đây. Các thành phần feedforward sẽ tính toán đầu ra theo đơn vị được xác định bởi đơn vị của các hệ số feedforward do người dùng cung cấp. Người dùng phải cẩn thận giữ cho các đơn vị nhất quán, vì thư viện không có hệ thống đơn vị an toàn kiểu (type-safe unit system).

SimpleMotorFeedforward

Lớp SimpleMotorFeedforward tính toán feedforward cho các cơ cấu bao gồm động cơ DC nam châm vĩnh cửu (permanent-magnet DC motors) không có tải ngoài nào ngoài ma sát và quán tính, chẳng hạn như bánh đà và hệ truyền động robot.

Để tạo một SimpleMotorFeedforward, chỉ cần khởi tạo nó với các hệ số cần thiết:

// Create a new SimpleMotorFeedforward with gains kS, kV, and kA
SimpleMotorFeedforward feedforward =
    new SimpleMotorFeedforward(kS, kV, kA);

// Create a new SimpleMotorFeedforward with gains kS, kV, and kA
SimpleMotorFeedforward feedforward =
    new SimpleMotorFeedforward(kS, kV, kA);

Lưu ý rằng hệ số kA là tùy chọn. Nếu cơ cấu không có nhiều quán tính, thì không cần thiết.

Để tính feedforward, chỉ cần gọi phương thức calculate() với vận tốc và gia tốc mong muốn của động cơ:

// Calculates the feedforward for a velocity of 10 units/second
// and an acceleration of 20 units/second^2
// Units are determined by the units of the gains passed
// in at construction.
feedforward.calculate(10, 20);

// Calculates the feedforward for a velocity of 10 units/second
// and an acceleration of 20 units/second^2
// Units are determined by the units of the gains passed
// in at construction.
feedforward.calculate(10, 20);

ArmFeedforward

Lớp ArmFeedforward tính toán feedforward cho các cánh tay (arms) được điều khiển trực tiếp bởi động cơ DC nam châm vĩnh cửu, với tải ngoài gồm ma sát, quán tính và khối lượng của cánh tay. Đây là mô hình chính xác cho hầu hết các cánh tay trong FTC.

Để tạo một ArmFeedforward, chỉ cần khởi tạo nó với các hệ số cần thiết:

// Create a new ArmFeedforward with gains kS, kCos, kV, and kA
ArmFeedforward feedforward = new ArmFeedforward(kS, kCos, kV, kA);

// Create a new ArmFeedforward with gains kS, kCos, kV, and kA
ArmFeedforward feedforward = new ArmFeedforward(kS, kCos, kV, kA);

Để tính feedforward, chỉ cần gọi phương thức calculate() với vị trí, vận tốc và gia tốc mong muốn của cánh tay:

// Calculates the feedforward for a position of 1 units,
// a velocity of 2 units/second, and
// an acceleration of 3 units/second^2
// Units are determined by the units of the gains passed
// in at construction.
feedforward.calculate(1, 2, 3);

// Calculates the feedforward for a position of 1 units,
// a velocity of 2 units/second, and
// an acceleration of 3 units/second^2
// Units are determined by the units of the gains passed
// in at construction.
feedforward.calculate(1, 2, 3);

ElevatorFeedforward

Lớp ElevatorFeedforward tính toán feedforward cho các cơ cấu nâng (elevators) bao gồm động cơ DC nam châm vĩnh cửu chịu tải bởi ma sát, quán tính và khối lượng của cơ cấu nâng. Đây là mô hình chính xác cho hầu hết các elevator trong FTC.

Để tạo một ElevatorFeedforward, chỉ cần khởi tạo nó với các hệ số cần thiết:

// Create a new ElevatorFeedforward with gains kS, kG, kV, and kA
ElevatorFeedforward feedforward = new ElevatorFeedforward(
    kS, kG, kV, kA
);

// Create a new ElevatorFeedforward with gains kS, kG, kV, and kA
ElevatorFeedforward feedforward = new ElevatorFeedforward(
    kS, kG, kV, kA
);

Để tính feedforward, chỉ cần gọi phương thức calculate() với vận tốc và gia tốc mong muốn của động cơ:

// Calculates the feedforward for a position of 10 units,
// velocity of 20 units/second,
// and an acceleration of 30 units/second^2
// Units are determined by the units of the gains passed
// in at construction.
feedforward.calculate(10, 20, 30);

// Calculates the feedforward for a position of 10 units,
// velocity of 20 units/second,
// and an acceleration of 30 units/second^2
// Units are determined by the units of the gains passed
// in at construction.
feedforward.calculate(10, 20, 30);

Using Feedforward to Control a Mechanism

Điều khiển feedforward có thể được sử dụng hoàn toàn độc lập, không cần bộ điều khiển feedback. Điều này được gọi là điều khiển vòng hở (open-loop control), và đối với nhiều cơ cấu (đặc biệt là hệ truyền động robot), cách này có thể hoàn toàn thỏa đáng. Một SimpleMotorFeedforward có thể được sử dụng để điều khiển hệ truyền động robot như sau:

public void tankDriveWithFeedforward(double leftVelocity,
                                     double rightVelocity) {
  leftMotor.set(feedforward.calculate(leftVelocity));
  rightMotor.set(feedforward.calculate(rightVelocity));
}

public void tankDriveWithFeedforward(double leftVelocity,
                                     double rightVelocity) {
  leftMotor.set(feedforward.calculate(leftVelocity));
  rightMotor.set(feedforward.calculate(rightVelocity));
}