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 initialPoseMeters The starting position of the robot on the field.
037   */
038  public DifferentialDriveOdometry(Rotation2d gyroAngle, Pose2d initialPoseMeters) {
039    m_poseMeters = initialPoseMeters;
040    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
041    m_previousAngle = initialPoseMeters.getRotation();
042    MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
043  }
044
045  /**
046   * Constructs a DifferentialDriveOdometry object with the default pose at the origin.
047   *
048   * @param gyroAngle The angle reported by the gyroscope.
049   */
050  public DifferentialDriveOdometry(Rotation2d gyroAngle) {
051    this(gyroAngle, new Pose2d());
052  }
053
054  /**
055   * Resets the robot's position on the field.
056   *
057   * <p>You NEED to reset your encoders (to zero) when calling this method.
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 poseMeters The position on the field that your robot is at.
063   * @param gyroAngle The angle reported by the gyroscope.
064   */
065  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
066    m_poseMeters = poseMeters;
067    m_previousAngle = poseMeters.getRotation();
068    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
069
070    m_prevLeftDistance = 0.0;
071    m_prevRightDistance = 0.0;
072  }
073
074  /**
075   * Returns the position of the robot on the field.
076   *
077   * @return The pose of the robot (x and y are in meters).
078   */
079  public Pose2d getPoseMeters() {
080    return m_poseMeters;
081  }
082
083  /**
084   * Updates the robot position on the field using distance measurements from encoders. This method
085   * is more numerically accurate than using velocities to integrate the pose and is also
086   * advantageous for teams that are using lower CPR encoders.
087   *
088   * @param gyroAngle The angle reported by the gyroscope.
089   * @param leftDistanceMeters The distance traveled by the left encoder.
090   * @param rightDistanceMeters The distance traveled by the right encoder.
091   * @return The new pose of the robot.
092   */
093  public Pose2d update(
094      Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
095    double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance;
096    double deltaRightDistance = rightDistanceMeters - m_prevRightDistance;
097
098    m_prevLeftDistance = leftDistanceMeters;
099    m_prevRightDistance = rightDistanceMeters;
100
101    double averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
102    var angle = gyroAngle.plus(m_gyroOffset);
103
104    var newPose =
105        m_poseMeters.exp(
106            new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians()));
107
108    m_previousAngle = angle;
109
110    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
111    return m_poseMeters;
112  }
113}