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 swerve 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 swerve drive encoders and swerve azimuth 014 * encoders. 015 * 016 * <p>Teams can use odometry during the autonomous period for complex tasks like path following. 017 * Furthermore, odometry can be used for latency compensation when using computer-vision systems. 018 */ 019public class SwerveDriveOdometry extends Odometry<SwerveModulePosition[]> { 020 private final int m_numModules; 021 022 /** 023 * Constructs a SwerveDriveOdometry object. 024 * 025 * @param kinematics The swerve drive kinematics for your drivetrain. 026 * @param gyroAngle The angle reported by the gyroscope. 027 * @param modulePositions The wheel positions reported by each module. 028 * @param initialPose The starting position of the robot on the field. 029 */ 030 public SwerveDriveOdometry( 031 SwerveDriveKinematics kinematics, 032 Rotation2d gyroAngle, 033 SwerveModulePosition[] modulePositions, 034 Pose2d initialPose) { 035 super(kinematics, gyroAngle, modulePositions, initialPose); 036 037 m_numModules = modulePositions.length; 038 039 MathSharedStore.reportUsage("SwerveDriveOdometry", ""); 040 } 041 042 /** 043 * Constructs a SwerveDriveOdometry object with the default pose at the origin. 044 * 045 * @param kinematics The swerve drive kinematics for your drivetrain. 046 * @param gyroAngle The angle reported by the gyroscope. 047 * @param modulePositions The wheel positions reported by each module. 048 */ 049 public SwerveDriveOdometry( 050 SwerveDriveKinematics kinematics, 051 Rotation2d gyroAngle, 052 SwerveModulePosition[] modulePositions) { 053 this(kinematics, gyroAngle, modulePositions, Pose2d.kZero); 054 } 055 056 @Override 057 public void resetPosition( 058 Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d pose) { 059 if (modulePositions.length != m_numModules) { 060 throw new IllegalArgumentException( 061 "Number of modules is not consistent with number of wheel locations provided in " 062 + "constructor"); 063 } 064 super.resetPosition(gyroAngle, modulePositions, pose); 065 } 066 067 @Override 068 public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) { 069 if (modulePositions.length != m_numModules) { 070 throw new IllegalArgumentException( 071 "Number of modules is not consistent with number of wheel locations provided in " 072 + "constructor"); 073 } 074 return super.update(gyroAngle, modulePositions); 075 } 076}