Lớp SwerveDriveKinematics là công cụ hữu ích để chuyển đổi giữa một đối tượng ChassisSpeeds và nhiều đối tượng SwerveModuleState, mỗi đối tượng chứa tốc độ (velocity) và góc (angle) cho từng module swerve trên một robot swerve drive. 

Lớp SwerveModuleState

Lớp SwerveModuleState chứa:

• Tốc độ bánh xe (velocity) của một module swerve (đơn vị: mét/giây),

• Góc module (angle).

Trong Java / Python, vận tốc must be in meters per second (phải ở đơn vị mét/giây).
Một góc 0 tương ứng với module hướng về phía trước robot. 

Tạo đối tượng Kinematics

Lớp SwerveDriveKinematics chấp nhận số lượng biến đổi của tham số constructor, trong đó mỗi tham số là vị trí tương đối của một module swerve so với trung tâm robot (dạng Translation2d). Số lượng tham số tương ứng với số module swerve. Robot swerve phải có ít nhất 2 module. 

Vị trí các module phải relative to the center of the robot (tương đối với tâm 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 về phía trái robot.

Ví dụ Java – Tạo đối tượng kinematics

// Locations for the swerve drive modules relative to the robot center:
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);
// Creating the kinematics object using the module locations
SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
    m_frontLeftLocation,
    m_frontRightLocation,
    m_backLeftLocation,
    m_backRightLocation
);

// Locations for the swerve drive modules relative to the robot center:
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);
// Creating the kinematics object using the module locations
SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
    m_frontLeftLocation,
    m_frontRightLocation,
    m_backLeftLocation,
    m_backRightLocation
);

Chuyển ChassisSpeeds → Trạng thái module (SwerveModuleState[])

Sử dụng phương thức:

toSwerveModuleStates(ChassisSpeeds speeds)

toSwerveModuleStates(ChassisSpeeds speeds)

Phương thức này chuyển một ChassisSpeeds thành một mảng SwerveModuleState — dùng khi bạn có vận tốc tuyến tính, vận tốc ngang và vận tốc góc, và cần quy đổi ra trạng thái từng module để điều khiển. 

Mảng kết quả trả về theo cùng thứ tự mà bạn đã truyền vào constructor của SwerveDriveKinematics. 

Field-Oriented Drive

Bạn có thể sử dụng ChassisSpeeds được tạo từ tốc độ theo hướng sân đấu (field-relative speeds) để lấy module states tương ứng. Điều này thường dùng trong field-oriented control — robot di chuyển theo hướng sân đấu thay vì robot. 

Sử dụng Trung tâm quay tùy chỉnh

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

Với tâm quay tùy chỉnh:

• vx và vy vẫn áp dụng cho toàn bộ robot,

• omega được đo từ tâm quay mới này.
  Nếu vx = 0, vy = 0, và omega ≠ 0, robot sẽ xoay quanh điểm đó.

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

Bạn cũng có thể chuyển từ một mảng các SwerveModuleState sang một đối tượng ChassisSpeeds bằng:

toChassisSpeeds(SwerveModuleState... states)

toChassisSpeeds(SwerveModuleState... states)

Kết quả là:

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

• vyMetersPerSecond: vận tốc ngang

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

Ví dụ Java – Chuyển SwerveModuleState[] về ChassisSpeeds:

SwerveModuleState flState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19));
SwerveModuleState frState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81));
SwerveModuleState blState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44));
SwerveModuleState brState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56));
ChassisSpeeds chassisSpeeds =
    m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
double forward = chassisSpeeds.vxMetersPerSecond;
double sideways = chassisSpeeds.vyMetersPerSecond;
double angular = chassisSpeeds.omegaRadiansPerSecond;

SwerveModuleState flState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19));
SwerveModuleState frState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81));
SwerveModuleState blState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44));
SwerveModuleState brState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56));
ChassisSpeeds chassisSpeeds =
    m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
double forward = chassisSpeeds.vxMetersPerSecond;
double sideways = chassisSpeeds.vyMetersPerSecond;
double angular = chassisSpeeds.omegaRadiansPerSecond;

Chú giải thuật ngữ quan trọng

• Swerve Drive Kinematics: toán học cho hệ truyền động swerve, chuyển đổi giữa vận tốc tổng thể robot và trạng thái từng module.

• ChassisSpeeds: đối tượng chứa vx, vy, và omega (linear + angular velocities).

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

• SwerveModuleState: trạng thái tốc độ + góc của module swerve.

• Field-Oriented Drive: điều khiển 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.