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