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