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