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.MathUsageId;
009import edu.wpi.first.math.geometry.Pose2d;
010import edu.wpi.first.math.geometry.Pose3d;
011import edu.wpi.first.math.geometry.Rotation2d;
012import edu.wpi.first.math.geometry.Rotation3d;
013import edu.wpi.first.math.geometry.Translation2d;
014import edu.wpi.first.math.geometry.Translation3d;
015
016/**
017 * Class for mecanum drive odometry. Odometry allows you to track the robot's position on the field
018 * over a course of a match using readings from your mecanum wheel encoders.
019 *
020 * <p>This class is meant to be an easy replacement for {@link MecanumDriveOdometry}, only requiring
021 * the addition of appropriate conversions between 2D and 3D versions of geometry classes. (See
022 * {@link Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link
023 * Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.)
024 *
025 * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
026 * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
027 */
028public class MecanumDriveOdometry3d extends Odometry3d<MecanumDriveWheelPositions> {
029  /**
030   * Constructs a MecanumDriveOdometry3d object.
031   *
032   * @param kinematics The mecanum drive kinematics for your drivetrain.
033   * @param gyroAngle The angle reported by the gyroscope.
034   * @param wheelPositions The distances driven by each wheel.
035   * @param initialPoseMeters The starting position of the robot on the field.
036   */
037  public MecanumDriveOdometry3d(
038      MecanumDriveKinematics kinematics,
039      Rotation3d gyroAngle,
040      MecanumDriveWheelPositions wheelPositions,
041      Pose3d initialPoseMeters) {
042    super(kinematics, gyroAngle, wheelPositions, initialPoseMeters);
043    MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
044  }
045
046  /**
047   * Constructs a MecanumDriveOdometry3d object with the default pose at the origin.
048   *
049   * @param kinematics The mecanum drive kinematics for your drivetrain.
050   * @param gyroAngle The angle reported by the gyroscope.
051   * @param wheelPositions The distances driven by each wheel.
052   */
053  public MecanumDriveOdometry3d(
054      MecanumDriveKinematics kinematics,
055      Rotation3d gyroAngle,
056      MecanumDriveWheelPositions wheelPositions) {
057    this(kinematics, gyroAngle, wheelPositions, Pose3d.kZero);
058  }
059}