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