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;
011import edu.wpi.first.math.geometry.Twist2d;
012
013/**
014 * Class for differential drive odometry. Odometry allows you to track the robot's position on the
015 * field over the course of a match using readings from 2 encoders and a gyroscope.
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 *
020 * <p>It is important that you reset your encoders to zero before using this class. Any subsequent
021 * pose resets also require the encoders to be reset to zero.
022 */
023public class DifferentialDriveOdometry {
024  private Pose2d m_poseMeters;
025
026  private Rotation2d m_gyroOffset;
027  private Rotation2d m_previousAngle;
028
029  private double m_prevLeftDistance;
030  private double m_prevRightDistance;
031
032  /**
033   * Constructs a DifferentialDriveOdometry object.
034   *
035   * @param gyroAngle The angle reported by the gyroscope.
036   * @param leftDistanceMeters The distance traveled by the left encoder.
037   * @param rightDistanceMeters The distance traveled by the right encoder.
038   * @param initialPoseMeters The starting position of the robot on the field.
039   */
040  public DifferentialDriveOdometry(
041      Rotation2d gyroAngle,
042      double leftDistanceMeters,
043      double rightDistanceMeters,
044      Pose2d initialPoseMeters) {
045    m_poseMeters = initialPoseMeters;
046    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
047    m_previousAngle = initialPoseMeters.getRotation();
048
049    m_prevLeftDistance = leftDistanceMeters;
050    m_prevRightDistance = rightDistanceMeters;
051
052    MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
053  }
054
055  /**
056   * Constructs a DifferentialDriveOdometry object.
057   *
058   * @param gyroAngle The angle reported by the gyroscope.
059   * @param leftDistanceMeters The distance traveled by the left encoder.
060   * @param rightDistanceMeters The distance traveled by the right encoder.
061   */
062  public DifferentialDriveOdometry(
063      Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
064    this(gyroAngle, leftDistanceMeters, rightDistanceMeters, new Pose2d());
065  }
066
067  /**
068   * Resets the robot's position on the field.
069   *
070   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
071   * automatically takes care of offsetting the gyro angle.
072   *
073   * @param gyroAngle The angle reported by the gyroscope.
074   * @param leftDistanceMeters The distance traveled by the left encoder.
075   * @param rightDistanceMeters The distance traveled by the right encoder.
076   * @param poseMeters The position on the field that your robot is at.
077   */
078  public void resetPosition(
079      Rotation2d gyroAngle,
080      double leftDistanceMeters,
081      double rightDistanceMeters,
082      Pose2d poseMeters) {
083    m_poseMeters = poseMeters;
084    m_previousAngle = poseMeters.getRotation();
085    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
086
087    m_prevLeftDistance = leftDistanceMeters;
088    m_prevRightDistance = rightDistanceMeters;
089  }
090
091  /**
092   * Returns the position of the robot on the field.
093   *
094   * @return The pose of the robot (x and y are in meters).
095   */
096  public Pose2d getPoseMeters() {
097    return m_poseMeters;
098  }
099
100  /**
101   * Updates the robot position on the field using distance measurements from encoders. This method
102   * is more numerically accurate than using velocities to integrate the pose and is also
103   * advantageous for teams that are using lower CPR encoders.
104   *
105   * @param gyroAngle The angle reported by the gyroscope.
106   * @param leftDistanceMeters The distance traveled by the left encoder.
107   * @param rightDistanceMeters The distance traveled by the right encoder.
108   * @return The new pose of the robot.
109   */
110  public Pose2d update(
111      Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
112    double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance;
113    double deltaRightDistance = rightDistanceMeters - m_prevRightDistance;
114
115    m_prevLeftDistance = leftDistanceMeters;
116    m_prevRightDistance = rightDistanceMeters;
117
118    double averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
119    var angle = gyroAngle.plus(m_gyroOffset);
120
121    var newPose =
122        m_poseMeters.exp(
123            new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians()));
124
125    m_previousAngle = angle;
126
127    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
128    return m_poseMeters;
129  }
130}