package com.arcrobotics.ftclib.kinematics

package com.arcrobotics.ftclib.kinematics

FTCLib cung cấp các lớp odometry riêng để sử dụng cho các hệ truyền động differential (dẫn động vi sai) và holonomic (dẫn động đa hướng).
Các lớp odometry này theo dõi vị trí của robot dưới dạng một Pose2d, nghĩa là vị trí được biểu diễn bằng vector:

Trong đó:

• x: khoảng cách robot di chuyển theo hướng tiến về phía trước

• y: khoảng cách robot di chuyển theo phương ngang

• θ (theta): hướng quay (heading) của robot

Khi sử dụng các lớp này, điều quan trọng là:

• Góc phải được biểu diễn bằng radian

• Khoảng cách phải sử dụng cùng một đơn vị đo nhất quán (ví dụ: inch)

Pose Exponential

Pose Exponential là một thuật ngữ chung trong FRC dùng để chỉ phương pháp odometry với vận tốc không đổi được áp dụng trong FTC.

FTCLib sử dụng pose exponentials vì thời gian chu kỳ (cycle time) của các vòng điều khiển trong FTC có thể thay đổi đáng kể.

Phương pháp này sử dụng các phương trình vi phân để giải bài toán xác định vị trí phi tuyến của robot khi biết độ cong (curvature) không đổi.

Vì lý do đó, phần toán học chi tiết không được trình bày tại đây.

Offsets và Trackwidth

• Trackwidth:
  Là khoảng cách giữa hai encoder song song.
  Giá trị này bắt buộc phải được truyền vào constructor và cần được tuning để đạt độ chính xác cao.

• Center wheel offset:
  Là khoảng cách từ tâm quay của robot đến vị trí của encoder ngang (lateral encoder).
  Thông số này chỉ cần thiết cho holonomic odometry.

Cách điều chỉnh:

• Có thể ước lượng ban đầu bằng đo đạc thực tế

• Sau đó chạy thử và tinh chỉnh cho đến khi dữ liệu odometry trở nên ổn định và chính xác

• Một cách khác là bắt đầu với giá trị lớn (ví dụ: 18) rồi dần điều chỉnh để hội tụ về giá trị phù hợp

Tạo Odometry

FTCLib cung cấp các ví dụ mẫu về cách sử dụng odometry, bao gồm cả dead wheel odometry.

Sử dụng lớp Odometry

Có ba constructor khác nhau cho lớp odometry, tùy thuộc vào cách bạn muốn cập nhật dữ liệu.

Cách 1: Sử dụng Supplier

• Truyền supplier (hàm lambda) để odometry tự động lấy dữ liệu encoder

• Chỉ cần gọi updatePose()

Cách 2: Cập nhật thủ công

• Gọi trực tiếp update()

• Truyền vào giá trị encoder trái, phải, và encoder ngang (nếu là holonomic)

Nếu bạn dùng supplier, updatePose() sẽ tự động gọi update() bên trong.

Ví dụ mã (Java)

// define our trackwidth
static final double TRACKWIDTH = 13.7;
// convert ticks to inches
static final double TICKS_TO_INCHES = 15.3;
// create our encoders
MotorEx encoderLeft, encoderRight;
encoderLeft = new MotorEx(hardwareMap, "left_encoder");
encoderRight = new MotorEx(hardwareMap, "right_encoder");
// create our odometry
DifferentialOdometry diffOdom = new DifferentialOdometry(
    () -> encoderLeft.getCurrentPosition() * TICKS_TO_INCHES,
    () -> encoderRight.getCurrentPosition() * TICKS_TO_INCHES,
    TRACKWIDTH
);
// update the initial position
diffOdom.updatePose(new Pose2d(1, 2, 0));
// control loop
while (!isStopRequested()) {
    /* implementation */
    // update the position
    diffOdom.updatePose();
}

// define our trackwidth
static final double TRACKWIDTH = 13.7;
// convert ticks to inches
static final double TICKS_TO_INCHES = 15.3;
// create our encoders
MotorEx encoderLeft, encoderRight;
encoderLeft = new MotorEx(hardwareMap, "left_encoder");
encoderRight = new MotorEx(hardwareMap, "right_encoder");
// create our odometry
DifferentialOdometry diffOdom = new DifferentialOdometry(
    () -> encoderLeft.getCurrentPosition() * TICKS_TO_INCHES,
    () -> encoderRight.getCurrentPosition() * TICKS_TO_INCHES,
    TRACKWIDTH
);
// update the initial position
diffOdom.updatePose(new Pose2d(1, 2, 0));
// control loop
while (!isStopRequested()) {
    /* implementation */
    // update the position
    diffOdom.updatePose();
}

Bạn nên gọi hàm cập nhật mỗi chu kỳ của vòng điều khiển (control loop).

Sử dụng Odometry Subsystem

OdometrySubsystem là một template subsystem được thiết kế để hỗ trợ lập trình theo mô hình command-based với odometry.

Ưu điểm:

• Vị trí robot được tự động cập nhật trong phương thức periodic()

• Độ chính xác cao hơn so với cập nhật thủ công

• Bắt buộc phải sử dụng constructor dựa trên supplier

Bạn cũng có thể tự xây dựng subsystem odometry riêng nếu muốn kiểm soát toàn bộ logic.

Liên quan

Lớp PurePursuitCommand sử dụng OdometrySubsystem để theo dõi và điều khiển quỹ đạo của robot.

Chú giải thuật ngữ

• Odometry: kỹ thuật ước lượng vị trí robot dựa trên encoder

• Pose2d: cấu trúc dữ liệu lưu vị trí (x, y) và hướng (θ)

• Differential drive: hệ truyền động vi sai (2 bánh chủ động)

• Holonomic drive: hệ truyền động cho phép di chuyển theo mọi hướng

• Supplier: hàm cung cấp dữ liệu động (Java functional interface)

Dead wheel: bánh encoder không truyền lực, chỉ đo chuyển động