Package swervelib.math

Class SwerveDriveOdometry2

java.lang.Object
edu.wpi.first.math.kinematics.SwerveDriveOdometry
swervelib.math.SwerveDriveOdometry2
public class SwerveDriveOdometry2 extends edu.wpi.first.math.kinematics.SwerveDriveOdometry
Clone of SwerveDriveOdometry except uses gyro pitch and roll to achieve a more accurate estimation. Originally made by Team 1466.

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    private edu.wpi.first.math.geometry.Rotation2d
    m_gyroOffset
    Gyro offset.
    private final edu.wpi.first.math.kinematics.SwerveDriveKinematics
    m_kinematics
    Swerve drive kinematics.
    private final int
    m_numModules
    Number of swerve modules.
    private edu.wpi.first.math.geometry.Pose2d
    m_poseMeters
    Estimated pose.
    private edu.wpi.first.math.geometry.Rotation2d
    m_previousAngle
    Previous gyroscope angle.
    private final edu.wpi.first.math.kinematics.SwerveModulePosition[]
    m_previousModulePositions
    Previous swerve module positions.
    private final edu.wpi.first.math.kinematics.SwerveModuleState[]
    m_zeroModuleStates
    Zero module states.

  • Constructor Summary

    Constructors
    Constructor
    Description
    SwerveDriveOdometry2(edu.wpi.first.math.kinematics.SwerveDriveKinematics kinematics, edu.wpi.first.math.geometry.Rotation2d gyroAngle, edu.wpi.first.math.kinematics.SwerveModulePosition[] modulePositions)
    Constructs a SwerveDriveOdometry object with the default pose at the origin.
    SwerveDriveOdometry2(edu.wpi.first.math.kinematics.SwerveDriveKinematics kinematics, edu.wpi.first.math.geometry.Rotation2d gyroAngle, edu.wpi.first.math.kinematics.SwerveModulePosition[] modulePositions, edu.wpi.first.math.geometry.Pose2d initialPose)
    Constructs a SwerveDriveOdometry object.

  • Method Summary

    Modifier and Type
    Method
    Description
    edu.wpi.first.math.geometry.Pose2d
    getPoseMeters()
    Returns the position of the robot on the field.
    void
    resetPosition(edu.wpi.first.math.geometry.Rotation2d gyroAngle, edu.wpi.first.math.kinematics.SwerveModulePosition[] modulePositions, edu.wpi.first.math.geometry.Pose2d pose)
    Resets the robot's position on the field.
    edu.wpi.first.math.geometry.Pose2d
    update(edu.wpi.first.math.geometry.Rotation2d gyroYaw, edu.wpi.first.math.geometry.Rotation2d pitch, edu.wpi.first.math.geometry.Rotation2d roll, edu.wpi.first.math.kinematics.SwerveModulePosition[] modulePositions)
    Updates the robot's position on the field using forward kinematics and integration of the pose over time.

    Methods inherited from class edu.wpi.first.math.kinematics.SwerveDriveOdometry

    update

    Methods inherited from class java.lang.Object

    clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

  • Field Details

    • m_kinematics

      private final edu.wpi.first.math.kinematics.SwerveDriveKinematics m_kinematics
      Swerve drive kinematics.

    • m_numModules

      private final int m_numModules
      Number of swerve modules.

    • m_previousModulePositions

      private final edu.wpi.first.math.kinematics.SwerveModulePosition[] m_previousModulePositions
      Previous swerve module positions.

    • m_zeroModuleStates

      private final edu.wpi.first.math.kinematics.SwerveModuleState[] m_zeroModuleStates
      Zero module states.

    • m_poseMeters

      private edu.wpi.first.math.geometry.Pose2d m_poseMeters
      Estimated pose.

    • m_gyroOffset

      private edu.wpi.first.math.geometry.Rotation2d m_gyroOffset
      Gyro offset.

    • m_previousAngle

      private edu.wpi.first.math.geometry.Rotation2d m_previousAngle
      Previous gyroscope angle.

  • Constructor Details

    • SwerveDriveOdometry2

      public SwerveDriveOdometry2(edu.wpi.first.math.kinematics.SwerveDriveKinematics kinematics, edu.wpi.first.math.geometry.Rotation2d gyroAngle, edu.wpi.first.math.kinematics.SwerveModulePosition[] modulePositions, edu.wpi.first.math.geometry.Pose2d initialPose)
      Constructs a SwerveDriveOdometry object.
      Parameters:
      kinematics - The swerve drive kinematics for your drivetrain.
      gyroAngle - The angle reported by the gyroscope.
      modulePositions - The wheel positions reported by each module.
      initialPose - The starting position of the robot on the field.

    • SwerveDriveOdometry2

      public SwerveDriveOdometry2(edu.wpi.first.math.kinematics.SwerveDriveKinematics kinematics, edu.wpi.first.math.geometry.Rotation2d gyroAngle, edu.wpi.first.math.kinematics.SwerveModulePosition[] modulePositions)
      Constructs a SwerveDriveOdometry object with the default pose at the origin.
      Parameters:
      kinematics - The swerve drive kinematics for your drivetrain.
      gyroAngle - The angle reported by the gyroscope.
      modulePositions - The wheel positions reported by each module.

  • Method Details

    • resetPosition

      public void resetPosition(edu.wpi.first.math.geometry.Rotation2d gyroAngle, edu.wpi.first.math.kinematics.SwerveModulePosition[] modulePositions, edu.wpi.first.math.geometry.Pose2d pose)
      Resets the robot's position on the field.

      The gyroscope angle does not need to be reset here on the user's robot code. The library automatically takes care of offsetting the gyro angle.

      Similarly, module positions do not need to be reset in user code.

      Overrides:
      resetPosition in class edu.wpi.first.math.kinematics.SwerveDriveOdometry
      Parameters:
      gyroAngle - The angle reported by the gyroscope.
      modulePositions - The wheel positions reported by each module.,
      pose - The position on the field that your robot is at.

    • getPoseMeters

      public edu.wpi.first.math.geometry.Pose2d getPoseMeters()
      Returns the position of the robot on the field.
      Overrides:
      getPoseMeters in class edu.wpi.first.math.kinematics.SwerveDriveOdometry
      Returns:
      The pose of the robot (x and y are in meters).

    • update

      public edu.wpi.first.math.geometry.Pose2d update(edu.wpi.first.math.geometry.Rotation2d gyroYaw, edu.wpi.first.math.geometry.Rotation2d pitch, edu.wpi.first.math.geometry.Rotation2d roll, edu.wpi.first.math.kinematics.SwerveModulePosition[] modulePositions)
      Updates the robot's position on the field using forward kinematics and integration of the pose over time. This method automatically calculates the current time to calculate period (difference between two timestamps). The period is used to calculate the change in distance from a velocity. This also takes in an angle parameter which is used instead of the angular rate that is calculated from forward kinematics. This also takes in pitch and roll to allow for more accurate pose estimation on angled surfaces using a rotation matrix.
      Parameters:
      gyroYaw - The yaw reported by the gyro.
      pitch - The pitch reported by the gyro.
      roll - The roll reported by the gyro.
      modulePositions - The current position of all swerve modules. Please provide the positions in the same order in which you instantiated your SwerveDriveKinematics.
      Returns:
      The new pose of the robot.