import com.arcrobotics.ftclib.kinematics.wpilibkinematics.SwerveDriveOdometry

import com.arcrobotics.ftclib.kinematics.wpilibkinematics.SwerveDriveOdometry

Người dùng có thể sử dụng các lớp kinematics cho swerve drive để thực hiện odometry. FTCLib/WPILib cung cấp lớp SwerveDriveOdometry để theo dõi vị trí robot swerve drive trên sân đấu bằng cách sử dụng dữ liệu từ các encoder module và một gyro để biết hướng robot.

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

Tạo đối tượng Odometry

Lớp SwerveDriveOdometry yêu cầu hai tham số bắt buộc và một tham số tùy chọn khi khởi tạo:

Tham số bắt buộc:

1. Đối tượng kinematics đại diện cho swerve drive của bạn (dạng SwerveDriveKinematics).

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

Tham số tùy chọn:

• Pose ban đầu của robot trên sân đấu (dạng Pose2d). Nếu không cung cấp, robot sẽ bắt đầu với: x=0,  y=0,  θ=0x = 0,\; y = 0,\; \theta = 0x=0,y=0,θ=0. Trong đó θ là hướng quay robot.

• Quy ước góc:

  • 0 độ / radian đại diện cho robot hướng trực tiếp về phía địch.

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

Mẫu Code – Khởi tạo và cập nhật Odometry

Dưới đây là ví dụ mẫu bằng Java để khởi tạo và sử dụng SwerveDriveOdometry:

// Định nghĩa vị trí các module swerve 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
SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
    m_frontLeftLocation, m_frontRightLocation,
    m_backLeftLocation, m_backRightLocation
);
// Tạo odometry với kinematics, gyro và pose ban đầu
SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(
    m_kinematics,
    getGyroHeading(),                       // Rotation2d từ gyro
    new Pose2d(5.0, 13.5, new Rotation2d()) // Pose2d ban đầu
);

// Định nghĩa vị trí các module swerve 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
SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
    m_frontLeftLocation, m_frontRightLocation,
    m_backLeftLocation, m_backRightLocation
);
// Tạo odometry với kinematics, gyro và pose ban đầu
SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(
    m_kinematics,
    getGyroHeading(),                       // Rotation2d từ gyro
    new Pose2d(5.0, 13.5, new Rotation2d()) // Pose2d ban đầu
);

Cập nhật vị trí robot

Odometry cần được cập nhật định kỳ để theo dõi vị trí robot liên tục (thường là trong phương thức periodic() của Subsystem).

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

@Override
public void periodic() {
    // Lấy góc robot từ gyro
    Rotation2d gyroAngle = getGyroHeading();
    // Lấy trạng thái của từng module swerve (tốc độ + góc)
    SwerveModuleState flState = frontLeftModule.getState();
    SwerveModuleState frState = frontRightModule.getState();
    SwerveModuleState blState = backLeftModule.getState();
    SwerveModuleState brState = backRightModule.getState();
    // Cập nhật odometry
    Pose2d currentPose = m_odometry.update(
        gyroAngle,
        flState,
        frState,
        blState,
        brState
    );
    // currentPose bây giờ là vị trí đã cập nhật của robot
}

@Override
public void periodic() {
    // Lấy góc robot từ gyro
    Rotation2d gyroAngle = getGyroHeading();
    // Lấy trạng thái của từng module swerve (tốc độ + góc)
    SwerveModuleState flState = frontLeftModule.getState();
    SwerveModuleState frState = frontRightModule.getState();
    SwerveModuleState blState = backLeftModule.getState();
    SwerveModuleState brState = backRightModule.getState();
    // Cập nhật odometry
    Pose2d currentPose = m_odometry.update(
        gyroAngle,
        flState,
        frState,
        blState,
        brState
    );
    // currentPose bây giờ là vị trí đã cập nhật của robot
}

• Ghi chú: Thứ tự các SwerveModuleState truyền vào phải đúng thứ tự bạn dùng khi tạo SwerveDriveKinematics.

Reset vị trí robot

Bạn có thể reset lại pose robot bất kỳ lúc nào bằng phương thức resetPose(...).

Mẫu Java – Reset Odometry:

// Pose mới muốn reset đến
Pose2d newPose = new Pose2d(2.0, 3.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(2.0, 3.0, Rotation2d.fromDegrees(90));
// Góc gyro hiện tại
Rotation2d currentGyro = getGyroHeading();
// Reset odometry
m_odometry.resetPose(newPose, currentGyro);

• Nếu bạn reset gyro, phải gọi resetPose(...) ngay sau đó với góc gyro mới để tránh sai lệch pose.

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

• Swerve Drive Odometry: kỹ thuật odometry áp dụng cho robot swerve drive để theo dõi vị trí robot theo thời gian thực.

• SwerveDriveKinematics: lớp kinematics xác định mối quan hệ giữa vận tốc robot (linear + angular) và trạng thái từng module swerve.

• Rotation2d: đại diện góc quay robot trong mặt phẳng 2D (dạng rotation hai chiều).

• Pose2d: đại diện pose (vị trí & hướng) của robot, gồm (x, y, θ).

• SwerveModuleState: trạng thái một module swerve, gồm tốc độ và góc mô-đun.