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;
012import edu.wpi.first.util.WPIUtilJNI;
013
014/**
015 * Class for mecanum drive odometry. Odometry allows you to track the robot's position on the field
016 * over a course of a match using readings from your mecanum wheel encoders.
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 */
021public class MecanumDriveOdometry {
022  private final MecanumDriveKinematics m_kinematics;
023  private Pose2d m_poseMeters;
024  private double m_prevTimeSeconds = -1;
025
026  private Rotation2d m_gyroOffset;
027  private Rotation2d m_previousAngle;
028
029  /**
030   * Constructs a MecanumDriveOdometry object.
031   *
032   * @param kinematics The mecanum drive kinematics for your drivetrain.
033   * @param gyroAngle The angle reported by the gyroscope.
034   * @param initialPoseMeters The starting position of the robot on the field.
035   */
036  public MecanumDriveOdometry(
037      MecanumDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPoseMeters) {
038    m_kinematics = kinematics;
039    m_poseMeters = initialPoseMeters;
040    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
041    m_previousAngle = initialPoseMeters.getRotation();
042    MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
043  }
044
045  /**
046   * Constructs a MecanumDriveOdometry object with the default pose at the origin.
047   *
048   * @param kinematics The mecanum drive kinematics for your drivetrain.
049   * @param gyroAngle The angle reported by the gyroscope.
050   */
051  public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle) {
052    this(kinematics, gyroAngle, new Pose2d());
053  }
054
055  /**
056   * Resets the robot's position on the field.
057   *
058   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
059   * automatically takes care of offsetting the gyro angle.
060   *
061   * @param poseMeters The position on the field that your robot is at.
062   * @param gyroAngle The angle reported by the gyroscope.
063   */
064  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
065    m_poseMeters = poseMeters;
066    m_previousAngle = poseMeters.getRotation();
067    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
068  }
069
070  /**
071   * Returns the position of the robot on the field.
072   *
073   * @return The pose of the robot (x and y are in meters).
074   */
075  public Pose2d getPoseMeters() {
076    return m_poseMeters;
077  }
078
079  /**
080   * Updates the robot's position on the field using forward kinematics and integration of the pose
081   * over time. This method takes in the current time as a parameter to calculate period (difference
082   * between two timestamps). The period is used to calculate the change in distance from a
083   * velocity. This also takes in an angle parameter which is used instead of the angular rate that
084   * is calculated from forward kinematics.
085   *
086   * @param currentTimeSeconds The current time in seconds.
087   * @param gyroAngle The angle reported by the gyroscope.
088   * @param wheelSpeeds The current wheel speeds.
089   * @return The new pose of the robot.
090   */
091  public Pose2d updateWithTime(
092      double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
093    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
094    m_prevTimeSeconds = currentTimeSeconds;
095
096    var angle = gyroAngle.plus(m_gyroOffset);
097
098    var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
099    var newPose =
100        m_poseMeters.exp(
101            new Twist2d(
102                chassisState.vxMetersPerSecond * period,
103                chassisState.vyMetersPerSecond * period,
104                angle.minus(m_previousAngle).getRadians()));
105
106    m_previousAngle = angle;
107    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
108    return m_poseMeters;
109  }
110
111  /**
112   * Updates the robot's position on the field using forward kinematics and integration of the pose
113   * over time. This method automatically calculates the current time to calculate period
114   * (difference between two timestamps). The period is used to calculate the change in distance
115   * from a velocity. This also takes in an angle parameter which is used instead of the angular
116   * rate that is calculated from forward kinematics.
117   *
118   * @param gyroAngle The angle reported by the gyroscope.
119   * @param wheelSpeeds The current wheel speeds.
120   * @return The new pose of the robot.
121   */
122  public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
123    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds);
124  }
125}