Người dùng có thể sử dụng các lớp kinematics cho mecanum drive để thực hiện odometry. FTCLib/WPILib chứa một lớp MecanumDriveOdometry có thể được sử dụng để theo dõi vị trí của một robot sử dụng mecanum drive trên sân đấu dựa vào dữ liệu từ các bộ encoder (encoder) ở từng bánh và gyro (gyroscope) để biết hướng robot.

Lưu ý: Vì phương pháp này chỉ sử dụng encoders và gyro, nên độ ước lượng vị trí (pose) của robot sẽ bị sai lệch (drift) theo thời gian, đặc biệt khi robot va chạm trong thi đấu. Tuy nhiên, odometry thường rất chính xác trong giai đoạn autonomous.

Khởi tạo đối tượng Odometry

Lớp MecanumDriveOdometry yêu cầu ba tham số bắt buộc và một tham số tùy chọn:

Tham số bắt buộc:

1. Kinematics object của cơ cấu mecanum (MecanumDriveKinematics) đại diện cho robot mecanum drive của bạn.

2. Góc robot hiện tại (do gyro cung cấp) dưới dạng một đối tượng Rotation2d.

3. Vị trí ban đầu của các bánh xe (wheel positions) dưới dạng đối tượng MecanumDriveWheelPositions — chứa vị trí các bánh (quãng đường) theo đơn vị mét. FIRST Robotics Competition Documentation

Tham số tùy chọn:

• Pose ban đầu của robot trên sân đấu, dưới dạng một đối tượng Pose2d. Nếu không cung cấp, robot được giả định bắt đầu tại:
  x=0,y=0,θ=0x = 0,\quad y = 0,\quad \theta = 0x=0,y=0,θ=0
  Trong đó:

  • x, y là tọa độ vị trí theo mặt phẳng sân;

  • θ (theta) là góc quay robot. FIRST Robotics Competition Documentation

• Quy ước góc:

  • 0 độ/radian biểu thị robot đang hướng trực tiếp về phía trạm đồng minh đối diện.

  • Khi robot quay sang trái (quay ngược chiều kim đồng hồ), giá trị gyro nên tăng lên.

Code mẫu – Khởi tạo Odometry (Java)

// Vị trí các bánh xe tương đối tâm robot (đơn vị: mét)
Translation2d m_frontLeftLocation  = new Translation2d(0.381, 0.381);
Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
Translation2d m_backLeftLocation   = new Translation2d(-0.381, 0.381);
Translation2d m_backRightLocation  = new Translation2d(-0.381, -0.381);
// Tạo đối tượng kinematics
MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics(
    m_frontLeftLocation,
    m_frontRightLocation,
    m_backLeftLocation,
    m_backRightLocation
);
// Tạo đối tượng odometry với wheel positions ban đầu
MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(
    m_kinematics,
    getGyroHeading(),  // Rotation2d từ gyro
    new MecanumDriveWheelPositions(
        m_frontLeftEncoder.getDistance(),
        m_frontRightEncoder.getDistance(),
        m_backLeftEncoder.getDistance(),
        m_backRightEncoder.getDistance()
    ),
    new Pose2d(5.0, 13.5, new Rotation2d()) // Pose2d ban đầu
);

// Vị trí các bánh xe tương đối tâm robot (đơn vị: mét)
Translation2d m_frontLeftLocation  = new Translation2d(0.381, 0.381);
Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
Translation2d m_backLeftLocation   = new Translation2d(-0.381, 0.381);
Translation2d m_backRightLocation  = new Translation2d(-0.381, -0.381);
// Tạo đối tượng kinematics
MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics(
    m_frontLeftLocation,
    m_frontRightLocation,
    m_backLeftLocation,
    m_backRightLocation
);
// Tạo đối tượng odometry với wheel positions ban đầu
MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(
    m_kinematics,
    getGyroHeading(),  // Rotation2d từ gyro
    new MecanumDriveWheelPositions(
        m_frontLeftEncoder.getDistance(),
        m_frontRightEncoder.getDistance(),
        m_backLeftEncoder.getDistance(),
        m_backRightEncoder.getDistance()
    ),
    new Pose2d(5.0, 13.5, new Rotation2d()) // Pose2d ban đầu
);

Cập nhật vị trí robot theo thời gian

Để robot liên tục theo dõi vị trí trong khi hoạt động, bạn cần gọi phương thức update(...) của đối tượng odometry mỗi chu kỳ điều khiển (thường trong periodic() của subsystem).

Phương thức update(...) nhận các tham số:

• Góc robot hiện tại (Rotation2d) từ gyro.

• Trạng thái các bánh xe (MecanumDriveWheelPositions) — vị trí encoder của 4 bánh.

Phương thức sẽ trả về một đối tượng Pose2d đại diện cho pose đã được cập nhật của robot.

Mẫu Java – Cập nhật trong periodic()

@Override
public void periodic() {
    // Lấy vị trí wheel encoder (meters)
    MecanumDriveWheelPositions wheelPositions = new MecanumDriveWheelPositions(
        m_frontLeftEncoder.getDistance(),
        m_frontRightEncoder.getDistance(),
        m_backLeftEncoder.getDistance(),
        m_backRightEncoder.getDistance()
    );
    // Lấy góc robot từ gyro
    Rotation2d gyroAngle = getGyroHeading();
    // Cập nhật pose
    Pose2d currentPose = m_odometry.update(gyroAngle, wheelPositions);
    // currentPose bây giờ chứa (x, y, θ) đã cập nhật của robot
}

@Override
public void periodic() {
    // Lấy vị trí wheel encoder (meters)
    MecanumDriveWheelPositions wheelPositions = new MecanumDriveWheelPositions(
        m_frontLeftEncoder.getDistance(),
        m_frontRightEncoder.getDistance(),
        m_backLeftEncoder.getDistance(),
        m_backRightEncoder.getDistance()
    );
    // Lấy góc robot từ gyro
    Rotation2d gyroAngle = getGyroHeading();
    // Cập nhật pose
    Pose2d currentPose = m_odometry.update(gyroAngle, wheelPositions);
    // currentPose bây giờ chứa (x, y, θ) đã cập nhật của robot
}

Reset lại vị trí robot

Bạn có thể reset pose robot nếu cần, ví dụ khi bắt đầu autonomous hoặc sau khi đặt lại gyro:

Reset Odometry (Java):

// Pose mới muốn reset đến
Pose2d newPose = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(90));
// Góc gyro hiện tại
Rotation2d currentGyro = getGyroHeading();
// Reset odometry
m_odometry.resetPose(newPose, currentGyro);

// Pose mới muốn reset đến
Pose2d newPose = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(90));
// Góc gyro hiện tại
Rotation2d currentGyro = getGyroHeading();
// Reset odometry
m_odometry.resetPose(newPose, currentGyro);

Lưu ý: Nếu bạn reset gyro, phải gọi resetPose(...) ngay sau đó với góc gyro mới để odometry không bị lệch.

Thuật ngữ kỹ thuật (chú giải)

• Odometry: kỹ thuật dùng dữ liệu encoder và gyro để ước lượng vị trí robot theo thời gian.

• MecanumDriveOdometry: lớp theo dõi pose robot mecanum drive.

• MecanumDriveKinematics: lớp mô tả quan hệ giữa ChassisSpeeds và vận tốc bánh trong cơ cấu mecanum.

• Rotation2d: biểu diễn góc quay robot trong mặt phẳng 2D.

• Pose2d: biểu diễn pose robot gồm (x, y, θ).

• MecanumDriveWheelPositions: thể hiện quãng đường (position) của từng bánh xe mecanum.

• Gyro/Gyroscope: cảm biến đo góc quay robot.