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}