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;
011
012/**
013 * Class for mecanum drive odometry. Odometry allows you to track the robot's position on the field
014 * over a course of a match using readings from your mecanum wheel encoders.
015 *
016 * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
017 * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
018 */
019public class MecanumDriveOdometry {
020  private final MecanumDriveKinematics m_kinematics;
021  private Pose2d m_poseMeters;
022  private MecanumDriveWheelPositions m_previousWheelPositions;
023
024  private Rotation2d m_gyroOffset;
025  private Rotation2d m_previousAngle;
026
027  /**
028   * Constructs a MecanumDriveOdometry object.
029   *
030   * @param kinematics The mecanum drive kinematics for your drivetrain.
031   * @param gyroAngle The angle reported by the gyroscope.
032   * @param wheelPositions The distances driven by each wheel.
033   * @param initialPoseMeters The starting position of the robot on the field.
034   */
035  public MecanumDriveOdometry(
036      MecanumDriveKinematics kinematics,
037      Rotation2d gyroAngle,
038      MecanumDriveWheelPositions wheelPositions,
039      Pose2d initialPoseMeters) {
040    m_kinematics = kinematics;
041    m_poseMeters = initialPoseMeters;
042    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
043    m_previousAngle = initialPoseMeters.getRotation();
044    m_previousWheelPositions =
045        new MecanumDriveWheelPositions(
046            wheelPositions.frontLeftMeters,
047            wheelPositions.frontRightMeters,
048            wheelPositions.rearLeftMeters,
049            wheelPositions.rearRightMeters);
050    MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
051  }
052
053  /**
054   * Constructs a MecanumDriveOdometry object with the default pose at the origin.
055   *
056   * @param kinematics The mecanum drive kinematics for your drivetrain.
057   * @param gyroAngle The angle reported by the gyroscope.
058   * @param wheelPositions The distances driven by each wheel.
059   */
060  public MecanumDriveOdometry(
061      MecanumDriveKinematics kinematics,
062      Rotation2d gyroAngle,
063      MecanumDriveWheelPositions wheelPositions) {
064    this(kinematics, gyroAngle, wheelPositions, 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 wheelPositions The distances driven by each wheel.
075   * @param poseMeters The position on the field that your robot is at.
076   */
077  public void resetPosition(
078      Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters) {
079    m_poseMeters = poseMeters;
080    m_previousAngle = poseMeters.getRotation();
081    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
082    m_previousWheelPositions =
083        new MecanumDriveWheelPositions(
084            wheelPositions.frontLeftMeters,
085            wheelPositions.frontRightMeters,
086            wheelPositions.rearLeftMeters,
087            wheelPositions.rearRightMeters);
088  }
089
090  /**
091   * Returns the position of the robot on the field.
092   *
093   * @return The pose of the robot (x and y are in meters).
094   */
095  public Pose2d getPoseMeters() {
096    return m_poseMeters;
097  }
098
099  /**
100   * Updates the robot's position on the field using forward kinematics and integration of the pose
101   * over time. This method takes in an angle parameter which is used instead of the angular rate
102   * that is calculated from forward kinematics, in addition to the current distance measurement at
103   * each wheel.
104   *
105   * @param gyroAngle The angle reported by the gyroscope.
106   * @param wheelPositions The distances driven by each wheel.
107   * @return The new pose of the robot.
108   */
109  public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
110    var angle = gyroAngle.plus(m_gyroOffset);
111
112    var wheelDeltas =
113        new MecanumDriveWheelPositions(
114            wheelPositions.frontLeftMeters - m_previousWheelPositions.frontLeftMeters,
115            wheelPositions.frontRightMeters - m_previousWheelPositions.frontRightMeters,
116            wheelPositions.rearLeftMeters - m_previousWheelPositions.rearLeftMeters,
117            wheelPositions.rearRightMeters - m_previousWheelPositions.rearRightMeters);
118
119    var twist = m_kinematics.toTwist2d(wheelDeltas);
120    twist.dtheta = angle.minus(m_previousAngle).getRadians();
121    var newPose = m_poseMeters.exp(twist);
122
123    m_previousAngle = angle;
124    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
125    m_previousWheelPositions =
126        new MecanumDriveWheelPositions(
127            wheelPositions.frontLeftMeters,
128            wheelPositions.frontRightMeters,
129            wheelPositions.rearLeftMeters,
130            wheelPositions.rearRightMeters);
131
132    return m_poseMeters;
133  }
134}