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}