+
+
+
-
edu.wpi.first.math.geometry.Rotation3d
-
-
+
edu.wpi.first.math.geometry.Rotation3d
+
+
Gets the current gyro Rotation3d of the robot, as reported by the imu.
-
double
-
-
+
double
+
+
-
double
-
-
+
double
+
+
-
-
-
-
edu.wpi.first.math.kinematics.SwerveModulePosition[]
-
+
+
-
Gets the current module positions (azimuth and wheel position (meters)).
+
-
-
+
edu.wpi.first.math.kinematics.SwerveModulePosition[]
+
-
+
Gets the current module positions (azimuth and wheel position (meters)).
-
edu.wpi.first.math.geometry.Rotation2d
-
+
+
-
Fetch the latest odometry heading, should be trusted over
getYaw().
+
edu.wpi.first.math.geometry.Rotation2d
-
+
-
Gets the current pitch angle of the robot, as reported by the imu.
+
Fetch the latest odometry heading, should be trusted over
getYaw().
-
edu.wpi.first.math.geometry.Pose2d
-
+
edu.wpi.first.math.geometry.Rotation2d
+
-
Gets the current pose (position and rotation) of the robot, as reported by odometry.
+
Gets the current pitch angle of the robot, as reported by the imu.
-
edu.wpi.first.math.kinematics.ChassisSpeeds
-
+
edu.wpi.first.math.geometry.Pose2d
+
-
Gets the current robot-relative velocity (x, y and omega) of the robot
+
Gets the current pose (position and rotation) of the robot, as reported by odometry.
-
edu.wpi.first.math.geometry.Rotation2d
-
+
edu.wpi.first.math.kinematics.ChassisSpeeds
+
-
Gets the current roll angle of the robot, as reported by the imu.
+
Gets the current robot-relative velocity (x, y and omega) of the robot
-
edu.wpi.first.math.kinematics.SwerveModuleState[]
-
+
edu.wpi.first.math.geometry.Rotation2d
+
-
Gets the current module states (azimuth and velocity)
+
Gets the current roll angle of the robot, as reported by the imu.
-
-
+
edu.wpi.first.math.kinematics.SwerveModuleState[]
+
+
Gets the current module states (azimuth and velocity)
+
+
+
+
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[]
-
-
+
edu.wpi.first.math.geometry.Pose2d[]
+
+
Get the swerve module poses and on the field relative to the robot.
-
edu.wpi.first.math.geometry.Rotation2d
-
-
+
edu.wpi.first.math.geometry.Rotation2d
+
+
Gets the current yaw angle of the robot, as reported by the imu.
-
void
-
-
+
void
+
+
Point all modules toward the robot center, thus making the robot very difficult to move.
+
private edu.wpi.first.math.kinematics.ChassisSpeeds
+
movementOptimizations (edu.wpi.first.math.kinematics.ChassisSpeeds velocity,
+ boolean uesChassisDiscretize,
+ boolean useAngularVelocitySkewCorrection)
+
+
Enable desired drive corrections
+
void
@@ -437,15 +476,30 @@
Method Summary
Set the conversion factor for the angle/azimuth motor controller.
void
-
+
+
Enables angular velocity skew correction in teleop and/or autonomous
+ and sets the angular velocity coefficient for both modes
+
+
void
+
+
Enable auto-centering module wheels.
+
void
+
+
+
Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop and/or auto
+
void
-
Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction
+
Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop
void
@@ -488,7 +542,7 @@
Method Summary
void
@@ -497,7 +551,7 @@
Method Summary
double optimalVoltage)
void
@@ -508,57 +562,64 @@
Method Summary
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.
+
Enable auto synchronization for encoders during a match.
void
-
+
setModuleStates (edu.wpi.first.math.kinematics.SwerveModuleState[] desiredStates,
+ boolean isOpenLoop)
-
Sets the drive motors to brake/coast mode.
+
Set the module states (azimuth and velocity) directly.
void
-
+
-
Set the odometry update period in seconds.
+
Sets the drive motors to brake/coast mode.
-
private void
-
setRawModuleStates (edu.wpi.first.math.kinematics.SwerveModuleState[] desiredStates,
- boolean isOpenLoop)
+
void
+
-
Set the module states (azimuth and velocity) directly.
+
Set the odometry update period in seconds.
-
void
-
setVisionMeasurementStdDevs (edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> visionMeasurementStdDevs)
+
private void
+
setRawModuleStates (edu.wpi.first.math.kinematics.SwerveModuleState[] desiredStates,
+ edu.wpi.first.math.kinematics.ChassisSpeeds desiredChassisSpeed,
+ boolean isOpenLoop)
-
Sets the pose estimator's trust of global measurements.
+
Set the module states (azimuth and velocity) directly.
void
-
+
setVisionMeasurementStdDevs (edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> visionMeasurementStdDevs)
-
Stop the odometry thread in favor of manually updating odometry.
+
Sets the pose estimator's trust of global measurements.
void
-
+
-
Synchronize angle motor integrated encoders with data from absolute encoders.
+
Stop the odometry thread in favor of manually updating odometry.
void
-
+
-
Update the cache validity period for the robot.
+
Synchronize angle motor integrated encoders with data from absolute encoders.
void
-
+
-
Update odometry should be run every loop.
+
Update the cache validity period for the robot.
void
-
+
+
Update odometry should be run every loop.
+
+
void
+
+
Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0.
@@ -657,6 +718,36 @@
chassisVelocityCorrection
+
+autonomousChassisVelocityCorrection
+public boolean autonomousChassisVelocityCorrection
+
+
+
+
+
+angularVelocityCorrection
+public boolean angularVelocityCorrection
+
+
+
+
+
+autonomousAngularVelocityCorrection
+public boolean autonomousAngularVelocityCorrection
+
+
+
+
+
+angularVelocityCoefficient
+public double angularVelocityCoefficient
+Angular Velocity Correction Coefficent (expected values between -0.15 and 0.15).
+
+
+
headingCorrection
public boolean headingCorrection
@@ -685,6 +776,14 @@ imu
+
+
+
Parameters:
@@ -1036,14 +1135,16 @@ getMaximumAngularVelocity
-
+
setRawModuleStates
private void setRawModuleStates (edu.wpi.first.math.kinematics.SwerveModuleState[] desiredStates,
+ edu.wpi.first.math.kinematics.ChassisSpeeds desiredChassisSpeed,
boolean isOpenLoop)
Set the module states (azimuth and velocity) directly.
Parameters:
desiredStates - A list of SwerveModuleStates to send to the modules.desiredChassisSpeed - The desired chassis speeds to set the robot to achieve.isOpenLoop - Whether to use closed-loop velocity control. Set to true to disable closed-loop.
@@ -1053,7 +1154,10 @@ setRawModuleStates
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.
+Set the module states (azimuth and velocity) directly. Used primarily for auto paths.
+ Does not allow for usage of desaturateWheelSpeeds(SwerveModuleState[] moduleStates,
+ ChassisSpeeds desiredChassisSpeed, double attainableMaxModuleSpeedMetersPerSecond,
+ double attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond)
Parameters:
desiredStates - A list of SwerveModuleStates to send to the modules.setMotorIdleMode
+
+setModuleEncoderAutoSynchronize
+public void setModuleEncoderAutoSynchronize (boolean enabled,
+ double deadband)
+Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a few seconds.
+
+Parameters:
+enabled - Enable statedeadband - Deadband in degrees, default is 3 degrees.
+
+
+
setMaximumSpeed
public void setMaximumSpeed (double maximumSpeed,
boolean updateModuleFeedforward,
double optimalVoltage)
@@ -1272,7 +1389,7 @@ setMaximumSpeed
setMaximumSpeed
public void setMaximumSpeed (double maximumSpeed)
@@ -1485,7 +1602,7 @@ setCosineCompensator
setChassisDiscretization
public void setChassisDiscretization (boolean enable,
double dtSeconds)
-Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction
+Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop
Parameters:
enable - Enable chassis velocity correction, which will use
@@ -1494,6 +1611,75 @@ setChassisDiscretization
+
+
+setChassisDiscretization
+public void setChassisDiscretization (boolean useInTeleop,
+ boolean useInAuto,
+ double dtSeconds)
+Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop and/or auto
+
+Parameters:
+useInTeleop - Enable chassis velocity correction, which will use
+ ChassisSpeeds.discretize(ChassisSpeeds, double) with the following in teleop.useInAuto - Enable chassis velocity correction, which will use
+ ChassisSpeeds.discretize(ChassisSpeeds, double) with the following in auto.dtSeconds - The duration of the timestep the speeds should be applied for.
+
+
+
+
+setAngularVelocityCompensation
+public void setAngularVelocityCompensation (boolean useInTeleop,
+ boolean useInAuto,
+ double angularVelocityCoeff)
+Enables angular velocity skew correction in teleop and/or autonomous
+ and sets the angular velocity coefficient for both modes
+
+Parameters:
+useInTeleop - Enables angular velocity correction in teleop.useInAuto - Enables angular velocity correction in autonomous.angularVelocityCoeff - The angular velocity coefficient. Expected values between -0.15 to 0.15.
+ Start with a value of 0.1, test in teleop.
+ When enabling for the first time if the skew is significantly worse try inverting the value.
+ Tune by moving in a straight line while rotating. Testing is best done with angular velocity controls on the right stick.
+ Change the value until you are visually happy with the skew.
+ Ensure your tune works with different translational and rotational magnitudes.
+ If this reduces skew in teleop, it may improve auto.
+
+
+
+
+angularVelocitySkewCorrection
+public edu.wpi.first.math.kinematics.ChassisSpeeds angularVelocitySkewCorrection (edu.wpi.first.math.kinematics.ChassisSpeeds velocity)
+Correct for skew that worsens as angular velocity increases
+
+Parameters:
+velocity - The chassis speeds to set the robot to achieve.Returns:
+ChassisSpeeds of the robot after angular velocity skew correction.
+
+
+
+
+movementOptimizations
+private edu.wpi.first.math.kinematics.ChassisSpeeds movementOptimizations (edu.wpi.first.math.kinematics.ChassisSpeeds velocity,
+ boolean uesChassisDiscretize,
+ boolean useAngularVelocitySkewCorrection)
+Enable desired drive corrections
+
+Parameters:
+velocity - The chassis speeds to set the robot to achieve.uesChassisDiscretize - Correct chassis velocity using 254's correction.useAngularVelocitySkewCorrection - Use the robot's angular velocity to correct for skew.Returns:
+The chassis speeds after optimizations.
+
+
+
diff --git a/docs/swervelib/SwerveDriveTest.html b/docs/swervelib/SwerveDriveTest.html
index fbbef67..adfbb0a 100644
--- a/docs/swervelib/SwerveDriveTest.html
+++ b/docs/swervelib/SwerveDriveTest.html
@@ -1,11 +1,11 @@
-
+
SwerveDriveTest
-
+
diff --git a/docs/swervelib/SwerveModule.html b/docs/swervelib/SwerveModule.html
index 718665e..6f3b7e1 100644
--- a/docs/swervelib/SwerveModule.html
+++ b/docs/swervelib/SwerveModule.html
@@ -1,11 +1,11 @@
-
+
SwerveModule
-
+
@@ -201,6 +201,16 @@
Field Summary
+
private double
+
+
+
Encoder synchronization deadband in degrees.
+
+
private boolean
+
+
+
Encoder, Absolute encoder synchronization enabled.
+
private boolean
@@ -365,6 +375,17 @@
Method Summary
Set the drive PIDF values.
void
+
+
+
Enable auto synchronization for encoders during a match.
+
+
void
+
+
+
Enable auto synchronization for encoders during a match.
+
+
void
setFeedforward (edu.wpi.first.math.controller.SimpleMotorFeedforward drive)
Set the feedforward attributes to the given parameters.
@@ -557,6 +578,20 @@
synchronizeEncoderQueued
Encoder synchronization queued.
+
+
+synchronizeEncoderEnabled
+private boolean synchronizeEncoderEnabled
+Encoder, Absolute encoder synchronization enabled.
+
+
+
+
+synchronizeEncoderDeadband
+private double synchronizeEncoderDeadband
+Encoder synchronization deadband in degrees.
+
+
@@ -619,6 +654,30 @@
queueSynchronizeEncoders
+
+setEncoderAutoSynchronize
+public void setEncoderAutoSynchronize (boolean enabled,
+ double deadband)
+Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a few seconds.
+
+Parameters:
+enabled - Enable statedeadband - Deadband in degrees, default is 3 degrees.
+
+
+
+
+setEncoderAutoSynchronize
+public void setEncoderAutoSynchronize (boolean enabled)
+Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a few seconds.
+
+Parameters:
+enabled - Enable state
+
+
+
setAntiJitter
public void setAntiJitter (boolean antiJitter)
diff --git a/docs/swervelib/encoders/AnalogAbsoluteEncoderSwerve.html b/docs/swervelib/encoders/AnalogAbsoluteEncoderSwerve.html
index 05797e7..88176a5 100644
--- a/docs/swervelib/encoders/AnalogAbsoluteEncoderSwerve.html
+++ b/docs/swervelib/encoders/AnalogAbsoluteEncoderSwerve.html
@@ -1,11 +1,11 @@
-
+
AnalogAbsoluteEncoderSwerve
CANCoderSwerve
CanAndMagSwerve
PWMDutyCycleEncoderSwerve
SparkMaxAnalogEncoderSwerve
SparkMaxEncoderSwerve
SwerveAbsoluteEncoder
swervelib.encoders
swervelib.encoders Class Hierarchy
ADIS16448Swerve
ADIS16470Swerve
ADXRS450Swerve
AnalogGyroSwerve
IMUVelocity
+
+JavaScript is disabled on your browser.
+
+
+
+
+
+
+
+
+
+public class IMUVelocity
+
extends Object Generic IMU Velocity filter.
+
+
+
+
+
+
+Field Summary
+Fields
+
+
+
+
+
private boolean
+
+
+
Prevents calculation when no previous measurement exists.
+
+
+
+
+
private final edu.wpi.first.wpilibj.Notifier
+
+
+
WPILib Notifier to keep IMU velocity up to date.
+
+
private edu.wpi.first.math.geometry.Rotation2d
+
+
+
Tracks the previous loop's position as a Rotation2d.
+
+
private double
+
+
+
Tracks the previous loop's recorded time.
+
+
private double
+
+
+
The calculated velocity of the robot based on averaged IMU measurements.
+
+
+
+
+
Linear filter used to calculate velocity, we use a custom filter class
+ to prevent unwanted operations.
+
+
+
+
+
+
+Constructor Summary
+Constructors
+
+
+
+
+
+
Constructor for the IMU Velocity.
+
+
+
+
+
+
+Method Summary
+
+
All Methods Static Methods Instance Methods Concrete Methods
+
+
+
+
+
+
+
+
+
Static factory for IMU Velocity.
+
+
double
+
+
+
Get the robot's angular velocity based on averaged meaasurements from the IMU.
+
+
private void
+
+
+
Update the robot's rotational velocity based on the current gyro position.
+
+
+
+
+
Methods inherited from class java.lang.Object
+
clone , equals , finalize , getClass , hashCode , notify , notifyAll , toString , wait , wait , wait
+
+
+
+
+
+
+
+
+Field Details
+
+
+
+
+
+
+velocityFilter
+
+Linear filter used to calculate velocity, we use a custom filter class
+ to prevent unwanted operations.
+
+
+
+
+notifier
+private final edu.wpi.first.wpilibj.Notifier notifier
+WPILib Notifier to keep IMU velocity up to date.
+
+
+
+
+firstCycle
+private boolean firstCycle
+Prevents calculation when no previous measurement exists.
+
+
+
+
+timestamp
+private double timestamp
+Tracks the previous loop's recorded time.
+
+
+
+
+position
+private edu.wpi.first.math.geometry.Rotation2d position
+Tracks the previous loop's position as a Rotation2d.
+
+
+
+
+velocity
+private double velocity
+The calculated velocity of the robot based on averaged IMU measurements.
+
+
+
+
+
+
+
+
+Constructor Details
+
+
+
+IMUVelocity
+public IMUVelocity (SwerveIMU gyro,
+ double periodSeconds,
+ int averagingTaps) Constructor for the IMU Velocity.
+
+Parameters:
+gyro - The SwerveIMU gyro.periodSeconds - The rate to collect measurements from the gyro, in the form (1/number of samples per second),
+ make sure this does not exceed the update rate of your IMU.averagingTaps - The number of samples to used for the moving average linear filter. Higher values will not
+ allow the system to update to changes in velocity, lower values may create a less smooth signal. Expected taps
+ will probably be ~2-8, with the goal of having the lowest smooth value.
+
+
+
+
+
+
+
+
+Method Details
+
+
+
+createIMUVelocity
+
+Static factory for IMU Velocity. Supported IMU rates will be as quick as possible
+ but will not exceed 100hz and will use 5 taps (supported IMUs are listed in swervelib's IMU folder).
+ Other gyroscopes will default to 50hz and 5 taps. For custom rates please use the IMUVelocity constructor.
+
+Parameters:
+gyro - The SwerveIMU gyro.Returns:
+IMUVelocity
+
+
+
+
+update
+private void update ()
+Update the robot's rotational velocity based on the current gyro position.
+
+
+
+
+getVelocity
+public double getVelocity ()
+Get the robot's angular velocity based on averaged meaasurements from the IMU.
+ Velocity is multiplied by 1e+6 (1,000,000) because the timestamps are in microseconds.
+
+Returns:
+robot's angular velocity in rads/s as a double.
+
+
+
+
+
+
+
+
+
+
+
+
NavXSwerve
Pigeon2Swerve
Field Summary
-(package private) com.ctre.phoenix6.hardware.Pigeon2
-
+private com.ctre.phoenix6.configs.Pigeon2Configurator
+
-
Pigeon2 IMU device.
+
Pigeon2 configurator.
private boolean
-
+(package private) com.ctre.phoenix6.hardware.Pigeon2
+
-
Inversion for the gyro
+
Pigeon2 IMU device.
private edu.wpi.first.math.geometry.Rotation3d
-
+private boolean
+
-
Offset for the Pigeon 2.
+
Inversion for the gyro
static double
-
+private edu.wpi.first.math.geometry.Rotation3d
+
+
Offset for the Pigeon 2.
+
static double
+
+
Wait time for status frames to show up.
@@ -237,6 +242,13 @@
invertedIMU
Inversion for the gyro
+
+
+cfg
+private com.ctre.phoenix6.configs.Pigeon2Configurator cfg
+Pigeon2 configurator.
+
+
diff --git a/docs/swervelib/imu/PigeonSwerve.html b/docs/swervelib/imu/PigeonSwerve.html
index 43cb3eb..bcf3f05 100644
--- a/docs/swervelib/imu/PigeonSwerve.html
+++ b/docs/swervelib/imu/PigeonSwerve.html
@@ -1,11 +1,11 @@
-
+
PigeonSwerve
-
+
diff --git a/docs/swervelib/imu/SwerveIMU.html b/docs/swervelib/imu/SwerveIMU.html
index b5f0e97..ddeee9e 100644
--- a/docs/swervelib/imu/SwerveIMU.html
+++ b/docs/swervelib/imu/SwerveIMU.html
@@ -1,11 +1,11 @@
-
+
SwerveIMU
-
+
diff --git a/docs/swervelib/imu/package-summary.html b/docs/swervelib/imu/package-summary.html
index 9f931ea..8f606f0 100644
--- a/docs/swervelib/imu/package-summary.html
+++ b/docs/swervelib/imu/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib.imu
-
+
@@ -100,20 +100,24 @@
Package swervelib.imu
Creates a IMU for AnalogGyro devices, only uses yaw.
-
+
-
Communicates with the NavX as the IMU.
+
Generic IMU Velocity filter.
-
+
-
SwerveIMU interface for the Pigeon2
+
Communicates with the NavX as the IMU.
-
+
-
SwerveIMU interface for the Pigeon.
+
SwerveIMU interface for the Pigeon2
-
+
+
SwerveIMU interface for the Pigeon.
+
+
+
Swerve IMU abstraction to define a standard interface with a swerve drive.
diff --git a/docs/swervelib/imu/package-tree.html b/docs/swervelib/imu/package-tree.html
index 60d4d97..b5ef9c3 100644
--- a/docs/swervelib/imu/package-tree.html
+++ b/docs/swervelib/imu/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.imu Class Hierarchy
-
+
@@ -59,6 +59,7 @@
Class Hierarchy
@@ -406,6 +411,13 @@
configuration
Current TalonFX configuration.
+
+
+cfg
+private com.ctre.phoenix6.configs.TalonFXConfigurator cfg
+Current TalonFX Configurator.
+
+
diff --git a/docs/swervelib/motors/TalonSRXSwerve.html b/docs/swervelib/motors/TalonSRXSwerve.html
index 4616121..8f80cdb 100644
--- a/docs/swervelib/motors/TalonSRXSwerve.html
+++ b/docs/swervelib/motors/TalonSRXSwerve.html
@@ -1,11 +1,11 @@
-
+
TalonSRXSwerve
-
+
diff --git a/docs/swervelib/motors/package-summary.html b/docs/swervelib/motors/package-summary.html
index 32b6906..d53ae47 100644
--- a/docs/swervelib/motors/package-summary.html
+++ b/docs/swervelib/motors/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib.motors
-
+
diff --git a/docs/swervelib/motors/package-tree.html b/docs/swervelib/motors/package-tree.html
index 0a2e6a8..347091a 100644
--- a/docs/swervelib/motors/package-tree.html
+++ b/docs/swervelib/motors/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.motors Class Hierarchy
-
+
diff --git a/docs/swervelib/package-summary.html b/docs/swervelib/package-summary.html
index fc47dd3..a053e1a 100644
--- a/docs/swervelib/package-summary.html
+++ b/docs/swervelib/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib
-
+
diff --git a/docs/swervelib/package-tree.html b/docs/swervelib/package-tree.html
index 1a91c89..f14b80c 100644
--- a/docs/swervelib/package-tree.html
+++ b/docs/swervelib/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib Class Hierarchy
-
+
diff --git a/docs/swervelib/parser/Cache.html b/docs/swervelib/parser/Cache.html
index 4f53168..9a2b34c 100644
--- a/docs/swervelib/parser/Cache.html
+++ b/docs/swervelib/parser/Cache.html
@@ -1,11 +1,11 @@
-
+
Cache
-
+
diff --git a/docs/swervelib/parser/PIDFConfig.html b/docs/swervelib/parser/PIDFConfig.html
index 9cfac15..ed95cbc 100644
--- a/docs/swervelib/parser/PIDFConfig.html
+++ b/docs/swervelib/parser/PIDFConfig.html
@@ -1,11 +1,11 @@
-
+
PIDFConfig
-
+
diff --git a/docs/swervelib/parser/SwerveControllerConfiguration.html b/docs/swervelib/parser/SwerveControllerConfiguration.html
index 837b99e..b0a1b6b 100644
--- a/docs/swervelib/parser/SwerveControllerConfiguration.html
+++ b/docs/swervelib/parser/SwerveControllerConfiguration.html
@@ -1,11 +1,11 @@
-
+
SwerveControllerConfiguration
-
+
diff --git a/docs/swervelib/parser/SwerveDriveConfiguration.html b/docs/swervelib/parser/SwerveDriveConfiguration.html
index 3cddbea..1fe89f2 100644
--- a/docs/swervelib/parser/SwerveDriveConfiguration.html
+++ b/docs/swervelib/parser/SwerveDriveConfiguration.html
@@ -1,11 +1,11 @@
-
+
SwerveDriveConfiguration
-
+
diff --git a/docs/swervelib/parser/SwerveModuleConfiguration.html b/docs/swervelib/parser/SwerveModuleConfiguration.html
index 8093286..faf6771 100644
--- a/docs/swervelib/parser/SwerveModuleConfiguration.html
+++ b/docs/swervelib/parser/SwerveModuleConfiguration.html
@@ -1,11 +1,11 @@
-
+
SwerveModuleConfiguration
-
+
diff --git a/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html b/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html
index 8bb911f..4881e30 100644
--- a/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html
+++ b/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html
@@ -1,11 +1,11 @@
-
+
SwerveModulePhysicalCharacteristics
-
+
diff --git a/docs/swervelib/parser/SwerveParser.html b/docs/swervelib/parser/SwerveParser.html
index 604278a..85e2e58 100644
--- a/docs/swervelib/parser/SwerveParser.html
+++ b/docs/swervelib/parser/SwerveParser.html
@@ -1,11 +1,11 @@
-
+
SwerveParser
-
+
diff --git a/docs/swervelib/parser/deserializer/PIDFRange.html b/docs/swervelib/parser/deserializer/PIDFRange.html
index aa0f379..8cb57f5 100644
--- a/docs/swervelib/parser/deserializer/PIDFRange.html
+++ b/docs/swervelib/parser/deserializer/PIDFRange.html
@@ -1,11 +1,11 @@
-
+
PIDFRange
-
+
diff --git a/docs/swervelib/parser/deserializer/package-summary.html b/docs/swervelib/parser/deserializer/package-summary.html
index 3880035..e0aea80 100644
--- a/docs/swervelib/parser/deserializer/package-summary.html
+++ b/docs/swervelib/parser/deserializer/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib.parser.deserializer
-
+
diff --git a/docs/swervelib/parser/deserializer/package-tree.html b/docs/swervelib/parser/deserializer/package-tree.html
index 713ef40..a5529ea 100644
--- a/docs/swervelib/parser/deserializer/package-tree.html
+++ b/docs/swervelib/parser/deserializer/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.parser.deserializer Class Hierarchy
-
+
diff --git a/docs/swervelib/parser/json/ControllerPropertiesJson.html b/docs/swervelib/parser/json/ControllerPropertiesJson.html
index 27bcd9f..6e04c9e 100644
--- a/docs/swervelib/parser/json/ControllerPropertiesJson.html
+++ b/docs/swervelib/parser/json/ControllerPropertiesJson.html
@@ -1,11 +1,11 @@
-
+
ControllerPropertiesJson
-
+
diff --git a/docs/swervelib/parser/json/DeviceJson.html b/docs/swervelib/parser/json/DeviceJson.html
index 899c2df..dfdfa3d 100644
--- a/docs/swervelib/parser/json/DeviceJson.html
+++ b/docs/swervelib/parser/json/DeviceJson.html
@@ -1,11 +1,11 @@
-
+
DeviceJson
-
+
diff --git a/docs/swervelib/parser/json/ModuleJson.html b/docs/swervelib/parser/json/ModuleJson.html
index 2b5cfcc..16ac8ca 100644
--- a/docs/swervelib/parser/json/ModuleJson.html
+++ b/docs/swervelib/parser/json/ModuleJson.html
@@ -1,11 +1,11 @@
-
+
ModuleJson
-
+
diff --git a/docs/swervelib/parser/json/MotorConfigDouble.html b/docs/swervelib/parser/json/MotorConfigDouble.html
index bdd707f..e1ae9a5 100644
--- a/docs/swervelib/parser/json/MotorConfigDouble.html
+++ b/docs/swervelib/parser/json/MotorConfigDouble.html
@@ -1,11 +1,11 @@
-
+
MotorConfigDouble
-
+
diff --git a/docs/swervelib/parser/json/MotorConfigInt.html b/docs/swervelib/parser/json/MotorConfigInt.html
index 1802865..bf943d8 100644
--- a/docs/swervelib/parser/json/MotorConfigInt.html
+++ b/docs/swervelib/parser/json/MotorConfigInt.html
@@ -1,11 +1,11 @@
-
+
MotorConfigInt
-
+
diff --git a/docs/swervelib/parser/json/PIDFPropertiesJson.html b/docs/swervelib/parser/json/PIDFPropertiesJson.html
index 1356166..0054d60 100644
--- a/docs/swervelib/parser/json/PIDFPropertiesJson.html
+++ b/docs/swervelib/parser/json/PIDFPropertiesJson.html
@@ -1,11 +1,11 @@
-
+
PIDFPropertiesJson
-
+
diff --git a/docs/swervelib/parser/json/PhysicalPropertiesJson.html b/docs/swervelib/parser/json/PhysicalPropertiesJson.html
index d866f60..d353dfe 100644
--- a/docs/swervelib/parser/json/PhysicalPropertiesJson.html
+++ b/docs/swervelib/parser/json/PhysicalPropertiesJson.html
@@ -1,11 +1,11 @@
-
+
PhysicalPropertiesJson
-
+
diff --git a/docs/swervelib/parser/json/SwerveDriveJson.html b/docs/swervelib/parser/json/SwerveDriveJson.html
index 6fddcfc..2b2b9fd 100644
--- a/docs/swervelib/parser/json/SwerveDriveJson.html
+++ b/docs/swervelib/parser/json/SwerveDriveJson.html
@@ -1,11 +1,11 @@
-
+
SwerveDriveJson
-
+
diff --git a/docs/swervelib/parser/json/modules/AngleConversionFactorsJson.html b/docs/swervelib/parser/json/modules/AngleConversionFactorsJson.html
index e939b7e..f2ed9f9 100644
--- a/docs/swervelib/parser/json/modules/AngleConversionFactorsJson.html
+++ b/docs/swervelib/parser/json/modules/AngleConversionFactorsJson.html
@@ -1,11 +1,11 @@
-
+
AngleConversionFactorsJson
-
+
diff --git a/docs/swervelib/parser/json/modules/BoolMotorJson.html b/docs/swervelib/parser/json/modules/BoolMotorJson.html
index a58ea59..7b4fbb1 100644
--- a/docs/swervelib/parser/json/modules/BoolMotorJson.html
+++ b/docs/swervelib/parser/json/modules/BoolMotorJson.html
@@ -1,11 +1,11 @@
-
+
BoolMotorJson
-
+
diff --git a/docs/swervelib/parser/json/modules/ConversionFactorsJson.html b/docs/swervelib/parser/json/modules/ConversionFactorsJson.html
index 1229664..8456688 100644
--- a/docs/swervelib/parser/json/modules/ConversionFactorsJson.html
+++ b/docs/swervelib/parser/json/modules/ConversionFactorsJson.html
@@ -1,11 +1,11 @@
-
+
ConversionFactorsJson
-
+
diff --git a/docs/swervelib/parser/json/modules/DriveConversionFactorsJson.html b/docs/swervelib/parser/json/modules/DriveConversionFactorsJson.html
index da3a82f..69fe288 100644
--- a/docs/swervelib/parser/json/modules/DriveConversionFactorsJson.html
+++ b/docs/swervelib/parser/json/modules/DriveConversionFactorsJson.html
@@ -1,11 +1,11 @@
-
+
DriveConversionFactorsJson
-
+
diff --git a/docs/swervelib/parser/json/modules/LocationJson.html b/docs/swervelib/parser/json/modules/LocationJson.html
index d0d856d..762b4e7 100644
--- a/docs/swervelib/parser/json/modules/LocationJson.html
+++ b/docs/swervelib/parser/json/modules/LocationJson.html
@@ -1,11 +1,11 @@
-
+
LocationJson
-
+
diff --git a/docs/swervelib/parser/json/modules/package-summary.html b/docs/swervelib/parser/json/modules/package-summary.html
index 27a8257..74bfec6 100644
--- a/docs/swervelib/parser/json/modules/package-summary.html
+++ b/docs/swervelib/parser/json/modules/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib.parser.json.modules
-
+
diff --git a/docs/swervelib/parser/json/modules/package-tree.html b/docs/swervelib/parser/json/modules/package-tree.html
index 32a1259..c874537 100644
--- a/docs/swervelib/parser/json/modules/package-tree.html
+++ b/docs/swervelib/parser/json/modules/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.parser.json.modules Class Hierarchy
-
+
diff --git a/docs/swervelib/parser/json/package-summary.html b/docs/swervelib/parser/json/package-summary.html
index e92e4c1..349f1e5 100644
--- a/docs/swervelib/parser/json/package-summary.html
+++ b/docs/swervelib/parser/json/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib.parser.json
-
+
diff --git a/docs/swervelib/parser/json/package-tree.html b/docs/swervelib/parser/json/package-tree.html
index b76d0ed..6e7f3fa 100644
--- a/docs/swervelib/parser/json/package-tree.html
+++ b/docs/swervelib/parser/json/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.parser.json Class Hierarchy
-
+
diff --git a/docs/swervelib/parser/package-summary.html b/docs/swervelib/parser/package-summary.html
index f2a13dd..1c72328 100644
--- a/docs/swervelib/parser/package-summary.html
+++ b/docs/swervelib/parser/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib.parser
-
+
diff --git a/docs/swervelib/parser/package-tree.html b/docs/swervelib/parser/package-tree.html
index be23ba4..a97781e 100644
--- a/docs/swervelib/parser/package-tree.html
+++ b/docs/swervelib/parser/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.parser Class Hierarchy
-
+
diff --git a/docs/swervelib/simulation/SwerveIMUSimulation.html b/docs/swervelib/simulation/SwerveIMUSimulation.html
index 49e2a58..8d90361 100644
--- a/docs/swervelib/simulation/SwerveIMUSimulation.html
+++ b/docs/swervelib/simulation/SwerveIMUSimulation.html
@@ -1,11 +1,11 @@
-
+
SwerveIMUSimulation
-
+
diff --git a/docs/swervelib/simulation/SwerveModuleSimulation.html b/docs/swervelib/simulation/SwerveModuleSimulation.html
index 3d51bd5..53959b6 100644
--- a/docs/swervelib/simulation/SwerveModuleSimulation.html
+++ b/docs/swervelib/simulation/SwerveModuleSimulation.html
@@ -1,11 +1,11 @@
-
+
SwerveModuleSimulation
-
+
diff --git a/docs/swervelib/simulation/package-summary.html b/docs/swervelib/simulation/package-summary.html
index 0c70929..7e3e5a0 100644
--- a/docs/swervelib/simulation/package-summary.html
+++ b/docs/swervelib/simulation/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib.simulation
-
+
diff --git a/docs/swervelib/simulation/package-tree.html b/docs/swervelib/simulation/package-tree.html
index 31d70f5..5fa3b35 100644
--- a/docs/swervelib/simulation/package-tree.html
+++ b/docs/swervelib/simulation/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.simulation Class Hierarchy
-
+
diff --git a/docs/swervelib/telemetry/Alert.AlertType.html b/docs/swervelib/telemetry/Alert.AlertType.html
index fb36cc3..67d5ddf 100644
--- a/docs/swervelib/telemetry/Alert.AlertType.html
+++ b/docs/swervelib/telemetry/Alert.AlertType.html
@@ -1,11 +1,11 @@
-
+
Alert.AlertType
-
+
diff --git a/docs/swervelib/telemetry/Alert.SendableAlerts.html b/docs/swervelib/telemetry/Alert.SendableAlerts.html
index 33eb173..032185c 100644
--- a/docs/swervelib/telemetry/Alert.SendableAlerts.html
+++ b/docs/swervelib/telemetry/Alert.SendableAlerts.html
@@ -1,11 +1,11 @@
-
+
Alert.SendableAlerts
-
+
diff --git a/docs/swervelib/telemetry/Alert.html b/docs/swervelib/telemetry/Alert.html
index 6c38620..f83d281 100644
--- a/docs/swervelib/telemetry/Alert.html
+++ b/docs/swervelib/telemetry/Alert.html
@@ -1,11 +1,11 @@
-
+
Alert
-
+
diff --git a/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html b/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html
index e47ec80..8e7f788 100644
--- a/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html
+++ b/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html
@@ -1,11 +1,11 @@
-
+
SwerveDriveTelemetry.TelemetryVerbosity
-
+
diff --git a/docs/swervelib/telemetry/SwerveDriveTelemetry.html b/docs/swervelib/telemetry/SwerveDriveTelemetry.html
index 34d3511..abb474d 100644
--- a/docs/swervelib/telemetry/SwerveDriveTelemetry.html
+++ b/docs/swervelib/telemetry/SwerveDriveTelemetry.html
@@ -1,11 +1,11 @@
-
+
SwerveDriveTelemetry
-
+
diff --git a/docs/swervelib/telemetry/package-summary.html b/docs/swervelib/telemetry/package-summary.html
index adc8e18..cc8b398 100644
--- a/docs/swervelib/telemetry/package-summary.html
+++ b/docs/swervelib/telemetry/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib.telemetry
-
+
diff --git a/docs/swervelib/telemetry/package-tree.html b/docs/swervelib/telemetry/package-tree.html
index 2e0c769..0d15c17 100644
--- a/docs/swervelib/telemetry/package-tree.html
+++ b/docs/swervelib/telemetry/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.telemetry Class Hierarchy
-
+
diff --git a/docs/type-search-index.js b/docs/type-search-index.js
index 2d5d5de..1a07770 100644
--- a/docs/type-search-index.js
+++ b/docs/type-search-index.js
@@ -1 +1 @@
-typeSearchIndex = [{"p":"swervelib.imu","l":"ADIS16448Swerve"},{"p":"swervelib.imu","l":"ADIS16470Swerve"},{"p":"swervelib.imu","l":"ADXRS450Swerve"},{"p":"swervelib.telemetry","l":"Alert"},{"p":"swervelib.telemetry","l":"Alert.AlertType"},{"l":"All Classes and Interfaces","u":"allclasses-index.html"},{"p":"swervelib.encoders","l":"AnalogAbsoluteEncoderSwerve"},{"p":"swervelib.imu","l":"AnalogGyroSwerve"},{"p":"swervelib.parser.json.modules","l":"AngleConversionFactorsJson"},{"p":"swervelib.parser.json.modules","l":"BoolMotorJson"},{"p":"swervelib.parser","l":"Cache"},{"p":"swervelib.encoders","l":"CanAndMagSwerve"},{"p":"swervelib.encoders","l":"CANCoderSwerve"},{"p":"swervelib.parser.json","l":"ControllerPropertiesJson"},{"p":"swervelib.parser.json.modules","l":"ConversionFactorsJson"},{"p":"swervelib.parser.json","l":"DeviceJson"},{"p":"swervelib.parser.json.modules","l":"DriveConversionFactorsJson"},{"p":"swervelib.parser.json.modules","l":"LocationJson"},{"p":"swervelib.math","l":"Matter"},{"p":"swervelib.parser.json","l":"ModuleJson"},{"p":"swervelib.parser.json","l":"MotorConfigDouble"},{"p":"swervelib.parser.json","l":"MotorConfigInt"},{"p":"swervelib.imu","l":"NavXSwerve"},{"p":"swervelib.parser.json","l":"PhysicalPropertiesJson"},{"p":"swervelib.parser","l":"PIDFConfig"},{"p":"swervelib.parser.json","l":"PIDFPropertiesJson"},{"p":"swervelib.parser.deserializer","l":"PIDFRange"},{"p":"swervelib.imu","l":"Pigeon2Swerve"},{"p":"swervelib.imu","l":"PigeonSwerve"},{"p":"swervelib.encoders","l":"PWMDutyCycleEncoderSwerve"},{"p":"swervelib.telemetry","l":"Alert.SendableAlerts"},{"p":"swervelib.motors","l":"SparkFlexSwerve"},{"p":"swervelib.motors","l":"SparkFlexSwerve.SparkMAX_slotIdx"},{"p":"swervelib.motors","l":"SparkMaxSwerve.SparkMAX_slotIdx"},{"p":"swervelib.encoders","l":"SparkMaxAnalogEncoderSwerve"},{"p":"swervelib.motors","l":"SparkMaxBrushedMotorSwerve"},{"p":"swervelib.encoders","l":"SparkMaxEncoderSwerve"},{"p":"swervelib.motors","l":"SparkMaxSwerve"},{"p":"swervelib.encoders","l":"SwerveAbsoluteEncoder"},{"p":"swervelib","l":"SwerveController"},{"p":"swervelib.parser","l":"SwerveControllerConfiguration"},{"p":"swervelib","l":"SwerveDrive"},{"p":"swervelib.parser","l":"SwerveDriveConfiguration"},{"p":"swervelib.parser.json","l":"SwerveDriveJson"},{"p":"swervelib.telemetry","l":"SwerveDriveTelemetry"},{"p":"swervelib","l":"SwerveDriveTest"},{"p":"swervelib.imu","l":"SwerveIMU"},{"p":"swervelib.simulation","l":"SwerveIMUSimulation"},{"p":"swervelib.math","l":"SwerveMath"},{"p":"swervelib","l":"SwerveModule"},{"p":"swervelib.parser","l":"SwerveModuleConfiguration"},{"p":"swervelib.parser","l":"SwerveModulePhysicalCharacteristics"},{"p":"swervelib.simulation","l":"SwerveModuleSimulation"},{"p":"swervelib.motors","l":"SwerveMotor"},{"p":"swervelib.parser","l":"SwerveParser"},{"p":"swervelib.motors","l":"TalonFXSwerve"},{"p":"swervelib.motors","l":"TalonSRXSwerve"},{"p":"swervelib.telemetry","l":"SwerveDriveTelemetry.TelemetryVerbosity"}];updateSearchResults();
\ No newline at end of file
+typeSearchIndex = [{"p":"swervelib.imu","l":"ADIS16448Swerve"},{"p":"swervelib.imu","l":"ADIS16470Swerve"},{"p":"swervelib.imu","l":"ADXRS450Swerve"},{"p":"swervelib.telemetry","l":"Alert"},{"p":"swervelib.telemetry","l":"Alert.AlertType"},{"l":"All Classes and Interfaces","u":"allclasses-index.html"},{"p":"swervelib.encoders","l":"AnalogAbsoluteEncoderSwerve"},{"p":"swervelib.imu","l":"AnalogGyroSwerve"},{"p":"swervelib.parser.json.modules","l":"AngleConversionFactorsJson"},{"p":"swervelib.parser.json.modules","l":"BoolMotorJson"},{"p":"swervelib.parser","l":"Cache"},{"p":"swervelib.encoders","l":"CanAndMagSwerve"},{"p":"swervelib.encoders","l":"CANCoderSwerve"},{"p":"swervelib.parser.json","l":"ControllerPropertiesJson"},{"p":"swervelib.parser.json.modules","l":"ConversionFactorsJson"},{"p":"swervelib.parser.json","l":"DeviceJson"},{"p":"swervelib.parser.json.modules","l":"DriveConversionFactorsJson"},{"p":"swervelib.math","l":"IMULinearMovingAverageFilter"},{"p":"swervelib.imu","l":"IMUVelocity"},{"p":"swervelib.parser.json.modules","l":"LocationJson"},{"p":"swervelib.math","l":"Matter"},{"p":"swervelib.parser.json","l":"ModuleJson"},{"p":"swervelib.parser.json","l":"MotorConfigDouble"},{"p":"swervelib.parser.json","l":"MotorConfigInt"},{"p":"swervelib.imu","l":"NavXSwerve"},{"p":"swervelib.parser.json","l":"PhysicalPropertiesJson"},{"p":"swervelib.parser","l":"PIDFConfig"},{"p":"swervelib.parser.json","l":"PIDFPropertiesJson"},{"p":"swervelib.parser.deserializer","l":"PIDFRange"},{"p":"swervelib.imu","l":"Pigeon2Swerve"},{"p":"swervelib.imu","l":"PigeonSwerve"},{"p":"swervelib.encoders","l":"PWMDutyCycleEncoderSwerve"},{"p":"swervelib.telemetry","l":"Alert.SendableAlerts"},{"p":"swervelib.motors","l":"SparkFlexSwerve"},{"p":"swervelib.motors","l":"SparkFlexSwerve.SparkMAX_slotIdx"},{"p":"swervelib.motors","l":"SparkMaxSwerve.SparkMAX_slotIdx"},{"p":"swervelib.encoders","l":"SparkMaxAnalogEncoderSwerve"},{"p":"swervelib.motors","l":"SparkMaxBrushedMotorSwerve"},{"p":"swervelib.encoders","l":"SparkMaxEncoderSwerve"},{"p":"swervelib.motors","l":"SparkMaxSwerve"},{"p":"swervelib.encoders","l":"SwerveAbsoluteEncoder"},{"p":"swervelib","l":"SwerveController"},{"p":"swervelib.parser","l":"SwerveControllerConfiguration"},{"p":"swervelib","l":"SwerveDrive"},{"p":"swervelib.parser","l":"SwerveDriveConfiguration"},{"p":"swervelib.parser.json","l":"SwerveDriveJson"},{"p":"swervelib.telemetry","l":"SwerveDriveTelemetry"},{"p":"swervelib","l":"SwerveDriveTest"},{"p":"swervelib.imu","l":"SwerveIMU"},{"p":"swervelib.simulation","l":"SwerveIMUSimulation"},{"p":"swervelib.math","l":"SwerveMath"},{"p":"swervelib","l":"SwerveModule"},{"p":"swervelib.parser","l":"SwerveModuleConfiguration"},{"p":"swervelib.parser","l":"SwerveModulePhysicalCharacteristics"},{"p":"swervelib.simulation","l":"SwerveModuleSimulation"},{"p":"swervelib.motors","l":"SwerveMotor"},{"p":"swervelib.parser","l":"SwerveParser"},{"p":"swervelib.motors","l":"TalonFXSwerve"},{"p":"swervelib.motors","l":"TalonSRXSwerve"},{"p":"swervelib.telemetry","l":"SwerveDriveTelemetry.TelemetryVerbosity"}];updateSearchResults();
\ No newline at end of file
diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java
index c6460e9..415f0bb 100644
--- a/swervelib/SwerveDrive.java
+++ b/swervelib/SwerveDrive.java
@@ -30,6 +30,7 @@
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;
import swervelib.encoders.CANCoderSwerve;
+import swervelib.imu.IMUVelocity;
import swervelib.imu.Pigeon2Swerve;
import swervelib.imu.SwerveIMU;
import swervelib.math.SwerveMath;
@@ -97,6 +98,24 @@ public class SwerveDrive
* correction.
*/
public boolean chassisVelocityCorrection = true;
+ /**
+ * Correct chassis velocity in {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} (auto) using 254's
+ * correction during auto.
+ */
+ public boolean autonomousChassisVelocityCorrection = false;
+ /**
+ * Correct for skew that scales with angular velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}
+ */
+ public boolean angularVelocityCorrection = false;
+ /**
+ * Correct for skew that scales with angular velocity in {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)}
+ * during auto.
+ */
+ public boolean autonomousAngularVelocityCorrection = false;
+ /**
+ * Angular Velocity Correction Coefficent (expected values between -0.15 and 0.15).
+ */
+ public double angularVelocityCoefficient = 0;
/**
* Whether to correct heading when driving translationally. Set to true to enable.
*/
@@ -113,6 +132,11 @@ public class SwerveDrive
* Swerve IMU device for sensing the heading of the robot.
*/
private SwerveIMU imu;
+ /**
+ * Class that calculates robot's yaw velocity using IMU measurements. Used for angularVelocityCorrection in
+ * {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}.
+ */
+ private IMUVelocity imuVelocity;
/**
* Simulation of the swerve drive.
*/
@@ -479,12 +503,7 @@ public void drive(
public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d centerOfRotationMeters)
{
- // Thank you to Jared Russell FRC254 for Open Loop Compensation Code
- // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5
- if (chassisVelocityCorrection)
- {
- velocity = ChassisSpeeds.discretize(velocity, discretizationdtSeconds);
- }
+ velocity = movementOptimizations(velocity, chassisVelocityCorrection, angularVelocityCorrection);
// Heading Angular Velocity Deadband, might make a configuration option later.
// Originally made by Team 1466 Webb Robotics.
@@ -518,7 +537,7 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent
// Calculate required module states via kinematics
SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(velocity, centerOfRotationMeters);
- setRawModuleStates(swerveModuleStates, isOpenLoop);
+ setRawModuleStates(swerveModuleStates, velocity, isOpenLoop);
}
/**
@@ -568,14 +587,15 @@ public double getMaximumAngularVelocity()
* Set the module states (azimuth and velocity) directly.
*
* @param desiredStates A list of SwerveModuleStates to send to the modules.
+ * @param desiredChassisSpeed The desired chassis speeds to set the robot to achieve.
* @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop.
*/
- private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop)
+ private void setRawModuleStates(SwerveModuleState[] desiredStates, ChassisSpeeds desiredChassisSpeed, boolean isOpenLoop)
{
// Desaturates wheel speeds
if (attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0)
{
- SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, getRobotVelocity(),
+ SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, desiredChassisSpeed,
maxSpeedMPS,
attainableMaxTranslationalSpeedMetersPerSecond,
attainableMaxRotationalVelocityRadiansPerSecond);
@@ -593,14 +613,23 @@ private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpe
/**
* Set the module states (azimuth and velocity) directly. Used primarily for auto paths.
+ * Does not allow for usage of desaturateWheelSpeeds(SwerveModuleState[] moduleStates,
+ * ChassisSpeeds desiredChassisSpeed, double attainableMaxModuleSpeedMetersPerSecond,
+ * double attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond)
*
* @param desiredStates A list of SwerveModuleStates to send to the modules.
* @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop.
*/
public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop)
{
- setRawModuleStates(kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates)),
- isOpenLoop);
+ desiredStates = kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates));
+ SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, maxSpeedMPS);
+
+ // Sets states
+ for (SwerveModule module : swerveModules)
+ {
+ module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop, false);
+ }
}
/**
@@ -610,11 +639,14 @@ public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoo
*/
public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
{
+
+ chassisSpeeds = movementOptimizations(chassisSpeeds, autonomousChassisVelocityCorrection, autonomousAngularVelocityCorrection);
+
SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond);
- setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), false);
+ setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), chassisSpeeds, false);
}
/**
@@ -836,9 +868,23 @@ public void setMotorIdleMode(boolean brake)
}
}
+ /**
+ * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a few seconds.
+ * @param enabled Enable state
+ * @param deadband Deadband in degrees, default is 3 degrees.
+ */
+ public void setModuleEncoderAutoSynchronize(boolean enabled, double deadband)
+ {
+ for(SwerveModule swerveModule : swerveModules)
+ {
+ swerveModule.setEncoderAutoSynchronize(enabled, deadband);
+ }
+ }
+
+
/**
* Set the maximum speed of the drive motors, modified {@link SwerveDrive#maxSpeedMPS} which is used for the
- * {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], boolean)} function and
+ * {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], ChassisSpeeds, boolean)} function and
* {@link 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.
*
@@ -866,7 +912,7 @@ public void setMaximumSpeed(double maximumSpeed, boolean updateModuleFeedforward
/**
* Set the maximum speed of the drive motors, modified {@link SwerveDrive#maxSpeedMPS} which is used for the
- * {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], boolean)} function and
+ * {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], ChassisSpeeds, boolean)} function and
* {@link 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
* {@link SwerveModule#setFeedforward(SimpleMotorFeedforward)}.
@@ -1205,7 +1251,7 @@ public void setCosineCompensator(boolean enabled)
}
/**
- * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction
+ * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop
*
* @param enable Enable chassis velocity correction, which will use
* {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following.
@@ -1213,8 +1259,101 @@ public void setCosineCompensator(boolean enabled)
*/
public void setChassisDiscretization(boolean enable, double dtSeconds)
{
- chassisVelocityCorrection = enable;
- discretizationdtSeconds = dtSeconds;
+ if(!SwerveDriveTelemetry.isSimulation)
+ {
+ chassisVelocityCorrection = enable;
+ discretizationdtSeconds = dtSeconds;
+ }
+ }
+
+ /**
+ * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop and/or auto
+ *
+ * @param useInTeleop Enable chassis velocity correction, which will use
+ * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in teleop.
+ * @param useInAuto Enable chassis velocity correction, which will use
+ * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in auto.
+ * @param dtSeconds The duration of the timestep the speeds should be applied for.
+ */
+ public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, double dtSeconds)
+ {
+ if(!SwerveDriveTelemetry.isSimulation)
+ {
+ chassisVelocityCorrection = useInTeleop;
+ autonomousChassisVelocityCorrection = useInAuto;
+ discretizationdtSeconds = dtSeconds;
+ }
+ }
+
+ /**
+ * Enables angular velocity skew correction in teleop and/or autonomous
+ * and sets the angular velocity coefficient for both modes
+ *
+ * @param useInTeleop Enables angular velocity correction in teleop.
+ * @param useInAuto Enables angular velocity correction in autonomous.
+ * @param angularVelocityCoeff The angular velocity coefficient. Expected values between -0.15 to 0.15.
+ * Start with a value of 0.1, test in teleop.
+ * When enabling for the first time if the skew is significantly worse try inverting the value.
+ * Tune by moving in a straight line while rotating. Testing is best done with angular velocity controls on the right stick.
+ * Change the value until you are visually happy with the skew.
+ * Ensure your tune works with different translational and rotational magnitudes.
+ * If this reduces skew in teleop, it may improve auto.
+ */
+ public void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAuto, double angularVelocityCoeff)
+ {
+ if(!SwerveDriveTelemetry.isSimulation)
+ {
+ imuVelocity = IMUVelocity.createIMUVelocity(imu);
+ angularVelocityCorrection = useInTeleop;
+ autonomousAngularVelocityCorrection = useInAuto;
+ angularVelocityCoefficient = angularVelocityCoeff;
+ }
+ }
+
+ /**
+ * Correct for skew that worsens as angular velocity increases
+ *
+ * @param velocity The chassis speeds to set the robot to achieve.
+ * @return {@link ChassisSpeeds} of the robot after angular velocity skew correction.
+ */
+ public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds velocity)
+ {
+ var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * angularVelocityCoefficient);
+ if(angularVelocity.getRadians() != 0.0){
+ velocity = ChassisSpeeds.fromRobotRelativeSpeeds(
+ velocity.vxMetersPerSecond,
+ velocity.vyMetersPerSecond,
+ velocity.omegaRadiansPerSecond,
+ getOdometryHeading());
+ velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading().plus(angularVelocity));
+ }
+ return velocity;
+ }
+
+ /**
+ * Enable desired drive corrections
+ *
+ * @param velocity The chassis speeds to set the robot to achieve.
+ * @param uesChassisDiscretize Correct chassis velocity using 254's correction.
+ * @param useAngularVelocitySkewCorrection Use the robot's angular velocity to correct for skew.
+ * @return The chassis speeds after optimizations.
+ */
+ private ChassisSpeeds movementOptimizations(ChassisSpeeds velocity, boolean uesChassisDiscretize, boolean useAngularVelocitySkewCorrection)
+ {
+
+ if(useAngularVelocitySkewCorrection)
+ {
+ velocity = angularVelocitySkewCorrection(velocity);
+ }
+
+ // Thank you to Jared Russell FRC254 for Open Loop Compensation Code
+ // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5
+ if (uesChassisDiscretize)
+ {
+ velocity = ChassisSpeeds.discretize(velocity, discretizationdtSeconds);
+ }
+
+ return velocity;
}
}
diff --git a/swervelib/SwerveDriveTest.java b/swervelib/SwerveDriveTest.java
index 848852c..a3d71df 100644
--- a/swervelib/SwerveDriveTest.java
+++ b/swervelib/SwerveDriveTest.java
@@ -356,8 +356,8 @@ public static void logAngularMotorVoltage(SwerveModule module, SysIdRoutineLog l
public static void logAngularMotorActivity(SwerveModule module, SysIdRoutineLog log, Supplier
powerSupplied)
{
double power = powerSupplied.get();
- double angle = module.getAbsolutePosition();
- double velocity = module.getAbsoluteEncoder().getVelocity();
+ double angle = module.getAngleMotor().getPosition();
+ double velocity = module.getAngleMotor().getVelocity();
SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Angle Power", power);
SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Angle Position", angle);
SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Absolute Encoder Velocity",
diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java
index efc3aef..2cdbf44 100644
--- a/swervelib/SwerveModule.java
+++ b/swervelib/SwerveModule.java
@@ -112,6 +112,14 @@ public class SwerveModule
* Encoder synchronization queued.
*/
private boolean synchronizeEncoderQueued = false;
+ /**
+ * Encoder, Absolute encoder synchronization enabled.
+ */
+ private boolean synchronizeEncoderEnabled = false;
+ /**
+ * Encoder synchronization deadband in degrees.
+ */
+ private double synchronizeEncoderDeadband = 3;
/**
@@ -242,12 +250,32 @@ public void setDriveMotorVoltageCompensation(double optimalVoltage)
*/
public void queueSynchronizeEncoders()
{
- if (absoluteEncoder != null)
+ if (absoluteEncoder != null && synchronizeEncoderEnabled)
{
synchronizeEncoderQueued = true;
}
}
+ /**
+ * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a few seconds.
+ * @param enabled Enable state
+ * @param deadband Deadband in degrees, default is 3 degrees.
+ */
+ public void setEncoderAutoSynchronize(boolean enabled, double deadband)
+ {
+ synchronizeEncoderEnabled = enabled;
+ synchronizeEncoderDeadband = deadband;
+ }
+
+ /**
+ * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a few seconds.
+ * @param enabled Enable state
+ */
+ public void setEncoderAutoSynchronize(boolean enabled)
+ {
+ synchronizeEncoderEnabled = enabled;
+ }
+
/**
* Set the antiJitter functionality, if true the modules will NOT auto center. Pushes the offsets to the angle motor
* controllers as well.
@@ -355,10 +383,12 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
// Prevent module rotation if angle is the same as the previous angle.
// Synchronize encoders if queued and send in the current position as the value from the absolute encoder.
- if (absoluteEncoder != null && synchronizeEncoderQueued)
+ if (absoluteEncoder != null && synchronizeEncoderQueued && synchronizeEncoderEnabled)
{
double absoluteEncoderPosition = getAbsolutePosition();
- angleMotor.setPosition(absoluteEncoderPosition);
+ if(Math.abs(angleMotor.getPosition() - absoluteEncoderPosition) >= synchronizeEncoderDeadband) {
+ angleMotor.setPosition(absoluteEncoderPosition);
+ }
angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition);
synchronizeEncoderQueued = false;
} else
diff --git a/swervelib/imu/IMUVelocity.java b/swervelib/imu/IMUVelocity.java
new file mode 100644
index 0000000..fba47a3
--- /dev/null
+++ b/swervelib/imu/IMUVelocity.java
@@ -0,0 +1,141 @@
+package swervelib.imu;
+
+import edu.wpi.first.hal.HALUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.Notifier;
+import swervelib.math.IMULinearMovingAverageFilter;
+
+/**
+ * Generic IMU Velocity filter.
+ */
+public class IMUVelocity {
+ /**
+ * Swerve IMU.
+ */
+ private final SwerveIMU gyro;
+ /**
+ * Linear filter used to calculate velocity, we use a custom filter class
+ * to prevent unwanted operations.
+ */
+ private final IMULinearMovingAverageFilter velocityFilter;
+ /**
+ * WPILib {@link Notifier} to keep IMU velocity up to date.
+ */
+ private final Notifier notifier;
+
+ /**
+ * Prevents calculation when no previous measurement exists.
+ */
+ private boolean firstCycle = true;
+ /**
+ * Tracks the previous loop's recorded time.
+ */
+ private double timestamp = 0.0;
+ /**
+ * Tracks the previous loop's position as a Rotation2d.
+ */
+ private Rotation2d position = new Rotation2d();
+ /**
+ * The calculated velocity of the robot based on averaged IMU measurements.
+ */
+ private double velocity = 0.0;
+
+ /**
+ * Constructor for the IMU Velocity.
+ *
+ * @param gyro The SwerveIMU gyro.
+ * @param periodSeconds The rate to collect measurements from the gyro, in the form (1/number of samples per second),
+ * make sure this does not exceed the update rate of your IMU.
+ * @param averagingTaps The number of samples to used for the moving average linear filter. Higher values will not
+ * allow the system to update to changes in velocity, lower values may create a less smooth signal. Expected taps
+ * will probably be ~2-8, with the goal of having the lowest smooth value.
+ *
+ */
+ public IMUVelocity(SwerveIMU gyro, double periodSeconds, int averagingTaps)
+ {
+ this.gyro = gyro;
+ velocityFilter = new IMULinearMovingAverageFilter(averagingTaps);
+ notifier = new Notifier(this::update);
+ notifier.startPeriodic(periodSeconds);
+ timestamp = HALUtil.getFPGATime();
+ }
+
+ /**
+ * Static factory for IMU Velocity. Supported IMU rates will be as quick as possible
+ * but will not exceed 100hz and will use 5 taps (supported IMUs are listed in swervelib's IMU folder).
+ * Other gyroscopes will default to 50hz and 5 taps. For custom rates please use the IMUVelocity constructor.
+ *
+ * @param gyro The SwerveIMU gyro.
+ * @return {@link IMUVelocity} for the given gyro with adjusted period readings for velocity.
+ */
+ public static IMUVelocity createIMUVelocity(SwerveIMU gyro)
+ {
+ double desiredNotifierPeriod = 1.0/50.0;
+ // ADIS16448_IMU ~200HZ:
+ // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java#L277
+ if (gyro instanceof ADIS16448Swerve)
+ {
+ desiredNotifierPeriod = 1.0/100.0;
+ }
+ // ADIS16470_IMU 200HZ
+ // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java#L345
+ else if (gyro instanceof ADIS16470Swerve)
+ {
+ desiredNotifierPeriod = 1.0/100.0;
+ }
+ // ADXRS450_Gyro 2000HZ?
+ // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java#L31
+ else if (gyro instanceof ADXRS450Swerve)
+ {
+ desiredNotifierPeriod = 1.0/100.0;
+ }
+ // NAX (AHRS): 60HZ
+ // https://github.com/kauailabs/navxmxp/blob/5e010ba810bb7f7eaab597e0b708e34f159984db/roborio/java/navx_frc/src/com/kauailabs/navx/frc/AHRS.java#L119C25-L119C61
+ else if (gyro instanceof NavXSwerve)
+ {
+ desiredNotifierPeriod = 1.0/60.0;
+ }
+ // Pigeon2 100HZ
+ // https://store.ctr-electronics.com/content/user-manual/Pigeon2%20User's%20Guide.pdf
+ else if (gyro instanceof Pigeon2Swerve)
+ {
+ desiredNotifierPeriod = 1.0/100.0;
+ }
+ // Pigeon 100HZ
+ // https://store.ctr-electronics.com/content/user-manual/Pigeon%20IMU%20User's%20Guide.pdf
+ else if (gyro instanceof PigeonSwerve)
+ {
+ desiredNotifierPeriod = 1.0/100.0;
+ }
+ return new IMUVelocity(gyro, desiredNotifierPeriod, 5);
+ }
+
+ /**
+ * Update the robot's rotational velocity based on the current gyro position.
+ */
+ private void update()
+ {
+ double newTimestamp = HALUtil.getFPGATime();
+ Rotation2d newPosition = Rotation2d.fromRadians(gyro.getRotation3d().getZ());
+
+ synchronized (this) {
+ if (!firstCycle) {
+ velocityFilter.addValue((newPosition.minus(position).getRadians()) / (newTimestamp - timestamp));
+ }
+ firstCycle = false;
+ timestamp = newTimestamp;
+ position = newPosition;
+ }
+ }
+
+ /**
+ * Get the robot's angular velocity based on averaged meaasurements from the IMU.
+ * Velocity is multiplied by 1e+6 (1,000,000) because the timestamps are in microseconds.
+ *
+ * @return robot's angular velocity in rads/s as a double.
+ */
+ public synchronized double getVelocity() {
+ velocity = velocityFilter.calculate();
+ return velocity * 1e+6;
+ }
+}
diff --git a/swervelib/imu/Pigeon2Swerve.java b/swervelib/imu/Pigeon2Swerve.java
index f29970e..22160d8 100644
--- a/swervelib/imu/Pigeon2Swerve.java
+++ b/swervelib/imu/Pigeon2Swerve.java
@@ -26,11 +26,15 @@ public class Pigeon2Swerve extends SwerveIMU
/**
* Offset for the Pigeon 2.
*/
- private Rotation3d offset = new Rotation3d();
+ private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
- private boolean invertedIMU = false;
+ private boolean invertedIMU = false;
+ /**
+ * Pigeon2 configurator.
+ */
+ private Pigeon2Configurator cfg;
/**
* Generate the SwerveIMU for pigeon.
@@ -41,6 +45,7 @@ public class Pigeon2Swerve extends SwerveIMU
public Pigeon2Swerve(int canid, String canbus)
{
imu = new Pigeon2(canid, canbus);
+ this.cfg = imu.getConfigurator();
SmartDashboard.putData(imu);
}
@@ -60,7 +65,6 @@ public Pigeon2Swerve(int canid)
@Override
public void factoryDefault()
{
- Pigeon2Configurator cfg = imu.getConfigurator();
Pigeon2Configuration config = new Pigeon2Configuration();
// Compass utilization causes readings to jump dramatically in some cases.
diff --git a/swervelib/math/IMULinearMovingAverageFilter.java b/swervelib/math/IMULinearMovingAverageFilter.java
new file mode 100644
index 0000000..f2a008a
--- /dev/null
+++ b/swervelib/math/IMULinearMovingAverageFilter.java
@@ -0,0 +1,54 @@
+package swervelib.math;
+
+import edu.wpi.first.util.DoubleCircularBuffer;
+
+/**
+ * A linear filter that does not calculate() each time a value is added to
+ * the DoubleCircularBuffer.
+ */
+public class IMULinearMovingAverageFilter {
+
+ /**
+ * Circular buffer storing the current IMU readings
+ */
+ private final DoubleCircularBuffer m_inputs;
+ /**
+ * Gain on each reading.
+ */
+ private final double m_inputGain;
+
+ /**
+ * Construct a linear moving average fitler
+ * @param bufferLength The number of values to average across
+ */
+ public IMULinearMovingAverageFilter(int bufferLength)
+ {
+ m_inputs = new DoubleCircularBuffer(bufferLength);
+ m_inputGain = 1.0 / bufferLength;
+ }
+
+ /**
+ * Add a value to the DoubleCircularBuffer
+ * @param input Value to add
+ */
+ public void addValue(double input)
+ {
+ m_inputs.addFirst(input);
+ }
+
+ /**
+ * Calculate the average of the samples in the buffer
+ * @return The average of the values in the buffer
+ */
+ public double calculate()
+ {
+ double returnVal = 0.0;
+
+ for(int i = 0; i < m_inputs.size(); i++)
+ {
+ returnVal += m_inputs.get(i) * m_inputGain;
+ }
+
+ return returnVal;
+ }
+}
diff --git a/swervelib/math/SwerveMath.java b/swervelib/math/SwerveMath.java
index ca278a9..b77838c 100644
--- a/swervelib/math/SwerveMath.java
+++ b/swervelib/math/SwerveMath.java
@@ -136,7 +136,7 @@ public static double calculateDegreesPerSteeringRotation(
public static double calculateMaxAngularVelocity(
double maxSpeed, double furthestModuleX, double furthestModuleY)
{
- return maxSpeed / new Rotation2d(furthestModuleX, furthestModuleY).getRadians();
+ return maxSpeed / Math.hypot(furthestModuleX, furthestModuleY);
}
/**
diff --git a/swervelib/motors/SparkMaxSwerve.java b/swervelib/motors/SparkMaxSwerve.java
index a2c421e..0f01417 100644
--- a/swervelib/motors/SparkMaxSwerve.java
+++ b/swervelib/motors/SparkMaxSwerve.java
@@ -224,12 +224,38 @@ public void configureIntegratedEncoder(double positionConversionFactor)
{
configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor));
configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60));
+ // Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
+ // Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement)
+ // Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
+ // https://www.chiefdelphi.com/t/shooter-encoder/400211/11
+ // This value was taken from:
+ // https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
+ // and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
+ configureSparkMax(() -> encoder.setMeasurementPeriod(10));
+ configureSparkMax(() -> encoder.setAverageDepth(2));
// Taken from
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
+ // Unused frames can be set to 65535 to decrease CAN ultilization.
configureCANStatusFrames(10, 20, 20, 500, 500, 200, 200);
} else
{
+ // By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5
+ // This needs to be set to 20ms or under to properly update the swerve module position for odometry
+ // Configuration taken from 3005, the team who helped develop the Max Swerve:
+ // https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244
+ // Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max.
+ // From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill,
+ // with limited testing 19ms did not return the same value while the module was constatntly rotating.
+ if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
+ {
+ configureCANStatusFrames(100, 20, 20, 200, 20, 8, 50);
+ }
+ // Need to test on analog encoders but the same concept should apply for them, worst thing that could happen is slightly more can use
+ else if(absoluteEncoder.getAbsoluteEncoder() instanceof SparkAnalogSensor)
+ {
+ configureCANStatusFrames(100, 20, 20, 19, 200, 200, 200);
+ }
configureSparkMax(() -> {
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
diff --git a/swervelib/motors/TalonFXSwerve.java b/swervelib/motors/TalonFXSwerve.java
index 213f199..d2ddb4b 100644
--- a/swervelib/motors/TalonFXSwerve.java
+++ b/swervelib/motors/TalonFXSwerve.java
@@ -49,6 +49,10 @@ public class TalonFXSwerve extends SwerveMotor
* Current TalonFX configuration.
*/
private TalonFXConfiguration configuration = new TalonFXConfiguration();
+ /**
+ * Current TalonFX Configurator.
+ */
+ private TalonFXConfigurator cfg;
/**
@@ -61,6 +65,7 @@ public TalonFXSwerve(TalonFX motor, boolean isDriveMotor)
{
this.isDriveMotor = isDriveMotor;
this.motor = motor;
+ this.cfg = motor.getConfigurator();
factoryDefaults();
clearStickyFaults();
@@ -102,7 +107,6 @@ public void factoryDefaults()
{
if (!factoryDefaultOccurred)
{
- TalonFXConfigurator cfg = motor.getConfigurator();
configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake;
configuration.ClosedLoopGeneral.ContinuousWrap = true;
cfg.apply(configuration);
@@ -156,7 +160,6 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder)
@Override
public void configureIntegratedEncoder(double positionConversionFactor)
{
- TalonFXConfigurator cfg = motor.getConfigurator();
cfg.refresh(configuration);
positionConversionFactor = 1 / positionConversionFactor;
@@ -246,7 +249,6 @@ public void configureCANStatusFrames(
public void configurePIDF(PIDFConfig config)
{
- TalonFXConfigurator cfg = motor.getConfigurator();
cfg.refresh(configuration.Slot0);
cfg.apply(
configuration.Slot0.withKP(config.p).withKI(config.i).withKD(config.d).withKS(config.f));
@@ -263,7 +265,6 @@ public void configurePIDF(PIDFConfig config)
@Override
public void configurePIDWrapping(double minInput, double maxInput)
{
- TalonFXConfigurator cfg = motor.getConfigurator();
cfg.refresh(configuration.ClosedLoopGeneral);
configuration.ClosedLoopGeneral.ContinuousWrap = true;
cfg.apply(configuration.ClosedLoopGeneral);
@@ -414,7 +415,6 @@ public void setPosition(double position)
if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation)
{
position = position < 0 ? (position % 360) + 360 : position;
- TalonFXConfigurator cfg = motor.getConfigurator();
cfg.setPosition(position / 360);
}
}
@@ -439,7 +439,6 @@ public void setVoltageCompensation(double nominalVoltage)
@Override
public void setCurrentLimit(int currentLimit)
{
- TalonFXConfigurator cfg = motor.getConfigurator();
cfg.refresh(configuration.CurrentLimits);
cfg.apply(
configuration.CurrentLimits.withStatorCurrentLimit(currentLimit)
@@ -454,7 +453,6 @@ public void setCurrentLimit(int currentLimit)
@Override
public void setLoopRampRate(double rampRate)
{
- TalonFXConfigurator cfg = motor.getConfigurator();
cfg.refresh(configuration.ClosedLoopRamps);
cfg.apply(configuration.ClosedLoopRamps.withVoltageClosedLoopRampPeriod(rampRate));
}
diff --git a/swervelib/parser/json/DeviceJson.java b/swervelib/parser/json/DeviceJson.java
index 48551e5..bcc269d 100644
--- a/swervelib/parser/json/DeviceJson.java
+++ b/swervelib/parser/json/DeviceJson.java
@@ -74,6 +74,8 @@ public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor)
return new SparkMaxEncoderSwerve(motor, 360);
case "sparkmax_analog":
return new SparkMaxAnalogEncoderSwerve(motor, 3.3);
+ case "sparkmax_analog5v":
+ return new SparkMaxAnalogEncoderSwerve(motor, 5);
case "canandcoder_can":
case "canandmag_can":
return new CanAndMagSwerve(id);
diff --git a/swervelib/parser/json/ModuleJson.java b/swervelib/parser/json/ModuleJson.java
index 8e4604c..a2e5a6d 100644
--- a/swervelib/parser/json/ModuleJson.java
+++ b/swervelib/parser/json/ModuleJson.java
@@ -142,7 +142,7 @@ public SwerveModuleConfiguration createModuleConfiguration(
// Backwards compatibility, auto-optimization.
if (conversionFactor.angle == 360 && absEncoder != null &&
- absEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder && angleMotor.getMotor() instanceof CANSparkMax)
+ absEncoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor && angleMotor.getMotor() instanceof CANSparkMax)
{
angleMotor.setAbsoluteEncoder(absEncoder);
}