Lớp DifferentialDriveOdometry được dùng để theo dõi vị trí robot truyền động kiểu differential drive trên sân đấu dựa trên dữ liệu encoder bánh xe và góc từ gyro (ví dụ IMU / gyroscope). Nó giúp bạn biết robot đang ở đâu theo hệ tọa độ mặt phẳng 2D. 

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

Tạo đối tượng Odometry

Để sử dụng odometry, bạn cần tạo một đối tượng DifferentialDriveOdometry. Constructor này yêu cầu:

1. Góc robot hiện tại do gyro trả về — dưới dạng một đối tượng Rotation2d.

2. (Tùy chọn) Pose ban đầu của robot — dưới dạng Pose2d. Nếu không cung cấp, robot sẽ được giả định bắt đầu tại vị trí
   Nếu không cung cấp, robot mặc định bắt đầu tại:

   1. x = 0

   2. y = 0

   3. θ = 0

Trong đó θ là hướng quay của robot.

Gợi ý cấu hình ban đầu:

• 0 độ / radian nghĩa là robot đang đối diện trực tiếp với trạm đồng minh đối thủ (facing forward).

• Khi robot quay sang trái, góc đo từ gyro nên tăng lên.

Lưu ý quan trọng: Trước khi tạo đối tượng odometry, phải reset (đặt lại) các encoder vị trí bánh xe về 0 để bắt đầu đúng.

Ví dụ Java:

// Tạo đối tượng odometry.
DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(
    getGyroHeading(),                   // Rotation2d từ gyro
    new Pose2d(5.0, 13.5, new Rotation2d()) // Pose2d ban đầu
);

// Tạo đối tượng odometry.
DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(
    getGyroHeading(),                   // Rotation2d từ gyro
    new Pose2d(5.0, 13.5, new Rotation2d()) // Pose2d ban đầu
);

Cập nhật vị trí robot

Sau khi tạo đối tượng odometry, bạn cần gọi phương thức update() định kỳ (thường là trong periodic() của Subsystem) để robot liên tục tính toán vị trí mới theo encoder và gyro cập nhật.

Phương thức update(...) nhận vào:

• Góc robot hiện tại từ gyro

• Khoảng cách encoder trái

• Khoảng cách encoder phải

Mỗi lần gọi update, odometry sẽ trả về pose mới cập nhật (một đối tượng Pose2d).

Ví dụ Java:

@Override
public void periodic() {
  // Lấy góc robot từ gyro
  Rotation2d gyroAngle = m_gyro.getRotation2d();
  // Cập nhật odometry với encoder trái / phải
  m_pose = m_odometry.update(
    gyroAngle,
    m_leftEncoder.getDistance(),
    m_rightEncoder.getDistance()
  );
}

@Override
public void periodic() {
  // Lấy góc robot từ gyro
  Rotation2d gyroAngle = m_gyro.getRotation2d();
  // Cập nhật odometry với encoder trái / phải
  m_pose = m_odometry.update(
    gyroAngle,
    m_leftEncoder.getDistance(),
    m_rightEncoder.getDistance()
  );
}

Nhớ đảm bảo đơn vị đo encoder là đơn vị dài phù hợp (ví dụ: mét) trước khi truyền vào phương thức.

Reset vị trí robot

Bạn có thể reset vị trí robot bất kỳ lúc nào bằng cách gọi phương thức resetPose(...). Phương thức này nhận vào:

• Pose mới mong muốn (ví dụ khi bạn biết robot thực sự đang ở đâu)

• Góc gyro hiện tại

Nếu bạn reset gyro trong quá trình chạy, phải reset odometry ngay sau đó với góc gyro mới để vị trí tính toán không lệch.

Thuật ngữ quan trọng

• Differential Drive Odometry: theo dõi vị trí robot bằng encoder và gyro.

• Rotation2d: góc robot trong mặt phẳng 2D.

• Pose2d: vị trí robot gồm (x, y, θ).

• Encoder distance: quãng đường bánh xe đã đi — phải được đặt ở đơn vị dài nhất quán.

Periodic update: cập nhật odometry nên được gọi liên tục ở mỗi chu kỳ điều khiển.