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 differential drive odometry. Odometry allows you to track the robot's position on the
014 * field over the course of a match using readings from 2 encoders and a gyroscope.
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 *
019 * <p>It is important that you reset your encoders to zero before using this class. Any subsequent
020 * pose resets also require the encoders to be reset to zero.
021 */
022public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPositions> {
023  /**
024   * Constructs a DifferentialDriveOdometry object.
025   *
026   * @param gyroAngle The angle reported by the gyroscope.
027   * @param leftDistanceMeters The distance traveled by the left encoder.
028   * @param rightDistanceMeters The distance traveled by the right encoder.
029   * @param initialPoseMeters The starting position of the robot on the field.
030   */
031  public DifferentialDriveOdometry(
032      Rotation2d gyroAngle,
033      double leftDistanceMeters,
034      double rightDistanceMeters,
035      Pose2d initialPoseMeters) {
036    super(
037        new DifferentialDriveKinematics(1),
038        gyroAngle,
039        new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
040        initialPoseMeters);
041    MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
042  }
043
044  /**
045   * Constructs a DifferentialDriveOdometry object.
046   *
047   * @param gyroAngle The angle reported by the gyroscope.
048   * @param leftDistanceMeters The distance traveled by the left encoder.
049   * @param rightDistanceMeters The distance traveled by the right encoder.
050   */
051  public DifferentialDriveOdometry(
052      Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
053    this(gyroAngle, leftDistanceMeters, rightDistanceMeters, new Pose2d());
054  }
055
056  /**
057   * Resets the robot's position on the field.
058   *
059   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
060   * automatically takes care of offsetting the gyro angle.
061   *
062   * @param gyroAngle The angle reported by the gyroscope.
063   * @param leftDistanceMeters The distance traveled by the left encoder.
064   * @param rightDistanceMeters The distance traveled by the right encoder.
065   * @param poseMeters The position on the field that your robot is at.
066   */
067  public void resetPosition(
068      Rotation2d gyroAngle,
069      double leftDistanceMeters,
070      double rightDistanceMeters,
071      Pose2d poseMeters) {
072    super.resetPosition(
073        gyroAngle,
074        new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
075        poseMeters);
076  }
077
078  /**
079   * Updates the robot position on the field using distance measurements from encoders. This method
080   * is more numerically accurate than using velocities to integrate the pose and is also
081   * advantageous for teams that are using lower CPR encoders.
082   *
083   * @param gyroAngle The angle reported by the gyroscope.
084   * @param leftDistanceMeters The distance traveled by the left encoder.
085   * @param rightDistanceMeters The distance traveled by the right encoder.
086   * @return The new pose of the robot.
087   */
088  public Pose2d update(
089      Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
090    return super.update(
091        gyroAngle, new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters));
092  }
093}