# SwerveDrive
declaration: package: swervelib, class: SwerveDrive

Package swervelib

# Class SwerveDrive

java.lang.Object
swervelib.SwerveDrive

public class SwerveDrive extends Object
Swerve Drive class representing and controlling the swerve drive.

## Field Summary

Fields
Modifier and Type
Field
Description
`private double`
`attainableMaxRotationalVelocityRadiansPerSecond`
The absolute max speed the robot can reach while rotating radians per second.
`private double`
`attainableMaxTranslationalSpeedMetersPerSecond`
The absolute max speed that your robot can reach while translating in meters per second.
`boolean`
`chassisVelocityCorrection`
Correct chassis velocity in `drive(Translation2d, double, boolean, boolean)` using 254's correction.
`edu.wpi.first.wpilibj.smartdashboard.Field2d`
`field`
Field object.
`private double`
`HEADING_CORRECTION_DEADBAND`
Deadband for speeds in heading correction.
`boolean`
`headingCorrection`
Whether to correct heading when driving translationally.
`private SwerveIMU`
`imu`
Swerve IMU device for sensing the heading of the robot.
`final Cache<edu.wpi.first.math.geometry.Rotation3d>`
`imuReadingCache`
IMU reading cache for robot readings.
`final edu.wpi.first.math.kinematics.SwerveDriveKinematics`
`kinematics`
Swerve Kinematics object.
`private double`
`lastHeadingRadians`
The last heading set in radians.
`private double`
`maxSpeedMPS`
Maximum speed of the robot in meters per second.
`private int`
`moduleSynchronizationCounter`
Counter to synchronize the modules relative encoder with absolute encoder when not moving.
`private final Lock`
`odometryLock`
Odometry lock to ensure thread safety.
`private final edu.wpi.first.wpilibj.Notifier`
`odometryThread`
WPILib `Notifier` to keep odometry up to date.
`private SwerveIMUSimulation`
`simIMU`
Simulation of the swerve drive.
`SwerveController`
`swerveController`
Swerve controller for controlling heading of the robot.
`final SwerveDriveConfiguration`
`swerveDriveConfiguration`
Swerve drive configuration.
`final edu.wpi.first.math.estimator.SwerveDrivePoseEstimator`
`swerveDrivePoseEstimator`
Swerve odometry.
`private final SwerveModule[]`
`swerveModules`
Swerve modules.
`private final Alert`
`tunerXRecommendation`
Alert to recommend Tuner X if the configuration is compatible.

## Constructor Summary

Constructors
Constructor
Description
`SwerveDrive(SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig, double maxSpeedMPS)`
Creates a new swerve drivebase subsystem.

## Method Summary

Modifier and Type
Method
Description
`void`
`addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d robotPose, double timestamp)`
Add a vision measurement to the `SwerveDrivePoseEstimator` and update the `SwerveIMU` gyro reading with the given timestamp of the vision measurement.
`void`
`addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d robotPose, double timestamp, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> visionMeasurementStdDevs)`
Add a vision measurement to the `SwerveDrivePoseEstimator` and update the `SwerveIMU` gyro reading with the given timestamp of the vision measurement.
`private void`
`checkIfTunerXCompatible()`
Check all components to ensure that Tuner X Swerve Generator is recommended instead.
`void`
`drive(edu.wpi.first.math.geometry.Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop)`
The primary method for controlling the drivebase.
`void`
`drive(edu.wpi.first.math.geometry.Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop, edu.wpi.first.math.geometry.Translation2d centerOfRotationMeters)`
The primary method for controlling the drivebase.
`void`
`drive(edu.wpi.first.math.kinematics.ChassisSpeeds velocity)`
Secondary method for controlling the drivebase.
`void`
`drive(edu.wpi.first.math.kinematics.ChassisSpeeds velocity, boolean isOpenLoop, edu.wpi.first.math.geometry.Translation2d centerOfRotationMeters)`
The primary method for controlling the drivebase.
`void`
`drive(edu.wpi.first.math.kinematics.ChassisSpeeds velocity, edu.wpi.first.math.geometry.Translation2d centerOfRotationMeters)`
Secondary method for controlling the drivebase.
`void`
`driveFieldOriented(edu.wpi.first.math.kinematics.ChassisSpeeds velocity)`
Secondary method of controlling the drive base given velocity and adjusting it for field oriented use.
`void`
`driveFieldOriented(edu.wpi.first.math.kinematics.ChassisSpeeds velocity, edu.wpi.first.math.geometry.Translation2d centerOfRotationMeters)`
Secondary method of controlling the drive base given velocity and adjusting it for field oriented use.
`Optional<edu.wpi.first.math.geometry.Translation3d>`
`getAccel()`
Gets current acceleration of the robot in m/s/s.
`edu.wpi.first.math.kinematics.ChassisSpeeds`
`getFieldVelocity()`
Gets the current field-relative velocity (x, y and omega) of the robot
`SwerveIMU`
`getGyro()`
Getter for the `SwerveIMU`.
`edu.wpi.first.math.geometry.Rotation3d`
`getGyroRotation3d()`
Gets the current gyro `Rotation3d` of the robot, as reported by the imu.
`double`
`getMaximumAngularVelocity()`
Get the maximum angular velocity, either `attainableMaxRotationalVelocityRadiansPerSecond` or `SwerveControllerConfiguration.maxAngularVelocity`.
`double`
`getMaximumVelocity()`
Get the maximum velocity from `attainableMaxTranslationalSpeedMetersPerSecond` or `maxSpeedMPS` whichever is higher.
`Map<String,SwerveModule>`
`getModuleMap()`
Get the `SwerveModule`'s as a `HashMap` where the key is the swerve module configuration name.
`edu.wpi.first.math.kinematics.SwerveModulePosition[]`
`getModulePositions()`
Gets the current module positions (azimuth and wheel position (meters)).
`SwerveModule[]`
`getModules()`
Get the `SwerveModule`s associated with the `SwerveDrive`.
`edu.wpi.first.math.geometry.Rotation2d`
`getOdometryHeading()`
Fetch the latest odometry heading, should be trusted over `getYaw()`.
`edu.wpi.first.math.geometry.Rotation2d`
`getPitch()`
Gets the current pitch angle of the robot, as reported by the imu.
`edu.wpi.first.math.geometry.Pose2d`
`getPose()`
Gets the current pose (position and rotation) of the robot, as reported by odometry.
`edu.wpi.first.math.kinematics.ChassisSpeeds`
`getRobotVelocity()`
Gets the current robot-relative velocity (x, y and omega) of the robot
`edu.wpi.first.math.geometry.Rotation2d`
`getRoll()`
Gets the current roll angle of the robot, as reported by the imu.
`edu.wpi.first.math.kinematics.SwerveModuleState[]`
`getStates()`
Gets the current module states (azimuth and velocity)
`SwerveController`
`getSwerveController()`
Helper function to get the `swerveController` for the `SwerveDrive` which can be used to generate `ChassisSpeeds` for the robot to orient it correctly given axis or angles, and apply `SlewRateLimiter` to given inputs.
`edu.wpi.first.math.geometry.Pose2d[]`
`getSwerveModulePoses(edu.wpi.first.math.geometry.Pose2d robotPose)`
Get the swerve module poses and on the field relative to the robot.
`edu.wpi.first.math.geometry.Rotation2d`
`getYaw()`
Gets the current yaw angle of the robot, as reported by the imu.
`void`
`lockPose()`
Point all modules toward the robot center, thus making the robot very difficult to move.
`void`
`postTrajectory(edu.wpi.first.math.trajectory.Trajectory trajectory)`
Post the trajectory to the field
`void`
`pushOffsetsToControllers()`
Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type.
`void`
`replaceSwerveModuleFeedforward(edu.wpi.first.math.controller.SimpleMotorFeedforward feedforward)`
Setup the swerve module feedforward.
`void`
`resetDriveEncoders()`
Reset the drive encoders on the robot, useful when manually resetting the robot without a reboot, like in autonomous.
`void`
`resetOdometry(edu.wpi.first.math.geometry.Pose2d pose)`
Resets odometry to the given pose.
`void`
`restoreInternalOffset()`
Restores Internal YAGSL Encoder offsets and sets the Encoder stored offset back to 0
`void`
`setAngleMotorConversionFactor(double conversionFactor)`
Set the conversion factor for the angle/azimuth motor controller.
`void`
`setChassisSpeeds(edu.wpi.first.math.kinematics.ChassisSpeeds chassisSpeeds)`
Set chassis speeds with closed-loop velocity control.
`void`
`setCosineCompensator(boolean enabled)`
Enable or disable the `SwerveModuleConfiguration.useCosineCompensator` for all `SwerveModule`'s in the swerve drive.
`void`
`setDriveMotorConversionFactor(double conversionFactor)`
Set the conversion factor for the drive motor controller.
`void`
`setGyro(edu.wpi.first.math.geometry.Rotation3d gyro)`
Set the expected gyroscope angle using a `Rotation3d` object.
`void`
`setGyroOffset(edu.wpi.first.math.geometry.Rotation3d offset)`
Set the gyro scope offset to a desired known rotation.
`void`
`setHeadingCorrection(boolean state)`
Set the heading correction capabilities of YAGSL.
`void`
`setHeadingCorrection(boolean state, double deadband)`
Set the heading correction capabilities of YAGSL.
`void`
`setMaximumSpeed(double maximumSpeed)`
Set the maximum speed of the drive motors, modified `maxSpeedMPS` which is used for the `setRawModuleStates(SwerveModuleState[], boolean)` function and `SwerveController.getTargetSpeeds(double, double, double, double, double)` functions.
`void`
`setMaximumSpeed(double maximumSpeed, boolean updateModuleFeedforward, double optimalVoltage)`
Set the maximum speed of the drive motors, modified `maxSpeedMPS` which is used for the `setRawModuleStates(SwerveModuleState[], boolean)` function and `SwerveController.getTargetSpeeds(double, double, double, double, double)` functions.
`void`
`setMaximumSpeeds(double attainableMaxModuleSpeedMetersPerSecond, double attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond)`
Set the maximum speeds for desaturation.
`void`
`setModuleStates(edu.wpi.first.math.kinematics.SwerveModuleState[] desiredStates, boolean isOpenLoop)`
Set the module states (azimuth and velocity) directly.
`void`
`setMotorIdleMode(boolean brake)`
Sets the drive motors to brake/coast mode.
`void`
`setOdometryPeriod(double period)`
Set the odometry update period in seconds.
`private void`
`setRawModuleStates(edu.wpi.first.math.kinematics.SwerveModuleState[] desiredStates, boolean isOpenLoop)`
Set the module states (azimuth and velocity) directly.
`void`
`stopOdometryThread()`
Stop the odometry thread in favor of manually updating odometry.
`void`
`synchronizeModuleEncoders()`
Synchronize angle motor integrated encoders with data from absolute encoders.
`void`
`updateCacheValidityPeriods(long imu, long driveMotor, long absoluteEncoder)`
Update the cache validity period for the robot.
`void`
`updateOdometry()`
Update odometry should be run every loop.
`void`
`zeroGyro()`
Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0.

### Methods inherited from class java.lang.Object

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

## Field Details

### kinematics

public final edu.wpi.first.math.kinematics.SwerveDriveKinematics kinematics
Swerve Kinematics object.

### swerveDriveConfiguration

public final SwerveDriveConfiguration swerveDriveConfiguration
Swerve drive configuration.

### swerveDrivePoseEstimator

public final edu.wpi.first.math.estimator.SwerveDrivePoseEstimator swerveDrivePoseEstimator
Swerve odometry.

### imuReadingCache

public final Cache<edu.wpi.first.math.geometry.Rotation3d> imuReadingCache
IMU reading cache for robot readings.

### swerveModules

private final SwerveModule[] swerveModules
Swerve modules.

### odometryThread

private final edu.wpi.first.wpilibj.Notifier odometryThread
WPILib `Notifier` to keep odometry up to date.

### odometryLock

private final Lock odometryLock
Odometry lock to ensure thread safety.

### tunerXRecommendation

private final Alert tunerXRecommendation
Alert to recommend Tuner X if the configuration is compatible.

### field

public edu.wpi.first.wpilibj.smartdashboard.Field2d field
Field object.

### swerveController

public SwerveController swerveController
Swerve controller for controlling heading of the robot.

### chassisVelocityCorrection

public boolean chassisVelocityCorrection
Correct chassis velocity in `drive(Translation2d, double, boolean, boolean)` using 254's correction.

### headingCorrection

public boolean headingCorrection
Whether to correct heading when driving translationally. Set to true to enable.

### HEADING_CORRECTION_DEADBAND

private double HEADING_CORRECTION_DEADBAND
Deadband for speeds in heading correction.

### imu

private SwerveIMU imu
Swerve IMU device for sensing the heading of the robot.

### simIMU

private SwerveIMUSimulation simIMU
Simulation of the swerve drive.

### moduleSynchronizationCounter

private int moduleSynchronizationCounter
Counter to synchronize the modules relative encoder with absolute encoder when not moving.

### lastHeadingRadians

private double lastHeadingRadians
The last heading set in radians.

### attainableMaxTranslationalSpeedMetersPerSecond

private double attainableMaxTranslationalSpeedMetersPerSecond
The absolute max speed that your robot can reach while translating in meters per second.

### attainableMaxRotationalVelocityRadiansPerSecond

private double attainableMaxRotationalVelocityRadiansPerSecond
The absolute max speed the robot can reach while rotating radians per second.

### maxSpeedMPS

private double maxSpeedMPS
Maximum speed of the robot in meters per second.

## Constructor Details

### SwerveDrive

public SwerveDrive(SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig, double maxSpeedMPS)
Creates a new swerve drivebase subsystem. Robot is controlled via the `drive(edu.wpi.first.math.kinematics.ChassisSpeeds)` method, or via the `setRawModuleStates(edu.wpi.first.math.kinematics.SwerveModuleState[], boolean)` method. The `drive(edu.wpi.first.math.kinematics.ChassisSpeeds)` method incorporates kinematics-- it takes a translation and rotation, as well as parameters for field-centric and closed-loop velocity control. `setRawModuleStates(edu.wpi.first.math.kinematics.SwerveModuleState[], boolean)` takes a list of SwerveModuleStates and directly passes them to the modules. This subsystem also handles odometry.

Parameters:
  `config` - The `SwerveDriveConfiguration` configuration to base the swerve drive off of.
  `controllerConfig` - The `SwerveControllerConfiguration` to use when creating the `SwerveController`.
  `maxSpeedMPS` - Maximum speed in meters per second, remember to use `Units.feetToMeters(double)` if you have feet per second!

## Method Details

### updateCacheValidityPeriods

public void updateCacheValidityPeriods(long imu, long driveMotor, long absoluteEncoder)
Update the cache validity period for the robot.

Parameters:
  `imu` - IMU reading cache validity period in milliseconds.
  `driveMotor` - Drive motor reading cache in milliseconds.
  `absoluteEncoder` - Absolute encoder reading cache in milliseconds.

### checkIfTunerXCompatible

private void checkIfTunerXCompatible()
Check all components to ensure that Tuner X Swerve Generator is recommended instead.

### setOdometryPeriod

public void setOdometryPeriod(double period)
Set the odometry update period in seconds.

Parameters:
  `period` - period in seconds.

### stopOdometryThread

public void stopOdometryThread()
Stop the odometry thread in favor of manually updating odometry.

### setAngleMotorConversionFactor

public void setAngleMotorConversionFactor(double conversionFactor)
Set the conversion factor for the angle/azimuth motor controller.

Parameters:
  `conversionFactor` - Angle motor conversion factor for PID, should be generated from `SwerveMath.calculateDegreesPerSteeringRotation(double, double)` or calculated.

### setDriveMotorConversionFactor

public void setDriveMotorConversionFactor(double conversionFactor)
Set the conversion factor for the drive motor controller.

Parameters:
  `conversionFactor` - Drive motor conversion factor for PID, should be generated from `SwerveMath.calculateMetersPerRotation(double, double, double)` or calculated.

### getOdometryHeading

public edu.wpi.first.math.geometry.Rotation2d getOdometryHeading()
Fetch the latest odometry heading, should be trusted over `getYaw()`.

Returns:
  `Rotation2d` of the robot heading.

### setHeadingCorrection

public void setHeadingCorrection(boolean state)
Set the heading correction capabilities of YAGSL.

Parameters:
  `state` - `headingCorrection` state.

### setHeadingCorrection

public void setHeadingCorrection(boolean state, double deadband)
Set the heading correction capabilities of YAGSL.

Parameters:
  `state` - `headingCorrection` state.
  `deadband` - `HEADING_CORRECTION_DEADBAND` deadband.

### driveFieldOriented

public void driveFieldOriented(edu.wpi.first.math.kinematics.ChassisSpeeds velocity)
Secondary method of controlling the drive base given velocity and adjusting it for field oriented use.

Parameters:
  `velocity` - Velocity of the robot desired.

### driveFieldOriented

public void driveFieldOriented(edu.wpi.first.math.kinematics.ChassisSpeeds velocity, edu.wpi.first.math.geometry.Translation2d centerOfRotationMeters)
Secondary method of controlling the drive base given velocity and adjusting it for field oriented use.

Parameters:
  `velocity` - Velocity of the robot desired.
  `centerOfRotationMeters` - The center of rotation in meters, 0 is the center of the robot.

### drive

public void drive(edu.wpi.first.math.kinematics.ChassisSpeeds velocity)
Secondary method for controlling the drivebase. Given a simple `ChassisSpeeds` set the swerve module states, to achieve the goal.

Parameters:
  `velocity` - The desired robot-oriented `ChassisSpeeds` for the robot to achieve.

### drive

public void drive(edu.wpi.first.math.kinematics.ChassisSpeeds velocity, edu.wpi.first.math.geometry.Translation2d centerOfRotationMeters)
Secondary method for controlling the drivebase. Given a simple `ChassisSpeeds` set the swerve module states, to achieve the goal.

Parameters:
  `velocity` - The desired robot-oriented `ChassisSpeeds` for the robot to achieve.
  `centerOfRotationMeters` - The center of rotation in meters, 0 is the center of the robot.

### drive

public void drive(edu.wpi.first.math.geometry.Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop, edu.wpi.first.math.geometry.Translation2d centerOfRotationMeters)
The primary method for controlling the drivebase. Takes a `Translation2d` and a rotation rate, and calculates and commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel velocities. Also has field- and robot-relative modes, which affect how the translation vector is used.

Parameters:
  `translation` - `Translation2d` that is the commanded linear velocity of the robot, in meters per second. In robot-relative mode, positive x is torwards the bow (front) and positive y is torwards port (left). In field-relative mode, positive x is away from the alliance wall (field North) and positive y is torwards the left wall when looking through the driver station glass (field West).
  `rotation` - Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot relativity.
  `fieldRelative` - Drive mode. True for field-relative, false for robot-relative.
  `isOpenLoop` - Whether to use closed-loop velocity control. Set to true to disable closed-loop.
  `centerOfRotationMeters` - The center of rotation in meters, 0 is the center of the robot.

### drive

public void drive(edu.wpi.first.math.geometry.Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop)
The primary method for controlling the drivebase. Takes a `Translation2d` and a rotation rate, and calculates and commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel velocities. Also has field- and robot-relative modes, which affect how the translation vector is used.

Parameters:
  `translation` - `Translation2d` that is the commanded linear velocity of the robot, in meters per second. In robot-relative mode, positive x is torwards the bow (front) and positive y is torwards port (left). In field-relative mode, positive x is away from the alliance wall (field North) and positive y is torwards the left wall when looking through the driver station glass (field West).
  `rotation` - Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot relativity.
  `fieldRelative` - Drive mode. True for field-relative, false for robot-relative.
  `isOpenLoop` - Whether to use closed-loop velocity control. Set to true to disable closed-loop.

### drive

public void drive(edu.wpi.first.math.kinematics.ChassisSpeeds velocity, boolean isOpenLoop, edu.wpi.first.math.geometry.Translation2d centerOfRotationMeters)
The primary method for controlling the drivebase. Takes a `ChassisSpeeds`, and calculates and commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel velocities. Also has field- and robot-relative modes, which affect how the translation vector is used.

Parameters:
  `velocity` - The chassis speeds to set the robot to achieve.
  `isOpenLoop` - Whether to use closed-loop velocity control. Set to true to disable closed-loop.
  `centerOfRotationMeters` - The center of rotation in meters, 0 is the center of the robot.

### setMaximumSpeeds

public void setMaximumSpeeds(double attainableMaxModuleSpeedMetersPerSecond, double attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond)
Set the maximum speeds for desaturation.

Parameters:
  `attainableMaxModuleSpeedMetersPerSecond` - The absolute max speed that a module can reach in meters per second.
  `attainableMaxTranslationalSpeedMetersPerSecond` - The absolute max speed that your robot can reach while translating in meters per second.
  `attainableMaxRotationalVelocityRadiansPerSecond` - The absolute max speed the robot can reach while rotating in radians per second.

### getMaximumVelocity

public double getMaximumVelocity()
Get the maximum velocity from `attainableMaxTranslationalSpeedMetersPerSecond` or `maxSpeedMPS` whichever is higher.

Returns:
  Maximum speed in meters/second.

### getMaximumAngularVelocity

public double getMaximumAngularVelocity()
Get the maximum angular velocity, either `attainableMaxRotationalVelocityRadiansPerSecond` or `SwerveControllerConfiguration.maxAngularVelocity`.

Returns:
  Maximum angular velocity in radians per second.

### setRawModuleStates

private void setRawModuleStates(edu.wpi.first.math.kinematics.SwerveModuleState[] desiredStates, boolean isOpenLoop)
Set the module states (azimuth and velocity) directly. Used primarily for auto pathing.

Parameters:
  `desiredStates` - A list of SwerveModuleStates to send to the modules.
  `isOpenLoop` - Whether to use closed-loop velocity control. Set to true to disable closed-loop.

### setModuleStates

public void setModuleStates(edu.wpi.first.math.kinematics.SwerveModuleState[] desiredStates, boolean isOpenLoop)
Set the module states (azimuth and velocity) directly. Used primarily for auto paths.

Parameters:
  `desiredStates` - A list of SwerveModuleStates to send to the modules.
  `isOpenLoop` - Whether to use closed-loop velocity control. Set to true to disable closed-loop.

### setChassisSpeeds

public void setChassisSpeeds(edu.wpi.first.math.kinematics.ChassisSpeeds chassisSpeeds)
Set chassis speeds with closed-loop velocity control.

Parameters:
  `chassisSpeeds` - Chassis speeds to set.

### getPose

public edu.wpi.first.math.geometry.Pose2d getPose()
Gets the current pose (position and rotation) of the robot, as reported by odometry.

Returns:
  The robot's pose

### getFieldVelocity

public edu.wpi.first.math.kinematics.ChassisSpeeds getFieldVelocity()
Gets the current field-relative velocity (x, y and omega) of the robot

Returns:
  A ChassisSpeeds object of the current field-relative velocity

### getRobotVelocity

public edu.wpi.first.math.kinematics.ChassisSpeeds getRobotVelocity()
Gets the current robot-relative velocity (x, y and omega) of the robot

Returns:
  A ChassisSpeeds object of the current robot-relative velocity

### resetOdometry

public void resetOdometry(edu.wpi.first.math.geometry.Pose2d pose)
Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this method. However, if either gyro angle or module position is reset, this must be called in order for odometry to keep working.

Parameters:
  `pose` - The pose to set the odometry to

### postTrajectory

public void postTrajectory(edu.wpi.first.math.trajectory.Trajectory trajectory)
Post the trajectory to the field

Parameters:
  `trajectory` - the trajectory to post.

### getStates

public edu.wpi.first.math.kinematics.SwerveModuleState[] getStates()
Gets the current module states (azimuth and velocity)

Returns:
  A list of SwerveModuleStates containing the current module states

### getModulePositions

public edu.wpi.first.math.kinematics.SwerveModulePosition[] getModulePositions()
Gets the current module positions (azimuth and wheel position (meters)).

Returns:
  A list of SwerveModulePositions containg the current module positions

### getGyro

public SwerveIMU getGyro()
Getter for the `SwerveIMU`.

Returns:
  generated `SwerveIMU`

### setGyro

public void setGyro(edu.wpi.first.math.geometry.Rotation3d gyro)
Set the expected gyroscope angle using a `Rotation3d` object. To reset gyro, set to a new `Rotation3d` subtracted from the current gyroscopic readings `SwerveIMU.getRotation3d()`.

Parameters:
  `gyro` - expected gyroscope angle as `Rotation3d`.

### zeroGyro

public void zeroGyro()
Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0.

### getYaw

public edu.wpi.first.math.geometry.Rotation2d getYaw()
Gets the current yaw angle of the robot, as reported by the imu. CCW positive, not wrapped.

Returns:
  The yaw as a `Rotation2d` angle

### getPitch

public edu.wpi.first.math.geometry.Rotation2d getPitch()
Gets the current pitch angle of the robot, as reported by the imu.

Returns:
  The heading as a `Rotation2d` angle

### getRoll

public edu.wpi.first.math.geometry.Rotation2d getRoll()
Gets the current roll angle of the robot, as reported by the imu.

Returns:
  The heading as a `Rotation2d` angle

### getGyroRotation3d

public edu.wpi.first.math.geometry.Rotation3d getGyroRotation3d()
Gets the current gyro `Rotation3d` of the robot, as reported by the imu.

Returns:
  The heading as a `Rotation3d` angle

### getAccel

public Optional<edu.wpi.first.math.geometry.Translation3d> getAccel()
Gets current acceleration of the robot in m/s/s. If gyro unsupported returns empty.

Returns:
  acceleration of the robot as a `Translation3d`

### setMotorIdleMode

public void setMotorIdleMode(boolean brake)
Sets the drive motors to brake/coast mode.

Parameters:
  `brake` - True to set motors to brake mode, false for coast.

### setMaximumSpeed

public void setMaximumSpeed(double maximumSpeed, boolean updateModuleFeedforward, double optimalVoltage)
Set the maximum speed of the drive motors, modified `maxSpeedMPS` which is used for the `setRawModuleStates(SwerveModuleState[], boolean)` function and `SwerveController.getTargetSpeeds(double, double, double, double, double)` functions. This function overrides what was placed in the JSON and could damage your motor/robot if set too high or unachievable rates.

Parameters:
  `maximumSpeed` - Maximum speed for the drive motors in meters / second.
  `updateModuleFeedforward` - Update the swerve module feedforward to account for the new maximum speed. This should be true unless you have replaced the drive motor feedforward with `replaceSwerveModuleFeedforward(SimpleMotorFeedforward)`
  `optimalVoltage` - Optimal voltage to use for the feedforward.

### setMaximumSpeed

public void setMaximumSpeed(double maximumSpeed)
Set the maximum speed of the drive motors, modified `maxSpeedMPS` which is used for the `setRawModuleStates(SwerveModuleState[], boolean)` function and `SwerveController.getTargetSpeeds(double, double, double, double, double)` functions. This function overrides what was placed in the JSON and could damage your motor/robot if set too high or unachievable rates. Overwrites the `SwerveModule.feedforward`.

Parameters:
  `maximumSpeed` - Maximum speed for the drive motors in meters / second.

### lockPose

public void lockPose()
Point all modules toward the robot center, thus making the robot very difficult to move. Forcing the robot to keep the current pose.

### getSwerveModulePoses

public edu.wpi.first.math.geometry.Pose2d[] getSwerveModulePoses(edu.wpi.first.math.geometry.Pose2d robotPose)
Get the swerve module poses and on the field relative to the robot.

Parameters:
  `robotPose` - Robot pose.
Returns:
  Swerve module poses.

### replaceSwerveModuleFeedforward

public void replaceSwerveModuleFeedforward(edu.wpi.first.math.controller.SimpleMotorFeedforward feedforward)
Setup the swerve module feedforward.

Parameters:
  `feedforward` - Feedforward for the drive motor on swerve modules.

### updateOdometry

public void updateOdometry()
Update odometry should be run every loop. Synchronizes module absolute encoders with relative encoders periodically. In simulation mode will also post the pose of each module. Updates SmartDashboard with module encoder readings and states.

### synchronizeModuleEncoders

public void synchronizeModuleEncoders()
Synchronize angle motor integrated encoders with data from absolute encoders.

### setGyroOffset

public void setGyroOffset(edu.wpi.first.math.geometry.Rotation3d offset)
Set the gyro scope offset to a desired known rotation. Unlike `setGyro(Rotation3d)` it DOES NOT take the current rotation into account.

Parameters:
  `offset` - `Rotation3d` known offset of the robot for gyroscope to use.

### addVisionMeasurement

public void addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d robotPose, double timestamp, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> visionMeasurementStdDevs)
Add a vision measurement to the `SwerveDrivePoseEstimator` and update the `SwerveIMU` gyro reading with the given timestamp of the vision measurement.

Parameters:
  `robotPose` - Robot `Pose2d` as measured by vision.
  `timestamp` - Timestamp the measurement was taken as time since startup, should be taken from `Timer.getFPGATimestamp()` or similar sources.
  `visionMeasurementStdDevs` - Vision measurement standard deviation that will be sent to the `SwerveDrivePoseEstimator`.The standard deviation of the vision measurement, for best accuracy calculate the standard deviation at 2 or more points and fit a line to it with the calculated optimal standard deviation. (Units should be meters per pixel). By optimizing this you can get * vision accurate to inches instead of feet.

### addVisionMeasurement

public void addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d robotPose, double timestamp)
Add a vision measurement to the `SwerveDrivePoseEstimator` and update the `SwerveIMU` gyro reading with the given timestamp of the vision measurement.

Parameters:
  `robotPose` - Robot `Pose2d` as measured by vision.
  `timestamp` - Timestamp the measurement was taken as time since startup, should be taken from `Timer.getFPGATimestamp()` or similar sources.

### getSwerveController

public SwerveController getSwerveController()
Helper function to get the `swerveController` for the `SwerveDrive` which can be used to generate `ChassisSpeeds` for the robot to orient it correctly given axis or angles, and apply `SlewRateLimiter` to given inputs. Important functions to look at are `SwerveController.getTargetSpeeds(double, double, double, double, double)`, `SwerveController.addSlewRateLimiters(SlewRateLimiter, SlewRateLimiter, SlewRateLimiter)`, `SwerveController.getRawTargetSpeeds(double, double, double)`.

Returns:
  `SwerveController` for the `SwerveDrive`.

### getModules

public SwerveModule[] getModules()
Get the `SwerveModule`s associated with the `SwerveDrive`.

Returns:
  `SwerveModule` array specified by configurations.

### getModuleMap

public Map<String,SwerveModule> getModuleMap()
Get the `SwerveModule`'s as a `HashMap` where the key is the swerve module configuration name.

Returns:
  `HashMap`(Module Name, SwerveModule)

### resetDriveEncoders

public void resetDriveEncoders()
Reset the drive encoders on the robot, useful when manually resetting the robot without a reboot, like in autonomous.

### pushOffsetsToControllers

public void pushOffsetsToControllers()
Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the internal offsets to prevent double offsetting.

### restoreInternalOffset

public void restoreInternalOffset()
Restores Internal YAGSL Encoder offsets and sets the Encoder stored offset back to 0

### setCosineCompensator

public void setCosineCompensator(boolean enabled)
Enable or disable the `SwerveModuleConfiguration.useCosineCompensator` for all `SwerveModule`'s in the swerve drive. The cosine compensator will slow down or speed up modules that are close to their desired state in theory.

Parameters:
  `enabled` - Usage of the cosine compensator.