Lớp MecanumDriveKinematics là một công cụ hữu ích để chuyển đổi giữa một đối tượng ChassisSpeeds và một đối tượng MecanumDriveWheelSpeeds, trong đó chứa vận tốc (velocities) của bốn bánh (wheels) của một robot sử dụng mecanum drive. 

Tạo đối tượng Kinematics

Lớp MecanumDriveKinematics chấp nhận bốn tham số constructor, mỗi tham số là vị trí của một bánh xe tương đối so với trung tâm robot (relative to the robot center) dưới dạng Translation2d. Thứ tự các tham số là:

1. bánh trước trái (front left)

2. bánh trước phải (front right)

3. bánh sau trái (back left)

4. bánh sau phải (back right)

Các vị trí bánh xe phải là relative to the center of the robot.

• Giá trị x dương biểu thị di chuyển về phía trước robot.

• Giá trị y dương biểu thị di chuyển sang trái robot.

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

// Vị trí 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
);

// Vị trí 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
);

Chuyển ChassisSpeeds → MecanumDriveWheelSpeeds

Phương thức toWheelSpeeds(ChassisSpeeds speeds) được sử dụng để chuyển một đối tượng ChassisSpeeds thành một đối tượng MecanumDriveWheelSpeeds.
Điều này hữu ích khi bạn có các giá trị:

• vận tốc tiến (forward)

• vận tốc ngang (sideways)

• và vận tốc quay (angular velocity)

và cần tính ra tốc độ riêng từng bánh. 

Ví dụ Java – Chuyển từ ChassisSpeeds sang Wheel Speeds

// Ví dụ: robot di chuyển 1 m/s tiến thẳng,
// 3 m/s sang trái, và quay với tốc độ 1.5 rad/s
ChassisSpeeds speeds = new ChassisSpeeds(1.0, 3.0, 1.5);
// Chuyển sang vận tốc bánh
MecanumDriveWheelSpeeds wheelSpeeds =
    m_kinematics.toWheelSpeeds(speeds);
// Lấy tốc độ từng bánh
double frontLeft  = wheelSpeeds.frontLeftMetersPerSecond;
double frontRight = wheelSpeeds.frontRightMetersPerSecond;
double backLeft   = wheelSpeeds.rearLeftMetersPerSecond;
double backRight  = wheelSpeeds.rearRightMetersPerSecond;

// Ví dụ: robot di chuyển 1 m/s tiến thẳng,
// 3 m/s sang trái, và quay với tốc độ 1.5 rad/s
ChassisSpeeds speeds = new ChassisSpeeds(1.0, 3.0, 1.5);
// Chuyển sang vận tốc bánh
MecanumDriveWheelSpeeds wheelSpeeds =
    m_kinematics.toWheelSpeeds(speeds);
// Lấy tốc độ từng bánh
double frontLeft  = wheelSpeeds.frontLeftMetersPerSecond;
double frontRight = wheelSpeeds.frontRightMetersPerSecond;
double backLeft   = wheelSpeeds.rearLeftMetersPerSecond;
double backRight  = wheelSpeeds.rearRightMetersPerSecond;

Field-Oriented Drive (Điều khiển theo hướng sân đấu)

Bạn có thể tạo một đối tượng ChassisSpeeds từ bộ vận tốc theo hướng sân đấu (field-relative speeds) và sử dụng nó để lấy wheel speeds từ toWheelSpeeds().
Điều này thường dùng khi bạn muốn robot di chuyển theo hướng trên sân thay vì theo hướng robot (ví dụ: robot đi phía trước sân, không lệ thuộc vào hướng robot hiện tại). 

Sử dụng tâm quay tuỳ chỉnh (Custom Center of Rotation)

Trong một vài trường hợp, bạn có thể muốn robot quay quanh một góc tùy chỉnh, ví dụ khi tránh chướng ngại hoặc thực hiện chiến thuật đặc biệt.
Phương thức toWheelSpeeds() cũng chấp nhận tham số thứ hai là tâm quay (center of rotation) dưới dạng Translation2d, so với tâm robot. 

Dù robot quay quanh điểm này, thì:

• vx và vy vận tốc vẫn áp dụng cho toàn robot

• nhưng omega (vận tốc góc) sẽ được đo từ tâm quay mới này.

Ví dụ: nếu vx = 0, vy = 0, và omega ≠ 0 thì robot sẽ quay quanh bánh hoặc điểm bạn đặt làm tâm quay. 

Chuyển ngược lại: MecanumDriveWheelSpeeds → ChassisSpeeds

Bạn cũng có thể dùng đối tượng kinematics để chuyển một đối tượng MecanumDriveWheelSpeeds sang một đối tượng ChassisSpeeds duy nhất bằng phương thức:

toChassisSpeeds(MecanumDriveWheelSpeeds speeds)

toChassisSpeeds(MecanumDriveWheelSpeeds speeds)

Kết quả trả về sẽ chứa:

• vxMetersPerSecond: vận tốc tiến

• vyMetersPerSecond: vận tốc ngang

• omegaRadiansPerSecond: vận tốc quay tổng thể của robot.

Ví dụ Java – Chuyển Wheel Speeds thành ChassisSpeeds

// Ví dụ vận tốc từng bánh:
MecanumDriveWheelSpeeds wheelSpeeds =
    new MecanumDriveWheelSpeeds(-17.67, 20.51, -13.44, 16.26);
// Chuyển về chassis speeds
ChassisSpeeds chassisSpeeds =
    m_kinematics.toChassisSpeeds(wheelSpeeds);
// Lấy các thành phần vận tốc
double forward = chassisSpeeds.vxMetersPerSecond;
double sideways = chassisSpeeds.vyMetersPerSecond;
double angular = chassisSpeeds.omegaRadiansPerSecond;

// Ví dụ vận tốc từng bánh:
MecanumDriveWheelSpeeds wheelSpeeds =
    new MecanumDriveWheelSpeeds(-17.67, 20.51, -13.44, 16.26);
// Chuyển về chassis speeds
ChassisSpeeds chassisSpeeds =
    m_kinematics.toChassisSpeeds(wheelSpeeds);
// Lấy các thành phần vận tốc
double forward = chassisSpeeds.vxMetersPerSecond;
double sideways = chassisSpeeds.vyMetersPerSecond;
double angular = chassisSpeeds.omegaRadiansPerSecond;

Chú giải thuật ngữ

• Mecanum Drive Kinematics: toán học đại diện cho mối quan hệ giữa vận tốc robot tổng thể và vận tốc từng bánh xe mecanum.

• ChassisSpeeds: struct/vật chứa vận tốc tổng thể của robot gồm vx, vy, và omega.

• MecanumDriveWheelSpeeds: struct/vật chứa vận tốc riêng của 4 bánh trong mecanum drive.

• Translation2d: vectơ vị trí 2D biểu diễn vị trí bánh so với tâm robot.

• Field-Oriented Drive: điều khiển robot theo hướng sân đấu thay vì theo hướng robot.

Tâm quay tùy chỉnh (custom center of rotation): điểm robot quay quanh ngoài tâm robot.