Class TwoDeadWheelOdometry3d

java.lang.Object
org.wpilib.math.kinematics.TwoDeadWheelOdometry3d
public class TwoDeadWheelOdometry3d extends Object
Class for double follower wheel odometry. Odometry allows you to track the robot's position on the field over the course of a match using readings from 2 encoders and a gyroscope.

This class is meant to be an easy replacement for TwoDeadWheelOdometry, only requiring the addition of appropriate conversions between 2D and 3D versions of geometry classes. (See Pose3d(Pose2d), Rotation3d(Rotation2d), Translation3d(Translation2d), and Pose3d.toPose2d().)

Teams can use odometry during the autonomous period for complex tasks like path following. Furthermore, odometry can be used for latency compensation when using computer-vision systems.

  • Constructor Details

    • TwoDeadWheelOdometry3d

      public TwoDeadWheelOdometry3d(double xWheelYPos, double yWheelXPos, double xWheelPos, double yWheelPos, Rotation3d gyroAngle, Pose3d initialPose)
      Constructs a TwoDeadWheelOdometry object.
      Parameters:
      xWheelYPos - The y-position of the forward-facing wheel relative to the center of the robot in meters.
      yWheelXPos - The x-position of the left-facing wheel relative to the center of the robot in meters.
      xWheelPos - The distance traveled by the forward-facing wheel, in meters.
      yWheelPos - The distance traveled by the left-facing wheel, in meters.
      gyroAngle - The angle reported by the gyroscope. This does not need to be offset to match the robot's orientation on the field.
      initialPose - The starting position of the robot on the field.

    • TwoDeadWheelOdometry3d

      public TwoDeadWheelOdometry3d(double xWheelYPos, double yWheelXPos, double xWheelPos, double yWheelPos, Rotation3d gyroAngle)
      Constructs a TwoDeadWheelOdometry object with the default pose at the origin.
      Parameters:
      xWheelYPos - The y-position of the forward-facing wheel relative to the center of the robot in meters.
      yWheelXPos - The x-position of the left-facing wheel relative to the center of the robot in meters.
      xWheelPos - The distance traveled by the forward-facing wheel, in meters.
      yWheelPos - The distance traveled by the left-facing wheel, in meters.
      gyroAngle - The angle reported by the gyroscope. This does not need to be offset to match the robot's orientation on the field.

    • TwoDeadWheelOdometry3d

      public TwoDeadWheelOdometry3d(Distance xWheelYPos, Distance yWheelXPos, Distance xWheelPos, Distance yWheelPos, Rotation3d gyroAngle, Pose3d initialPose)
      Constructs a TwoDeadWheelOdometry object.
      Parameters:
      xWheelYPos - The y-position of the forward-facing wheel relative to the center of the robot in meters.
      yWheelXPos - The x-position of the left-facing wheel relative to the center of the robot in meters.
      xWheelPos - The distance traveled by the forward-facing wheel, in meters.
      yWheelPos - The distance traveled by the left-facing wheel, in meters.
      gyroAngle - The angle reported by the gyroscope. This does not need to be offset to match the robot's orientation on the field.
      initialPose - The starting position of the robot on the field.

    • TwoDeadWheelOdometry3d

      public TwoDeadWheelOdometry3d(Distance xWheelYPos, Distance yWheelXPos, Distance xWheelPos, Distance yWheelPos, Rotation3d gyroAngle)
      Constructs a TwoDeadWheelOdometry object with the default pose at the origin.
      Parameters:
      xWheelYPos - The y-position of the forward-facing wheel relative to the center of the robot in meters.
      yWheelXPos - The x-position of the left-facing wheel relative to the center of the robot in meters.
      xWheelPos - The distance traveled by the forward-facing wheel, in meters.
      yWheelPos - The distance traveled by the left-facing wheel, in meters.
      gyroAngle - The angle reported by the gyroscope. This does not need to be offset to match the robot's orientation on the field.

  • Method Details

    • resetPosition

      public void resetPosition(double xWheelPos, double yWheelPos, Rotation3d gyroAngle, Pose3d pose)
      Resets the robot's position on the field.

      The gyroscope angle does not need to be reset here in the user's robot code.

      Parameters:
      xWheelPos - The distance traveled by the forward-facing wheel, in meters.
      yWheelPos - The distance traveled by the left-facing wheel, in meters.
      gyroAngle - The angle reported by the gyroscope. This does not need to be offset to match the robot's orientation on the field.
      pose - The new position of the robot on the field.

    • resetPose

      public void resetPose(Pose3d pose)
      Resets the pose.
      Parameters:
      pose - The pose to reset to.

    • resetTranslation

      public void resetTranslation(Translation3d translation)
      Resets the translation of the pose.
      Parameters:
      translation - The translation to reset to.

    • resetRotation

      public void resetRotation(Rotation3d rotation)
      Resets the rotation of the pose.
      Parameters:
      rotation - The rotation to reset to.

    • getPose

      public Pose3d getPose()
      Returns the position of the robot on the field.
      Returns:
      The pose of the robot (x, y, and z are in meters).

    • update

      public Pose3d update(double xWheelPos, double yWheelPos, Rotation3d gyroAngle)
      Updates the robot's position.
      Parameters:
      xWheelPos - The distance traveled by the forward-facing wheel, in meters.
      yWheelPos - The distance traveled by the left-facing wheel, in meters.
      gyroAngle - The angle reported by the gyroscope. This does not need to be offset to match the robot's orientation on the field.
      Returns:
      The updated pose.

    • toChassisVelocities

      public ChassisVelocities toChassisVelocities(double vx, double vy, double omega)
      Converts measured wheel velocities to chassis velocities using inverse kinematics.
      Parameters:
      vx - The velocity of the forward-facing wheel, in meters per second.
      vy - The velocity of the sideways-facing wheel, in meters per second.
      omega - The angular velocity of the robot as reported by the gyro, in radians per second.
      Returns:
      The velocity of the chassis.