001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.kinematics;
006
007import edu.wpi.first.math.MathSharedStore;
008import edu.wpi.first.math.geometry.Pose2d;
009import edu.wpi.first.math.geometry.Rotation2d;
010
011/**
012 * Class for mecanum drive odometry. Odometry allows you to track the robot's position on the field
013 * over a course of a match using readings from your mecanum wheel encoders.
014 *
015 * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
016 * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
017 */
018public class MecanumDriveOdometry extends Odometry<MecanumDriveWheelPositions> {
019  /**
020   * Constructs a MecanumDriveOdometry object.
021   *
022   * @param kinematics The mecanum drive kinematics for your drivetrain.
023   * @param gyroAngle The angle reported by the gyroscope.
024   * @param wheelPositions The distances driven by each wheel.
025   * @param initialPose The starting position of the robot on the field.
026   */
027  public MecanumDriveOdometry(
028      MecanumDriveKinematics kinematics,
029      Rotation2d gyroAngle,
030      MecanumDriveWheelPositions wheelPositions,
031      Pose2d initialPose) {
032    super(kinematics, gyroAngle, wheelPositions, initialPose);
033    MathSharedStore.reportUsage("MecanumDriveOdometry", "");
034  }
035
036  /**
037   * Constructs a MecanumDriveOdometry object with the default pose at the origin.
038   *
039   * @param kinematics The mecanum drive kinematics for your drivetrain.
040   * @param gyroAngle The angle reported by the gyroscope.
041   * @param wheelPositions The distances driven by each wheel.
042   */
043  public MecanumDriveOdometry(
044      MecanumDriveKinematics kinematics,
045      Rotation2d gyroAngle,
046      MecanumDriveWheelPositions wheelPositions) {
047    this(kinematics, gyroAngle, wheelPositions, Pose2d.kZero);
048  }
049}