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<SwerveDriveWheelPositions> {
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, new SwerveDriveWheelPositions(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, new Pose2d());
055  }
056
057  /**
058   * Resets the robot's position on the field.
059   *
060   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
061   * automatically takes care of offsetting the gyro angle.
062   *
063   * <p>Similarly, module positions do not need to be reset in user code.
064   *
065   * @param gyroAngle The angle reported by the gyroscope.
066   * @param modulePositions The wheel positions reported by each module.,
067   * @param pose The position on the field that your robot is at.
068   */
069  public void resetPosition(
070      Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d pose) {
071    resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), pose);
072  }
073
074  @Override
075  public void resetPosition(
076      Rotation2d gyroAngle, SwerveDriveWheelPositions modulePositions, Pose2d pose) {
077    if (modulePositions.positions.length != m_numModules) {
078      throw new IllegalArgumentException(
079          "Number of modules is not consistent with number of wheel locations provided in "
080              + "constructor");
081    }
082    super.resetPosition(gyroAngle, modulePositions, pose);
083  }
084
085  /**
086   * Updates the robot's position on the field using forward kinematics and integration of the pose
087   * over time. This method automatically calculates the current time to calculate period
088   * (difference between two timestamps). The period is used to calculate the change in distance
089   * from a velocity. This also takes in an angle parameter which is used instead of the angular
090   * rate that is calculated from forward kinematics.
091   *
092   * @param gyroAngle The angle reported by the gyroscope.
093   * @param modulePositions The current position of all swerve modules. Please provide the positions
094   *     in the same order in which you instantiated your SwerveDriveKinematics.
095   * @return The new pose of the robot.
096   */
097  public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
098    return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions));
099  }
100
101  @Override
102  public Pose2d update(Rotation2d gyroAngle, SwerveDriveWheelPositions modulePositions) {
103    if (modulePositions.positions.length != m_numModules) {
104      throw new IllegalArgumentException(
105          "Number of modules is not consistent with number of wheel locations provided in "
106              + "constructor");
107    }
108    return super.update(gyroAngle, modulePositions);
109  }
110}