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 static edu.wpi.first.units.Units.Meters;
008
009import edu.wpi.first.math.MathSharedStore;
010import edu.wpi.first.math.MathUsageId;
011import edu.wpi.first.math.geometry.Pose2d;
012import edu.wpi.first.math.geometry.Rotation2d;
013import edu.wpi.first.units.Distance;
014import edu.wpi.first.units.Measure;
015
016/**
017 * Class for differential drive odometry. Odometry allows you to track the robot's position on the
018 * field over the course of a match using readings from 2 encoders and a gyroscope.
019 *
020 * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
021 * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
022 *
023 * <p>It is important that you reset your encoders to zero before using this class. Any subsequent
024 * pose resets also require the encoders to be reset to zero.
025 */
026public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPositions> {
027  /**
028   * Constructs a DifferentialDriveOdometry object.
029   *
030   * @param gyroAngle The angle reported by the gyroscope.
031   * @param leftDistanceMeters The distance traveled by the left encoder.
032   * @param rightDistanceMeters The distance traveled by the right encoder.
033   * @param initialPoseMeters The starting position of the robot on the field.
034   */
035  public DifferentialDriveOdometry(
036      Rotation2d gyroAngle,
037      double leftDistanceMeters,
038      double rightDistanceMeters,
039      Pose2d initialPoseMeters) {
040    super(
041        new DifferentialDriveKinematics(1),
042        gyroAngle,
043        new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
044        initialPoseMeters);
045    MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
046  }
047
048  /**
049   * Constructs a DifferentialDriveOdometry object.
050   *
051   * @param gyroAngle The angle reported by the gyroscope.
052   * @param leftDistance The distance traveled by the left encoder.
053   * @param rightDistance The distance traveled by the right encoder.
054   * @param initialPoseMeters The starting position of the robot on the field.
055   */
056  public DifferentialDriveOdometry(
057      Rotation2d gyroAngle,
058      Measure<Distance> leftDistance,
059      Measure<Distance> rightDistance,
060      Pose2d initialPoseMeters) {
061    this(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), initialPoseMeters);
062  }
063
064  /**
065   * Constructs a DifferentialDriveOdometry object.
066   *
067   * @param gyroAngle The angle reported by the gyroscope.
068   * @param leftDistanceMeters The distance traveled by the left encoder.
069   * @param rightDistanceMeters The distance traveled by the right encoder.
070   */
071  public DifferentialDriveOdometry(
072      Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
073    this(gyroAngle, leftDistanceMeters, rightDistanceMeters, new Pose2d());
074  }
075
076  /**
077   * Constructs a DifferentialDriveOdometry object.
078   *
079   * @param gyroAngle The angle reported by the gyroscope.
080   * @param leftDistance The distance traveled by the left encoder.
081   * @param rightDistance The distance traveled by the right encoder.
082   */
083  public DifferentialDriveOdometry(
084      Rotation2d gyroAngle, Measure<Distance> leftDistance, Measure<Distance> rightDistance) {
085    this(gyroAngle, leftDistance, rightDistance, new Pose2d());
086  }
087
088  /**
089   * Resets the robot's position on the field.
090   *
091   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
092   * automatically takes care of offsetting the gyro angle.
093   *
094   * @param gyroAngle The angle reported by the gyroscope.
095   * @param leftDistanceMeters The distance traveled by the left encoder.
096   * @param rightDistanceMeters The distance traveled by the right encoder.
097   * @param poseMeters The position on the field that your robot is at.
098   */
099  public void resetPosition(
100      Rotation2d gyroAngle,
101      double leftDistanceMeters,
102      double rightDistanceMeters,
103      Pose2d poseMeters) {
104    super.resetPosition(
105        gyroAngle,
106        new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
107        poseMeters);
108  }
109
110  /**
111   * Resets the robot's position on the field.
112   *
113   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
114   * automatically takes care of offsetting the gyro angle.
115   *
116   * @param gyroAngle The angle reported by the gyroscope.
117   * @param leftDistance The distance traveled by the left encoder.
118   * @param rightDistance The distance traveled by the right encoder.
119   * @param poseMeters The position on the field that your robot is at.
120   */
121  public void resetPosition(
122      Rotation2d gyroAngle,
123      Measure<Distance> leftDistance,
124      Measure<Distance> rightDistance,
125      Pose2d poseMeters) {
126    resetPosition(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), poseMeters);
127  }
128
129  /**
130   * Updates the robot position on the field using distance measurements from encoders. This method
131   * is more numerically accurate than using velocities to integrate the pose and is also
132   * advantageous for teams that are using lower CPR encoders.
133   *
134   * @param gyroAngle The angle reported by the gyroscope.
135   * @param leftDistanceMeters The distance traveled by the left encoder.
136   * @param rightDistanceMeters The distance traveled by the right encoder.
137   * @return The new pose of the robot.
138   */
139  public Pose2d update(
140      Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
141    return super.update(
142        gyroAngle, new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters));
143  }
144}