SwerveDrive JSON parsed class.SwerveDriveOdometry except uses gyro pitch and roll to achieve a more accurate estimation.SwerveDrive following frc-web-components.SwerveDrive IMU.SwerveDrive IMU.SwerveModuleSwerveDrivePoseEstimator which takes into account gyroscope pitch and roll to achieve a more
- accurate estimation, originally made by Team 1466.SwerveDrivePoseEstimator and update the SwerveIMU gyro reading with
@@ -127,10 +123,6 @@ loadScripts(document, 'script');
SwerveDrivePoseEstimator and update the SwerveIMU gyro reading with
the given timestamp of the vision measurement.ADIS16448_IMU device.SwerveModuleState2 optimizations so the angle is reversed and speed is reversed to ensure the module
+SwerveModuleState optimizations so the angle is reversed and speed is reversed to ensure the module
never turns more than 90deg.Notifier to keep odometry up to date.SwerveControllerConfiguration.maxSpeed and
SwerveDriveConfiguration.maxSpeed which is used for the
- SwerveDrive.setRawModuleStates(SwerveModuleState2[], boolean) function and
+ SwerveDrive.setRawModuleStates(SwerveModuleState[], boolean) function and
SwerveController.getTargetSpeeds(double, double, double, double, double) functions.SwerveControllerConfiguration.maxSpeed and
SwerveDriveConfiguration.maxSpeed which is used for the
- SwerveDrive.setRawModuleStates(SwerveModuleState2[], boolean) function and
+ SwerveDrive.setRawModuleStates(SwerveModuleState[], boolean) function and
SwerveController.getTargetSpeeds(double, double, double, double, double) functions.SwerveModuleState2 will be optimized to prevent spinning more than 90deg.SwerveModuleState will be optimized to prevent spinning more than 90deg.SwerveDriveOdometry except uses gyro pitch and roll to achieve a more accurate estimation.SwerveModuleState2 based on the SwerveModuleState with the radians per second defined.SwerveDrivePoseEstimator which takes into account gyroscope pitch and roll to achieve a more
- accurate estimation, originally made by Team 1466.SwerveModuleState.SwerveMotor for the SwerveModule.SwerveModuleState2 of the simulated module.SwerveModuleState of the simulated module.final SwerveKinematics2final edu.wpi.first.math.kinematics.SwerveDriveKinematicsprivate SwerveIMUSimulationprivate final edu.wpi.first.wpilibj.NotifierNotifier to keep odometry up to date.private SwerveIMUSimulationedu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> final SwerveDriveConfigurationfinal SwerveDriveConfigurationfinal SwervePoseEstimator2final edu.wpi.first.math.estimator.SwerveDrivePoseEstimatorprivate final SwerveModule[]private final SwerveModule[]edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> edu.wpi.first.math.kinematics.SwerveModuleState[]SwerveControllerConfiguration.maxSpeed and
SwerveDriveConfiguration.maxSpeed which is used for the
- setRawModuleStates(SwerveModuleState2[], boolean) function and
+ setRawModuleStates(SwerveModuleState[], boolean) function and
SwerveController.getTargetSpeeds(double, double, double, double, double) functions.voidSwerveControllerConfiguration.maxSpeed and
SwerveDriveConfiguration.maxSpeed which is used for the
- setRawModuleStates(SwerveModuleState2[], boolean) function and
+ setRawModuleStates(SwerveModuleState[], boolean) function and
SwerveController.getTargetSpeeds(double, double, double, double, double) functions.voidvoidsetModuleStateOptimization(boolean optimizationEnabled) SwerveModuleState2 will be optimized to prevent spinning more than 90deg.SwerveModuleState will be optimized to prevent spinning more than 90deg.voidsetModuleStates(SwerveModuleState2[] desiredStates,
+setModuleStates(edu.wpi.first.math.kinematics.SwerveModuleState[] desiredStates,
boolean isOpenLoop)
Set the module states (azimuth and velocity) directly.
@@ -397,12 +402,22 @@ loadScripts(document, 'script');
Sets the drive motors to brake/coast mode.
-private void
-setRawModuleStates(SwerveModuleState2[] desiredStates,
- boolean isOpenLoop)
+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
+
+
+Stop the odometry thread in favor of manually updating odometry.
+
void
@@ -438,7 +453,7 @@ loadScripts(document, 'script');
-
kinematics
-
+public final edu.wpi.first.math.kinematics.SwerveDriveKinematics kinematics
Swerve Kinematics object utilizing second order kinematics.
@@ -452,7 +467,7 @@ loadScripts(document, 'script');
-
swerveDrivePoseEstimator
-
+public final edu.wpi.first.math.estimator.SwerveDrivePoseEstimator swerveDrivePoseEstimator
Swerve odometry.
@@ -536,6 +551,13 @@ loadScripts(document, 'script');
The last heading set in radians.
+-
+
+odometryThread
+private final edu.wpi.first.wpilibj.Notifier odometryThread
+WPILib Notifier to keep odometry up to date.
+
+
@@ -550,9 +572,9 @@ loadScripts(document, 'script');
Creates a new swerve drivebase subsystem. Robot is controlled via the drive(edu.wpi.first.math.geometry.Translation2d, double, boolean, boolean) method, or via the
- setRawModuleStates(swervelib.math.SwerveModuleState2[], boolean) method. The drive(edu.wpi.first.math.geometry.Translation2d, double, boolean, boolean) method incorporates kinematics-- it
+ setRawModuleStates(edu.wpi.first.math.kinematics.SwerveModuleState[], boolean) method. The drive(edu.wpi.first.math.geometry.Translation2d, double, boolean, boolean) method incorporates kinematics-- it
takes a translation and rotation, as well as parameters for field-centric and closed-loop velocity control.
- setRawModuleStates(swervelib.math.SwerveModuleState2[], boolean) takes a list of SwerveModuleStates and directly passes them to the modules.
+ 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:
@@ -571,14 +593,32 @@ loadScripts(document, 'script');
Method Details
-
+
+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.
+
+
+-
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
+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. This method
defaults to no heading correction.
@@ -640,9 +680,9 @@ loadScripts(document, 'script');
-
-
+
setRawModuleStates
-private void setRawModuleStates(SwerveModuleState2[] desiredStates,
+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.
@@ -653,9 +693,9 @@ loadScripts(document, 'script');
-
-
+
setModuleStates
-public void setModuleStates(SwerveModuleState2[] desiredStates,
+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.
@@ -736,7 +776,7 @@ loadScripts(document, 'script');
-
getStates
-
+public edu.wpi.first.math.kinematics.SwerveModuleState[] getStates()
Gets the current module states (azimuth and velocity)
- Returns:
@@ -836,7 +876,7 @@ loadScripts(document, 'script');
boolean updateModuleFeedforward)
Set the maximum speed of the drive motors, modified SwerveControllerConfiguration.maxSpeed and
SwerveDriveConfiguration.maxSpeed which is used for the
- setRawModuleStates(SwerveModuleState2[], boolean) function and
+ 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.
@@ -854,7 +894,7 @@ loadScripts(document, 'script');
public void setMaximumSpeed(double maximumSpeed)
Set the maximum speed of the drive motors, modified SwerveControllerConfiguration.maxSpeed and
SwerveDriveConfiguration.maxSpeed which is used for the
- setRawModuleStates(SwerveModuleState2[], boolean) function and
+ 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.
@@ -1023,7 +1063,7 @@ loadScripts(document, 'script');
setModuleStateOptimization
public void setModuleStateOptimization(boolean optimizationEnabled)
-Configure whether the SwerveModuleState2 will be optimized to prevent spinning more than 90deg.
+Configure whether the SwerveModuleState will be optimized to prevent spinning more than 90deg.
- Parameters:
optimizationEnabled - Whether the module optimization is enabled.SwerveModule
@@ -121,7 +121,7 @@ loadScripts(document, 'script');
Feedforward for drive motor during closed loop control.
-
+edu.wpi.first.math.kinematics.SwerveModuleState
Last swerve module state applied.
@@ -134,7 +134,7 @@ loadScripts(document, 'script');
boolean
-Enable SwerveModuleState2 optimizations so the angle is reversed and speed is reversed to ensure the module
+Enable SwerveModuleState optimizations so the angle is reversed and speed is reversed to ensure the module
never turns more than 90deg.
private SwerveModuleSimulation
@@ -207,7 +207,7 @@ loadScripts(document, 'script');
Get the relative angle in degrees.
-
+edu.wpi.first.math.kinematics.SwerveModuleState
getState()
Get the Swerve Module state.
@@ -223,7 +223,7 @@ loadScripts(document, 'script');
Set the angle for the module.
void
-setDesiredState(SwerveModuleState2 desiredState,
+setDesiredState(edu.wpi.first.math.kinematics.SwerveModuleState desiredState,
boolean isOpenLoop,
boolean force)
@@ -303,7 +303,7 @@ loadScripts(document, 'script');
-
lastState
-
+public edu.wpi.first.math.kinematics.SwerveModuleState lastState
Last swerve module state applied.
@@ -311,7 +311,7 @@ loadScripts(document, 'script');
moduleStateOptimization
public boolean moduleStateOptimization
-Enable SwerveModuleState2 optimizations so the angle is reversed and speed is reversed to ensure the module
+Enable SwerveModuleState optimizations so the angle is reversed and speed is reversed to ensure the module
never turns more than 90deg.
@@ -366,9 +366,9 @@ loadScripts(document, 'script');
-
-
+
setDesiredState
-public void setDesiredState(SwerveModuleState2 desiredState,
+public void setDesiredState(edu.wpi.first.math.kinematics.SwerveModuleState desiredState,
boolean isOpenLoop,
boolean force)
Set the desired state of the swerve module.
WARNING: If you are not using one of the functions from
@@ -396,7 +396,7 @@ loadScripts(document, 'script');
-
getState
-
+public edu.wpi.first.math.kinematics.SwerveModuleState getState()
Get the Swerve Module state.
- Returns:
diff --git a/docs/swervelib/encoders/AnalogAbsoluteEncoderSwerve.html b/docs/swervelib/encoders/AnalogAbsoluteEncoderSwerve.html
index d11bc83..3288f02 100644
--- a/docs/swervelib/encoders/AnalogAbsoluteEncoderSwerve.html
+++ b/docs/swervelib/encoders/AnalogAbsoluteEncoderSwerve.html
@@ -1,7 +1,7 @@
-
+
AnalogAbsoluteEncoderSwerve
diff --git a/docs/swervelib/encoders/CANCoderSwerve.html b/docs/swervelib/encoders/CANCoderSwerve.html
index d7742ed..11309fb 100644
--- a/docs/swervelib/encoders/CANCoderSwerve.html
+++ b/docs/swervelib/encoders/CANCoderSwerve.html
@@ -1,7 +1,7 @@
-
+
CANCoderSwerve
diff --git a/docs/swervelib/encoders/CanAndCoderSwerve.html b/docs/swervelib/encoders/CanAndCoderSwerve.html
index 825b4de..7abaaa3 100644
--- a/docs/swervelib/encoders/CanAndCoderSwerve.html
+++ b/docs/swervelib/encoders/CanAndCoderSwerve.html
@@ -1,7 +1,7 @@
-
+
CanAndCoderSwerve
diff --git a/docs/swervelib/encoders/SparkMaxEncoderSwerve.html b/docs/swervelib/encoders/SparkMaxEncoderSwerve.html
index 2fbef91..9bd2f52 100644
--- a/docs/swervelib/encoders/SparkMaxEncoderSwerve.html
+++ b/docs/swervelib/encoders/SparkMaxEncoderSwerve.html
@@ -1,7 +1,7 @@
-
+
SparkMaxEncoderSwerve
diff --git a/docs/swervelib/encoders/SwerveAbsoluteEncoder.html b/docs/swervelib/encoders/SwerveAbsoluteEncoder.html
index 2739a41..d0fef3f 100644
--- a/docs/swervelib/encoders/SwerveAbsoluteEncoder.html
+++ b/docs/swervelib/encoders/SwerveAbsoluteEncoder.html
@@ -1,7 +1,7 @@
-
+
SwerveAbsoluteEncoder
diff --git a/docs/swervelib/encoders/package-summary.html b/docs/swervelib/encoders/package-summary.html
index ebf01a9..92c88f0 100644
--- a/docs/swervelib/encoders/package-summary.html
+++ b/docs/swervelib/encoders/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.encoders
diff --git a/docs/swervelib/encoders/package-tree.html b/docs/swervelib/encoders/package-tree.html
index d781e0a..284c04f 100644
--- a/docs/swervelib/encoders/package-tree.html
+++ b/docs/swervelib/encoders/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.encoders Class Hierarchy
diff --git a/docs/swervelib/imu/ADIS16448Swerve.html b/docs/swervelib/imu/ADIS16448Swerve.html
index ad4c750..acd62ba 100644
--- a/docs/swervelib/imu/ADIS16448Swerve.html
+++ b/docs/swervelib/imu/ADIS16448Swerve.html
@@ -1,7 +1,7 @@
-
+
ADIS16448Swerve
diff --git a/docs/swervelib/imu/ADIS16470Swerve.html b/docs/swervelib/imu/ADIS16470Swerve.html
index 5061e9e..57805a4 100644
--- a/docs/swervelib/imu/ADIS16470Swerve.html
+++ b/docs/swervelib/imu/ADIS16470Swerve.html
@@ -1,7 +1,7 @@
-
+
ADIS16470Swerve
diff --git a/docs/swervelib/imu/ADXRS450Swerve.html b/docs/swervelib/imu/ADXRS450Swerve.html
index 16bdade..30d19ef 100644
--- a/docs/swervelib/imu/ADXRS450Swerve.html
+++ b/docs/swervelib/imu/ADXRS450Swerve.html
@@ -1,7 +1,7 @@
-
+
ADXRS450Swerve
diff --git a/docs/swervelib/imu/AnalogGyroSwerve.html b/docs/swervelib/imu/AnalogGyroSwerve.html
index 5fe4853..a2bc573 100644
--- a/docs/swervelib/imu/AnalogGyroSwerve.html
+++ b/docs/swervelib/imu/AnalogGyroSwerve.html
@@ -1,7 +1,7 @@
-
+
AnalogGyroSwerve
diff --git a/docs/swervelib/imu/NavXSwerve.html b/docs/swervelib/imu/NavXSwerve.html
index 9bbcb01..ae7f50a 100644
--- a/docs/swervelib/imu/NavXSwerve.html
+++ b/docs/swervelib/imu/NavXSwerve.html
@@ -1,7 +1,7 @@
-
+
NavXSwerve
diff --git a/docs/swervelib/imu/Pigeon2Swerve.html b/docs/swervelib/imu/Pigeon2Swerve.html
index 010e805..8c0c47b 100644
--- a/docs/swervelib/imu/Pigeon2Swerve.html
+++ b/docs/swervelib/imu/Pigeon2Swerve.html
@@ -1,7 +1,7 @@
-
+
Pigeon2Swerve
diff --git a/docs/swervelib/imu/PigeonSwerve.html b/docs/swervelib/imu/PigeonSwerve.html
index 4803ade..54fe281 100644
--- a/docs/swervelib/imu/PigeonSwerve.html
+++ b/docs/swervelib/imu/PigeonSwerve.html
@@ -1,7 +1,7 @@
-
+
PigeonSwerve
diff --git a/docs/swervelib/imu/SwerveIMU.html b/docs/swervelib/imu/SwerveIMU.html
index 82d250e..b905e2b 100644
--- a/docs/swervelib/imu/SwerveIMU.html
+++ b/docs/swervelib/imu/SwerveIMU.html
@@ -1,7 +1,7 @@
-
+
SwerveIMU
diff --git a/docs/swervelib/imu/package-summary.html b/docs/swervelib/imu/package-summary.html
index 13aa712..4924a5b 100644
--- a/docs/swervelib/imu/package-summary.html
+++ b/docs/swervelib/imu/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.imu
diff --git a/docs/swervelib/imu/package-tree.html b/docs/swervelib/imu/package-tree.html
index 5e44fca..6d3d666 100644
--- a/docs/swervelib/imu/package-tree.html
+++ b/docs/swervelib/imu/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.imu Class Hierarchy
diff --git a/docs/swervelib/math/Matter.html b/docs/swervelib/math/Matter.html
index 78e6b67..a208522 100644
--- a/docs/swervelib/math/Matter.html
+++ b/docs/swervelib/math/Matter.html
@@ -1,7 +1,7 @@
-
+
Matter
diff --git a/docs/swervelib/math/SwerveMath.html b/docs/swervelib/math/SwerveMath.html
index fa11695..a2c6efb 100644
--- a/docs/swervelib/math/SwerveMath.html
+++ b/docs/swervelib/math/SwerveMath.html
@@ -1,7 +1,7 @@
-
+
SwerveMath
@@ -107,8 +107,8 @@ loadScripts(document, 'script');
Method
Description
static void
-antiJitter(SwerveModuleState2 moduleState,
- SwerveModuleState2 lastModuleState,
+antiJitter(edu.wpi.first.math.kinematics.SwerveModuleState moduleState,
+ edu.wpi.first.math.kinematics.SwerveModuleState lastModuleState,
double maxSpeed)
Perform anti-jitter within modules if the speed requested is too low.
@@ -438,16 +438,16 @@ loadScripts(document, 'script');
-
-
+
antiJitter
-public static void antiJitter(SwerveModuleState2 moduleState,
- SwerveModuleState2 lastModuleState,
+public static void antiJitter(edu.wpi.first.math.kinematics.SwerveModuleState moduleState,
+ edu.wpi.first.math.kinematics.SwerveModuleState lastModuleState,
double maxSpeed)
Perform anti-jitter within modules if the speed requested is too low.
- Parameters:
-moduleState - Current SwerveModuleState2 requested.lastModuleState - Previous SwerveModuleState2 used.moduleState - Current SwerveModuleState requested.lastModuleState - Previous SwerveModuleState used.maxSpeed - Maximum speed of the modules, should be in SwerveDriveConfiguration.maxSpeed.
diff --git a/docs/swervelib/math/package-summary.html b/docs/swervelib/math/package-summary.html
index 40af332..86b8f7a 100644
--- a/docs/swervelib/math/package-summary.html
+++ b/docs/swervelib/math/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.math
@@ -90,28 +90,10 @@ loadScripts(document, 'script');
Object with significant mass that needs to be taken into account.
-
-
-Clone of SwerveDriveOdometry except uses gyro pitch and roll to achieve a more accurate estimation.
-
-
-
-Clone of WPI SwerveKinematics, which implements second order kinematics when calculating modules states from chassis
- speed.
-
Mathematical functions which pertain to swerve drive.
-
-
-Second order kinematics swerve module state.
-
-
-
-Clone of SwerveDrivePoseEstimator which takes into account gyroscope pitch and roll to achieve a more
- accurate estimation, originally made by Team 1466.
-
diff --git a/docs/swervelib/math/package-tree.html b/docs/swervelib/math/package-tree.html
index ff7a45d..df5787d 100644
--- a/docs/swervelib/math/package-tree.html
+++ b/docs/swervelib/math/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.math Class Hierarchy
@@ -60,24 +60,7 @@ loadScripts(document, 'script');
- java.lang.Object
- swervelib.math.Matter
-- edu.wpi.first.math.kinematics.SwerveDriveKinematics
-
-- swervelib.math.SwerveKinematics2
-
-
-- edu.wpi.first.math.kinematics.SwerveDriveOdometry
-
-- swervelib.math.SwerveDriveOdometry2
-
-
- swervelib.math.SwerveMath
-- edu.wpi.first.math.kinematics.SwerveModuleState (implements java.lang.Comparable<T>)
-
-- swervelib.math.SwerveModuleState2
-
-
-- swervelib.math.SwervePoseEstimator2
-- swervelib.math.SwervePoseEstimator2.InterpolationRecord (implements edu.wpi.first.math.interpolation.Interpolatable<T>)
diff --git a/docs/swervelib/motors/SparkMaxBrushedMotorSwerve.html b/docs/swervelib/motors/SparkMaxBrushedMotorSwerve.html
index 7e1176c..17030db 100644
--- a/docs/swervelib/motors/SparkMaxBrushedMotorSwerve.html
+++ b/docs/swervelib/motors/SparkMaxBrushedMotorSwerve.html
@@ -1,7 +1,7 @@
-
+
SparkMaxBrushedMotorSwerve
diff --git a/docs/swervelib/motors/SparkMaxSwerve.SparkMAX_slotIdx.html b/docs/swervelib/motors/SparkMaxSwerve.SparkMAX_slotIdx.html
index 4d423ba..f19652a 100644
--- a/docs/swervelib/motors/SparkMaxSwerve.SparkMAX_slotIdx.html
+++ b/docs/swervelib/motors/SparkMaxSwerve.SparkMAX_slotIdx.html
@@ -1,7 +1,7 @@
-
+
SparkMaxSwerve.SparkMAX_slotIdx
diff --git a/docs/swervelib/motors/SparkMaxSwerve.html b/docs/swervelib/motors/SparkMaxSwerve.html
index 948c2cd..9e4672e 100644
--- a/docs/swervelib/motors/SparkMaxSwerve.html
+++ b/docs/swervelib/motors/SparkMaxSwerve.html
@@ -1,7 +1,7 @@
-
+
SparkMaxSwerve
diff --git a/docs/swervelib/motors/SwerveMotor.html b/docs/swervelib/motors/SwerveMotor.html
index e2726f3..d4b91d4 100644
--- a/docs/swervelib/motors/SwerveMotor.html
+++ b/docs/swervelib/motors/SwerveMotor.html
@@ -1,7 +1,7 @@
-
+
SwerveMotor
diff --git a/docs/swervelib/motors/TalonFXSwerve.html b/docs/swervelib/motors/TalonFXSwerve.html
index 3c838ab..d7fe916 100644
--- a/docs/swervelib/motors/TalonFXSwerve.html
+++ b/docs/swervelib/motors/TalonFXSwerve.html
@@ -1,7 +1,7 @@
-
+
TalonFXSwerve
diff --git a/docs/swervelib/motors/TalonSRXSwerve.html b/docs/swervelib/motors/TalonSRXSwerve.html
index 413f341..62cfcfe 100644
--- a/docs/swervelib/motors/TalonSRXSwerve.html
+++ b/docs/swervelib/motors/TalonSRXSwerve.html
@@ -1,7 +1,7 @@
-
+
TalonSRXSwerve
diff --git a/docs/swervelib/motors/package-summary.html b/docs/swervelib/motors/package-summary.html
index 2a4e95b..db083c3 100644
--- a/docs/swervelib/motors/package-summary.html
+++ b/docs/swervelib/motors/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.motors
diff --git a/docs/swervelib/motors/package-tree.html b/docs/swervelib/motors/package-tree.html
index d40c168..8c74c7c 100644
--- a/docs/swervelib/motors/package-tree.html
+++ b/docs/swervelib/motors/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.motors Class Hierarchy
diff --git a/docs/swervelib/package-summary.html b/docs/swervelib/package-summary.html
index 99c8623..760bd4e 100644
--- a/docs/swervelib/package-summary.html
+++ b/docs/swervelib/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib
diff --git a/docs/swervelib/package-tree.html b/docs/swervelib/package-tree.html
index a43eb08..5f00661 100644
--- a/docs/swervelib/package-tree.html
+++ b/docs/swervelib/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib Class Hierarchy
diff --git a/docs/swervelib/parser/PIDFConfig.html b/docs/swervelib/parser/PIDFConfig.html
index 6c21e69..dd69ded 100644
--- a/docs/swervelib/parser/PIDFConfig.html
+++ b/docs/swervelib/parser/PIDFConfig.html
@@ -1,7 +1,7 @@
-
+
PIDFConfig
diff --git a/docs/swervelib/parser/SwerveControllerConfiguration.html b/docs/swervelib/parser/SwerveControllerConfiguration.html
index 20b60f7..3b48b3c 100644
--- a/docs/swervelib/parser/SwerveControllerConfiguration.html
+++ b/docs/swervelib/parser/SwerveControllerConfiguration.html
@@ -1,7 +1,7 @@
-
+
SwerveControllerConfiguration
diff --git a/docs/swervelib/parser/SwerveDriveConfiguration.html b/docs/swervelib/parser/SwerveDriveConfiguration.html
index 90214f0..23e189f 100644
--- a/docs/swervelib/parser/SwerveDriveConfiguration.html
+++ b/docs/swervelib/parser/SwerveDriveConfiguration.html
@@ -1,7 +1,7 @@
-
+
SwerveDriveConfiguration
diff --git a/docs/swervelib/parser/SwerveModuleConfiguration.html b/docs/swervelib/parser/SwerveModuleConfiguration.html
index d0d8660..2f6718c 100644
--- a/docs/swervelib/parser/SwerveModuleConfiguration.html
+++ b/docs/swervelib/parser/SwerveModuleConfiguration.html
@@ -1,7 +1,7 @@
-
+
SwerveModuleConfiguration
diff --git a/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html b/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html
index be8cda2..c942361 100644
--- a/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html
+++ b/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html
@@ -1,7 +1,7 @@
-
+
SwerveModulePhysicalCharacteristics
diff --git a/docs/swervelib/parser/SwerveParser.html b/docs/swervelib/parser/SwerveParser.html
index 4a7db52..113c4fb 100644
--- a/docs/swervelib/parser/SwerveParser.html
+++ b/docs/swervelib/parser/SwerveParser.html
@@ -1,7 +1,7 @@
-
+
SwerveParser
diff --git a/docs/swervelib/parser/deserializer/PIDFRange.html b/docs/swervelib/parser/deserializer/PIDFRange.html
index 619dd97..74e0cb5 100644
--- a/docs/swervelib/parser/deserializer/PIDFRange.html
+++ b/docs/swervelib/parser/deserializer/PIDFRange.html
@@ -1,7 +1,7 @@
-
+
PIDFRange
diff --git a/docs/swervelib/parser/deserializer/package-summary.html b/docs/swervelib/parser/deserializer/package-summary.html
index 91ec27a..404ebcc 100644
--- a/docs/swervelib/parser/deserializer/package-summary.html
+++ b/docs/swervelib/parser/deserializer/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser.deserializer
diff --git a/docs/swervelib/parser/deserializer/package-tree.html b/docs/swervelib/parser/deserializer/package-tree.html
index 5cd63ab..e657e95 100644
--- a/docs/swervelib/parser/deserializer/package-tree.html
+++ b/docs/swervelib/parser/deserializer/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser.deserializer Class Hierarchy
diff --git a/docs/swervelib/parser/json/ControllerPropertiesJson.html b/docs/swervelib/parser/json/ControllerPropertiesJson.html
index 7b0eab2..8614ff0 100644
--- a/docs/swervelib/parser/json/ControllerPropertiesJson.html
+++ b/docs/swervelib/parser/json/ControllerPropertiesJson.html
@@ -1,7 +1,7 @@
-
+
ControllerPropertiesJson
diff --git a/docs/swervelib/parser/json/DeviceJson.html b/docs/swervelib/parser/json/DeviceJson.html
index 6b4b927..21d1f7d 100644
--- a/docs/swervelib/parser/json/DeviceJson.html
+++ b/docs/swervelib/parser/json/DeviceJson.html
@@ -1,7 +1,7 @@
-
+
DeviceJson
diff --git a/docs/swervelib/parser/json/ModuleJson.html b/docs/swervelib/parser/json/ModuleJson.html
index 5cfde78..87375ab 100644
--- a/docs/swervelib/parser/json/ModuleJson.html
+++ b/docs/swervelib/parser/json/ModuleJson.html
@@ -1,7 +1,7 @@
-
+
ModuleJson
diff --git a/docs/swervelib/parser/json/MotorConfigDouble.html b/docs/swervelib/parser/json/MotorConfigDouble.html
index 8600daf..90f6976 100644
--- a/docs/swervelib/parser/json/MotorConfigDouble.html
+++ b/docs/swervelib/parser/json/MotorConfigDouble.html
@@ -1,7 +1,7 @@
-
+
MotorConfigDouble
diff --git a/docs/swervelib/parser/json/MotorConfigInt.html b/docs/swervelib/parser/json/MotorConfigInt.html
index e092c98..4f1a44f 100644
--- a/docs/swervelib/parser/json/MotorConfigInt.html
+++ b/docs/swervelib/parser/json/MotorConfigInt.html
@@ -1,7 +1,7 @@
-
+
MotorConfigInt
diff --git a/docs/swervelib/parser/json/PIDFPropertiesJson.html b/docs/swervelib/parser/json/PIDFPropertiesJson.html
index 12506da..3b0ba95 100644
--- a/docs/swervelib/parser/json/PIDFPropertiesJson.html
+++ b/docs/swervelib/parser/json/PIDFPropertiesJson.html
@@ -1,7 +1,7 @@
-
+
PIDFPropertiesJson
diff --git a/docs/swervelib/parser/json/PhysicalPropertiesJson.html b/docs/swervelib/parser/json/PhysicalPropertiesJson.html
index ec98e80..ffc751a 100644
--- a/docs/swervelib/parser/json/PhysicalPropertiesJson.html
+++ b/docs/swervelib/parser/json/PhysicalPropertiesJson.html
@@ -1,7 +1,7 @@
-
+
PhysicalPropertiesJson
diff --git a/docs/swervelib/parser/json/SwerveDriveJson.html b/docs/swervelib/parser/json/SwerveDriveJson.html
index 56930bd..0e00034 100644
--- a/docs/swervelib/parser/json/SwerveDriveJson.html
+++ b/docs/swervelib/parser/json/SwerveDriveJson.html
@@ -1,7 +1,7 @@
-
+
SwerveDriveJson
diff --git a/docs/swervelib/parser/json/modules/BoolMotorJson.html b/docs/swervelib/parser/json/modules/BoolMotorJson.html
index 3f9a0d2..4c5f2e5 100644
--- a/docs/swervelib/parser/json/modules/BoolMotorJson.html
+++ b/docs/swervelib/parser/json/modules/BoolMotorJson.html
@@ -1,7 +1,7 @@
-
+
BoolMotorJson
diff --git a/docs/swervelib/parser/json/modules/LocationJson.html b/docs/swervelib/parser/json/modules/LocationJson.html
index 3d7997f..0bb30c3 100644
--- a/docs/swervelib/parser/json/modules/LocationJson.html
+++ b/docs/swervelib/parser/json/modules/LocationJson.html
@@ -1,7 +1,7 @@
-
+
LocationJson
diff --git a/docs/swervelib/parser/json/modules/package-summary.html b/docs/swervelib/parser/json/modules/package-summary.html
index 80381aa..039ff0e 100644
--- a/docs/swervelib/parser/json/modules/package-summary.html
+++ b/docs/swervelib/parser/json/modules/package-summary.html
@@ -1,7 +1,7 @@
-
+
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 3d8490c..b0074a0 100644
--- a/docs/swervelib/parser/json/modules/package-tree.html
+++ b/docs/swervelib/parser/json/modules/package-tree.html
@@ -1,7 +1,7 @@
-
+
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 322fdaa..3586aeb 100644
--- a/docs/swervelib/parser/json/package-summary.html
+++ b/docs/swervelib/parser/json/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser.json
diff --git a/docs/swervelib/parser/json/package-tree.html b/docs/swervelib/parser/json/package-tree.html
index 8329b8f..ae8086a 100644
--- a/docs/swervelib/parser/json/package-tree.html
+++ b/docs/swervelib/parser/json/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser.json Class Hierarchy
diff --git a/docs/swervelib/parser/package-summary.html b/docs/swervelib/parser/package-summary.html
index 3e093f2..9d06773 100644
--- a/docs/swervelib/parser/package-summary.html
+++ b/docs/swervelib/parser/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser
diff --git a/docs/swervelib/parser/package-tree.html b/docs/swervelib/parser/package-tree.html
index f31ec00..fcf3bee 100644
--- a/docs/swervelib/parser/package-tree.html
+++ b/docs/swervelib/parser/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser Class Hierarchy
diff --git a/docs/swervelib/simulation/SwerveIMUSimulation.html b/docs/swervelib/simulation/SwerveIMUSimulation.html
index eda8ce9..0a3868f 100644
--- a/docs/swervelib/simulation/SwerveIMUSimulation.html
+++ b/docs/swervelib/simulation/SwerveIMUSimulation.html
@@ -1,7 +1,7 @@
-
+
SwerveIMUSimulation
@@ -166,8 +166,8 @@ loadScripts(document, 'script');
Set the heading of the robot.
void
-updateOdometry(SwerveKinematics2 kinematics,
- SwerveModuleState2[] states,
+updateOdometry(edu.wpi.first.math.kinematics.SwerveDriveKinematics kinematics,
+ edu.wpi.first.math.kinematics.SwerveModuleState[] states,
edu.wpi.first.math.geometry.Pose2d[] modulePoses,
edu.wpi.first.wpilibj.smartdashboard.Field2d field)
@@ -291,18 +291,18 @@ loadScripts(document, 'script');
-
-
+
updateOdometry
-public void updateOdometry(SwerveKinematics2 kinematics,
- SwerveModuleState2[] states,
+public void updateOdometry(edu.wpi.first.math.kinematics.SwerveDriveKinematics kinematics,
+ edu.wpi.first.math.kinematics.SwerveModuleState[] states,
edu.wpi.first.math.geometry.Pose2d[] modulePoses,
edu.wpi.first.wpilibj.smartdashboard.Field2d field)
- Parameters:
-kinematics - SwerveKinematics2 of the swerve drive.states - SwerveModuleState2 array of the module states.kinematics - SwerveDriveKinematics of the swerve drive.states - SwerveModuleState array of the module states.modulePoses - Pose2d representing the swerve modules.field - Field2d to update.
diff --git a/docs/swervelib/simulation/SwerveModuleSimulation.html b/docs/swervelib/simulation/SwerveModuleSimulation.html
index 2e32d6e..411398a 100644
--- a/docs/swervelib/simulation/SwerveModuleSimulation.html
+++ b/docs/swervelib/simulation/SwerveModuleSimulation.html
@@ -1,7 +1,7 @@
-
+
SwerveModuleSimulation
@@ -111,7 +111,7 @@ loadScripts(document, 'script');
Last time queried.
-private SwerveModuleState2
+private edu.wpi.first.math.kinematics.SwerveModuleState
Current simulated swerve module state.
@@ -155,13 +155,13 @@ loadScripts(document, 'script');
Get the simulated swerve module position.
-
+edu.wpi.first.math.kinematics.SwerveModuleState
getState()
-Get the SwerveModuleState2 of the simulated module.
+Get the SwerveModuleState of the simulated module.
void
-updateStateAndPosition(SwerveModuleState2 desiredState)
+updateStateAndPosition(edu.wpi.first.math.kinematics.SwerveModuleState desiredState)
Update the position and state of the module.
@@ -220,7 +220,7 @@ loadScripts(document, 'script');
-
state
-
+private edu.wpi.first.math.kinematics.SwerveModuleState state
Current simulated swerve module state.
@@ -248,10 +248,10 @@ loadScripts(document, 'script');
Method Details
-
-
+
updateStateAndPosition
-
-Update the position and state of the module. Called from SwerveModule.setDesiredState(swervelib.math.SwerveModuleState2, boolean, boolean) function
+public void updateStateAndPosition(edu.wpi.first.math.kinematics.SwerveModuleState desiredState)
+Update the position and state of the module. Called from SwerveModule.setDesiredState(edu.wpi.first.math.kinematics.SwerveModuleState, boolean, boolean) function
when simulated.
- Parameters:
@@ -273,11 +273,11 @@ loadScripts(document, 'script');
-
getState
-
-Get the SwerveModuleState2 of the simulated module.
+public edu.wpi.first.math.kinematics.SwerveModuleState getState()
+Get the SwerveModuleState of the simulated module.
- Returns:
-SwerveModuleState2 of the simulated module.SwerveModuleState of the simulated module.
diff --git a/docs/swervelib/simulation/ctre/PhysicsSim.SimProfile.html b/docs/swervelib/simulation/ctre/PhysicsSim.SimProfile.html
index 5c28573..bdb929e 100644
--- a/docs/swervelib/simulation/ctre/PhysicsSim.SimProfile.html
+++ b/docs/swervelib/simulation/ctre/PhysicsSim.SimProfile.html
@@ -1,7 +1,7 @@
-
+
PhysicsSim.SimProfile
diff --git a/docs/swervelib/simulation/ctre/PhysicsSim.html b/docs/swervelib/simulation/ctre/PhysicsSim.html
index 58b9447..4ec4f0d 100644
--- a/docs/swervelib/simulation/ctre/PhysicsSim.html
+++ b/docs/swervelib/simulation/ctre/PhysicsSim.html
@@ -1,7 +1,7 @@
-
+
PhysicsSim
diff --git a/docs/swervelib/simulation/ctre/TalonFXSimProfile.html b/docs/swervelib/simulation/ctre/TalonFXSimProfile.html
index a97deb9..b1f401f 100644
--- a/docs/swervelib/simulation/ctre/TalonFXSimProfile.html
+++ b/docs/swervelib/simulation/ctre/TalonFXSimProfile.html
@@ -1,7 +1,7 @@
-
+
TalonFXSimProfile
diff --git a/docs/swervelib/simulation/ctre/TalonSRXSimProfile.html b/docs/swervelib/simulation/ctre/TalonSRXSimProfile.html
index 446855c..af3f4a1 100644
--- a/docs/swervelib/simulation/ctre/TalonSRXSimProfile.html
+++ b/docs/swervelib/simulation/ctre/TalonSRXSimProfile.html
@@ -1,7 +1,7 @@
-
+
TalonSRXSimProfile
diff --git a/docs/swervelib/simulation/ctre/VictorSPXSimProfile.html b/docs/swervelib/simulation/ctre/VictorSPXSimProfile.html
index 264b30d..119d8da 100644
--- a/docs/swervelib/simulation/ctre/VictorSPXSimProfile.html
+++ b/docs/swervelib/simulation/ctre/VictorSPXSimProfile.html
@@ -1,7 +1,7 @@
-
+
VictorSPXSimProfile
diff --git a/docs/swervelib/simulation/ctre/package-summary.html b/docs/swervelib/simulation/ctre/package-summary.html
index 5261681..ea113ae 100644
--- a/docs/swervelib/simulation/ctre/package-summary.html
+++ b/docs/swervelib/simulation/ctre/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.simulation.ctre
diff --git a/docs/swervelib/simulation/ctre/package-tree.html b/docs/swervelib/simulation/ctre/package-tree.html
index ab92c93..8459df1 100644
--- a/docs/swervelib/simulation/ctre/package-tree.html
+++ b/docs/swervelib/simulation/ctre/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.simulation.ctre Class Hierarchy
diff --git a/docs/swervelib/simulation/package-summary.html b/docs/swervelib/simulation/package-summary.html
index ad5b292..a52ef83 100644
--- a/docs/swervelib/simulation/package-summary.html
+++ b/docs/swervelib/simulation/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.simulation
diff --git a/docs/swervelib/simulation/package-tree.html b/docs/swervelib/simulation/package-tree.html
index b4acb60..0d0bb46 100644
--- a/docs/swervelib/simulation/package-tree.html
+++ b/docs/swervelib/simulation/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.simulation Class Hierarchy
diff --git a/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html b/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html
index f8e4119..0f40282 100644
--- a/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html
+++ b/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html
@@ -1,7 +1,7 @@
-
+
SwerveDriveTelemetry.TelemetryVerbosity
diff --git a/docs/swervelib/telemetry/SwerveDriveTelemetry.html b/docs/swervelib/telemetry/SwerveDriveTelemetry.html
index 5395723..9adecac 100644
--- a/docs/swervelib/telemetry/SwerveDriveTelemetry.html
+++ b/docs/swervelib/telemetry/SwerveDriveTelemetry.html
@@ -1,7 +1,7 @@
-
+
SwerveDriveTelemetry
diff --git a/docs/swervelib/telemetry/package-summary.html b/docs/swervelib/telemetry/package-summary.html
index 0e38229..df7cdd3 100644
--- a/docs/swervelib/telemetry/package-summary.html
+++ b/docs/swervelib/telemetry/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.telemetry
diff --git a/docs/swervelib/telemetry/package-tree.html b/docs/swervelib/telemetry/package-tree.html
index afe8119..4c64838 100644
--- a/docs/swervelib/telemetry/package-tree.html
+++ b/docs/swervelib/telemetry/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.telemetry Class Hierarchy
diff --git a/docs/type-search-index.js b/docs/type-search-index.js
index 096b337..4d58ee0 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"},{"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":"BoolMotorJson"},{"p":"swervelib.encoders","l":"CanAndCoderSwerve"},{"p":"swervelib.encoders","l":"CANCoderSwerve"},{"p":"swervelib.parser.json","l":"ControllerPropertiesJson"},{"p":"swervelib.parser.json","l":"DeviceJson"},{"p":"swervelib.math","l":"SwervePoseEstimator2.InterpolationRecord"},{"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.simulation.ctre","l":"PhysicsSim"},{"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.simulation.ctre","l":"PhysicsSim.SimProfile"},{"p":"swervelib.motors","l":"SparkMaxSwerve.SparkMAX_slotIdx"},{"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.math","l":"SwerveDriveOdometry2"},{"p":"swervelib.telemetry","l":"SwerveDriveTelemetry"},{"p":"swervelib.imu","l":"SwerveIMU"},{"p":"swervelib.simulation","l":"SwerveIMUSimulation"},{"p":"swervelib.math","l":"SwerveKinematics2"},{"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.math","l":"SwerveModuleState2"},{"p":"swervelib.motors","l":"SwerveMotor"},{"p":"swervelib.parser","l":"SwerveParser"},{"p":"swervelib.math","l":"SwervePoseEstimator2"},{"p":"swervelib.simulation.ctre","l":"TalonFXSimProfile"},{"p":"swervelib.motors","l":"TalonFXSwerve"},{"p":"swervelib.simulation.ctre","l":"TalonSRXSimProfile"},{"p":"swervelib.motors","l":"TalonSRXSwerve"},{"p":"swervelib.telemetry","l":"SwerveDriveTelemetry.TelemetryVerbosity"},{"p":"swervelib.simulation.ctre","l":"VictorSPXSimProfile"}];updateSearchResults();
\ No newline at end of file
+typeSearchIndex = [{"p":"swervelib.imu","l":"ADIS16448Swerve"},{"p":"swervelib.imu","l":"ADIS16470Swerve"},{"p":"swervelib.imu","l":"ADXRS450Swerve"},{"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":"BoolMotorJson"},{"p":"swervelib.encoders","l":"CanAndCoderSwerve"},{"p":"swervelib.encoders","l":"CANCoderSwerve"},{"p":"swervelib.parser.json","l":"ControllerPropertiesJson"},{"p":"swervelib.parser.json","l":"DeviceJson"},{"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.simulation.ctre","l":"PhysicsSim"},{"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.simulation.ctre","l":"PhysicsSim.SimProfile"},{"p":"swervelib.motors","l":"SparkMaxSwerve.SparkMAX_slotIdx"},{"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.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.simulation.ctre","l":"TalonFXSimProfile"},{"p":"swervelib.motors","l":"TalonFXSwerve"},{"p":"swervelib.simulation.ctre","l":"TalonSRXSimProfile"},{"p":"swervelib.motors","l":"TalonSRXSwerve"},{"p":"swervelib.telemetry","l":"SwerveDriveTelemetry.TelemetryVerbosity"},{"p":"swervelib.simulation.ctre","l":"VictorSPXSimProfile"}];updateSearchResults();
\ No newline at end of file
diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java
index a003e09..b764128 100644
--- a/swervelib/SwerveDrive.java
+++ b/swervelib/SwerveDrive.java
@@ -13,11 +13,14 @@ import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
@@ -26,10 +29,7 @@ import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import swervelib.imu.SwerveIMU;
-import swervelib.math.SwerveKinematics2;
import swervelib.math.SwerveMath;
-import swervelib.math.SwerveModuleState2;
-import swervelib.math.SwervePoseEstimator2;
import swervelib.parser.SwerveControllerConfiguration;
import swervelib.parser.SwerveDriveConfiguration;
import swervelib.simulation.SwerveIMUSimulation;
@@ -45,7 +45,7 @@ public class SwerveDrive
/**
* Swerve Kinematics object utilizing second order kinematics.
*/
- public final SwerveKinematics2 kinematics;
+ public final SwerveDriveKinematics kinematics;
/**
* Swerve drive configuration.
*/
@@ -53,7 +53,7 @@ public class SwerveDrive
/**
* Swerve odometry.
*/
- public final SwervePoseEstimator2 swerveDrivePoseEstimator;
+ public final SwerveDrivePoseEstimator swerveDrivePoseEstimator;
/**
* Swerve modules.
*/
@@ -101,6 +101,10 @@ public class SwerveDrive
* The last heading set in radians.
*/
private double lastHeadingRadians = 0;
+ /**
+ * WPILib {@link Notifier} to keep odometry up to date.
+ */
+ private final Notifier odometryThread;
/**
* Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the
@@ -119,7 +123,8 @@ public class SwerveDrive
swerveDriveConfiguration = config;
swerveController = new SwerveController(controllerConfig);
// Create Kinematics from swerve module locations.
- kinematics = new SwerveKinematics2(config.moduleLocationsMeters);
+ kinematics = new SwerveDriveKinematics(config.moduleLocationsMeters);
+ odometryThread = new Notifier(this::updateOdometry);
// Create an integrator for angle if the robot is being simulated to emulate an IMU
// If the robot is real, instantiate the IMU instead.
@@ -136,7 +141,7 @@ public class SwerveDrive
// odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions());
swerveDrivePoseEstimator =
- new SwervePoseEstimator2(
+ new SwerveDrivePoseEstimator(
kinematics,
getYaw(),
getModulePositions(),
@@ -178,11 +183,32 @@ public class SwerveDrive
SwerveDriveTelemetry.measuredStates = new double[SwerveDriveTelemetry.moduleCount * 2];
SwerveDriveTelemetry.desiredStates = new double[SwerveDriveTelemetry.moduleCount * 2];
}
+
+ odometryThread.startPeriodic(SwerveDriveTelemetry.isSimulation ? 0.01 : 0.02);
}
/**
- * 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
+ * Set the odometry update period in seconds.
+ *
+ * @param period period in seconds.
+ */
+ public void setOdometryPeriod(double period)
+ {
+ odometryThread.stop();
+ odometryThread.startPeriodic(period);
+ }
+
+ /**
+ * Stop the odometry thread in favor of manually updating odometry.
+ */
+ public void stopOdometryThread()
+ {
+ odometryThread.stop();
+ }
+
+ /**
+ * The primary method for controlling the drivebase. Takes a {@link 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. This method
* defaults to no heading correction.
*
@@ -270,7 +296,7 @@ public class SwerveDrive
}
// Calculate required module states via kinematics
- SwerveModuleState2[] swerveModuleStates = kinematics.toSwerveModuleStates(velocity);
+ SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(velocity);
setRawModuleStates(swerveModuleStates, isOpenLoop);
}
@@ -303,16 +329,16 @@ public class SwerveDrive
* @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.
*/
- private void setRawModuleStates(SwerveModuleState2[] desiredStates, boolean isOpenLoop)
+ private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop)
{
// Desaturates wheel speeds
if (swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond != 0 ||
swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond != 0)
{
- SwerveKinematics2.desaturateWheelSpeeds(desiredStates, getRobotVelocity(),
- swerveDriveConfiguration.maxSpeed,
- swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond,
- swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond);
+ SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, getRobotVelocity(),
+ swerveDriveConfiguration.maxSpeed,
+ swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond,
+ swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond);
}
// Sets states
@@ -345,7 +371,7 @@ public class SwerveDrive
* @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(SwerveModuleState2[] desiredStates, boolean isOpenLoop)
+ public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop)
{
setRawModuleStates(kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates)),
isOpenLoop);
@@ -431,9 +457,9 @@ public class SwerveDrive
*
* @return A list of SwerveModuleStates containing the current module states
*/
- public SwerveModuleState2[] getStates()
+ public SwerveModuleState[] getStates()
{
- SwerveModuleState2[] states = new SwerveModuleState2[swerveDriveConfiguration.moduleCount];
+ SwerveModuleState[] states = new SwerveModuleState[swerveDriveConfiguration.moduleCount];
for (SwerveModule module : swerveModules)
{
states[module.moduleNumber] = module.getState();
@@ -589,7 +615,7 @@ public class SwerveDrive
/**
* Set the maximum speed of the drive motors, modified {@link SwerveControllerConfiguration#maxSpeed} and
* {@link SwerveDriveConfiguration#maxSpeed} which is used for the
- * {@link SwerveDrive#setRawModuleStates(SwerveModuleState2[], boolean)} function and
+ * {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], 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.
*
@@ -617,7 +643,7 @@ public class SwerveDrive
/**
* Set the maximum speed of the drive motors, modified {@link SwerveControllerConfiguration#maxSpeed} and
* {@link SwerveDriveConfiguration#maxSpeed} which is used for the
- * {@link SwerveDrive#setRawModuleStates(SwerveModuleState2[], boolean)} function and
+ * {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], 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#feedforward}.
@@ -638,8 +664,8 @@ public class SwerveDrive
// Sets states
for (SwerveModule swerveModule : swerveModules)
{
- SwerveModuleState2 desiredState =
- new SwerveModuleState2(0, swerveModule.configuration.moduleLocation.getAngle(), 0);
+ SwerveModuleState desiredState =
+ new SwerveModuleState(0, swerveModule.configuration.moduleLocation.getAngle());
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
{
SwerveDriveTelemetry.desiredStates[swerveModule.moduleNumber * 2] =
@@ -695,7 +721,7 @@ public class SwerveDrive
public void updateOdometry()
{
// Update odometry
- swerveDrivePoseEstimator.update(getYaw(), getPitch(), getRoll(), getModulePositions());
+ swerveDrivePoseEstimator.update(getYaw(), getModulePositions());
// Update angle accumulator if the robot is simulated
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
@@ -722,11 +748,11 @@ public class SwerveDrive
field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition());
}
- double sumOmega = 0;
+ double sumVelocity = 0;
for (SwerveModule module : swerveModules)
{
- SwerveModuleState2 moduleState = module.getState();
- sumOmega += Math.abs(moduleState.omegaRadPerSecond);
+ SwerveModuleState moduleState = module.getState();
+ sumVelocity += Math.abs(moduleState.speedMetersPerSecond);
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
SmartDashboard.putNumber(
@@ -744,7 +770,7 @@ public class SwerveDrive
// If the robot isn't moving synchronize the encoders every 100ms (Inspired by democrat's SDS
// lib)
// To ensure that everytime we initialize it works.
- if (sumOmega <= .01 && ++moduleSynchronizationCounter > 5)
+ if (sumVelocity <= .01 && ++moduleSynchronizationCounter > 5)
{
synchronizeModuleEncoders();
moduleSynchronizationCounter = 0;
@@ -896,7 +922,7 @@ public class SwerveDrive
}
/**
- * Configure whether the {@link SwerveModuleState2} will be optimized to prevent spinning more than 90deg.
+ * Configure whether the {@link SwerveModuleState} will be optimized to prevent spinning more than 90deg.
*
* @param optimizationEnabled Whether the module optimization is enabled.
*/
diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java
index 942ea4b..da0b7bf 100644
--- a/swervelib/SwerveModule.java
+++ b/swervelib/SwerveModule.java
@@ -3,10 +3,10 @@ package swervelib;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.math.SwerveMath;
-import swervelib.math.SwerveModuleState2;
import swervelib.motors.SwerveMotor;
import swervelib.parser.SwerveModuleConfiguration;
import swervelib.simulation.SwerveModuleSimulation;
@@ -46,9 +46,9 @@ public class SwerveModule
/**
* Last swerve module state applied.
*/
- public SwerveModuleState2 lastState;
+ public SwerveModuleState lastState;
/**
- * Enable {@link SwerveModuleState2} optimizations so the angle is reversed and speed is reversed to ensure the module
+ * Enable {@link SwerveModuleState} optimizations so the angle is reversed and speed is reversed to ensure the module
* never turns more than 90deg.
*/
public boolean moduleStateOptimization = true;
@@ -147,14 +147,12 @@ public class SwerveModule
* @param force Disables optimizations that prevent movement in the angle motor and forces the desired state
* onto the swerve module.
*/
- public void setDesiredState(SwerveModuleState2 desiredState, boolean isOpenLoop, boolean force)
+ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force)
{
if (moduleStateOptimization)
{
- desiredState = SwerveModuleState2.optimize(desiredState,
- Rotation2d.fromDegrees(getAbsolutePosition()),
- lastState,
- configuration.moduleSteerFFCL);
+ desiredState = SwerveModuleState.optimize(desiredState,
+ Rotation2d.fromDegrees(getAbsolutePosition()));
}
if (isOpenLoop)
@@ -181,24 +179,21 @@ public class SwerveModule
{
SmartDashboard.putNumber("Module[" + configuration.name + "] Speed Setpoint:", desiredState.speedMetersPerSecond);
SmartDashboard.putNumber("Module[" + configuration.name + "] Angle Setpoint:", desiredState.angle.getDegrees());
- SmartDashboard.putNumber("Module[" + configuration.name + "] Omega:",
- Math.toDegrees(desiredState.omegaRadPerSecond));
}
// Prevent module rotation if angle is the same as the previous angle.
if (desiredState.angle != lastState.angle || synchronizeEncoderQueued)
{
- double moduleFF = desiredState.omegaRadPerSecond * configuration.moduleSteerFFCL;
// Synchronize encoders if queued and send in the current position as the value from the absolute encoder.
if (absoluteEncoder != null && synchronizeEncoderQueued)
{
double absoluteEncoderPosition = getAbsolutePosition();
angleMotor.setPosition(absoluteEncoderPosition);
- angleMotor.setReference(desiredState.angle.getDegrees(), moduleFF, absoluteEncoderPosition);
+ angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition);
synchronizeEncoderQueued = false;
} else
{
- angleMotor.setReference(desiredState.angle.getDegrees(), moduleFF);
+ angleMotor.setReference(desiredState.angle.getDegrees(), 0);
}
}
@@ -226,7 +221,7 @@ public class SwerveModule
*
* @return Current SwerveModule state.
*/
- public SwerveModuleState2 getState()
+ public SwerveModuleState getState()
{
double velocity;
Rotation2d azimuth;
@@ -240,7 +235,7 @@ public class SwerveModule
{
return simModule.getState();
}
- return new SwerveModuleState2(velocity, azimuth, omega);
+ return new SwerveModuleState(velocity, azimuth);
}
/**
diff --git a/swervelib/math/SwerveDriveOdometry2.java b/swervelib/math/SwerveDriveOdometry2.java
deleted file mode 100644
index 8cc71cd..0000000
--- a/swervelib/math/SwerveDriveOdometry2.java
+++ /dev/null
@@ -1,246 +0,0 @@
-package swervelib.math;
-
-import edu.wpi.first.math.MathSharedStore;
-import edu.wpi.first.math.MathUsageId;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-import org.ejml.simple.SimpleMatrix;
-
-/**
- * Clone of {@link SwerveDriveOdometry} except uses gyro pitch and roll to achieve a more accurate estimation.
- * Originally made by Team 1466.
- */
-public class SwerveDriveOdometry2 extends SwerveDriveOdometry
-{
-
- /**
- * Swerve drive kinematics.
- */
- private final SwerveDriveKinematics m_kinematics;
- /**
- * Number of swerve modules.
- */
- private final int m_numModules;
- /**
- * Previous swerve module positions.
- */
- private final SwerveModulePosition[] m_previousModulePositions;
- /**
- * Zero module states.
- */
- private final SwerveModuleState[] m_zeroModuleStates;
- /**
- * Estimated pose.
- */
- private Pose2d m_poseMeters;
- /**
- * Gyro offset.
- */
- private Rotation2d m_gyroOffset;
- /**
- * Previous gyroscope angle.
- */
- private Rotation2d m_previousAngle;
-
- /**
- * Constructs a SwerveDriveOdometry object.
- *
- * @param kinematics The swerve drive kinematics for your drivetrain.
- * @param gyroAngle The angle reported by the gyroscope.
- * @param modulePositions The wheel positions reported by each module.
- * @param initialPose The starting position of the robot on the field.
- */
- public SwerveDriveOdometry2(
- SwerveDriveKinematics kinematics,
- Rotation2d gyroAngle,
- SwerveModulePosition[] modulePositions,
- Pose2d initialPose)
- {
- super(kinematics, gyroAngle, modulePositions, initialPose);
- m_kinematics = kinematics;
- m_poseMeters = initialPose;
- m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
- m_previousAngle = initialPose.getRotation();
- m_numModules = modulePositions.length;
-
- m_previousModulePositions = new SwerveModulePosition[m_numModules];
- for (int index = 0; index < m_numModules; index++)
- {
- m_previousModulePositions[index] =
- new SwerveModulePosition(
- modulePositions[index].distanceMeters, modulePositions[index].angle);
- }
- m_zeroModuleStates = m_kinematics.toSwerveModuleStates(new ChassisSpeeds(1, 0, 0));
-
- MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
- }
-
- /**
- * Constructs a SwerveDriveOdometry object with the default pose at the origin.
- *
- * @param kinematics The swerve drive kinematics for your drivetrain.
- * @param gyroAngle The angle reported by the gyroscope.
- * @param modulePositions The wheel positions reported by each module.
- */
- public SwerveDriveOdometry2(
- SwerveDriveKinematics kinematics,
- Rotation2d gyroAngle,
- SwerveModulePosition[] modulePositions)
- {
- this(kinematics, gyroAngle, modulePositions, new Pose2d());
- }
-
- /**
- * Resets the robot's position on the field.
- *
- * The gyroscope angle does not need to be reset here on the user's robot code. The library
- * automatically takes care of offsetting the gyro angle.
- *
- *
Similarly, module positions do not need to be reset in user code.
- *
- * @param gyroAngle The angle reported by the gyroscope.
- * @param modulePositions The wheel positions reported by each module.,
- * @param pose The position on the field that your robot is at.
- */
- public void resetPosition(
- Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d pose)
- {
- super.resetPosition(gyroAngle, modulePositions, pose);
- if (modulePositions.length != m_numModules)
- {
- throw new IllegalArgumentException(
- "Number of modules is not consistent with number of wheel locations provided in "
- + "constructor");
- }
-
- m_poseMeters = pose;
- m_previousAngle = pose.getRotation();
- m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
- for (int index = 0; index < m_numModules; index++)
- {
- m_previousModulePositions[index] =
- new SwerveModulePosition(
- modulePositions[index].distanceMeters, modulePositions[index].angle);
- }
- }
-
- /**
- * Returns the position of the robot on the field.
- *
- * @return The pose of the robot (x and y are in meters).
- */
- public Pose2d getPoseMeters()
- {
- return m_poseMeters;
- }
-
- /**
- * Updates the robot's position on the field using forward kinematics and integration of the pose over time. This
- * method automatically calculates the current time to calculate period (difference between two timestamps). The
- * period is used to calculate the change in distance from a velocity. This also takes in an angle parameter which is
- * used instead of the angular rate that is calculated from forward kinematics. This also takes in pitch and roll to
- * allow for more accurate pose estimation on angled surfaces using a rotation matrix.
- *
- * @param gyroYaw The yaw reported by the gyro.
- * @param pitch The pitch reported by the gyro.
- * @param roll The roll reported by the gyro.
- * @param modulePositions The current position of all swerve modules. Please provide the positions in the same order
- * in which you instantiated your SwerveDriveKinematics.
- * @return The new pose of the robot.
- */
- public Pose2d update(
- Rotation2d gyroYaw,
- Rotation2d pitch,
- Rotation2d roll,
- SwerveModulePosition[] modulePositions)
- {
- super.update(gyroYaw, modulePositions);
- if (modulePositions.length != m_numModules)
- {
- throw new IllegalArgumentException(
- "Number of modules is not consistent with number of wheel locations provided in "
- + "constructor");
- }
-
- var moduleDeltas = new SwerveModulePosition[m_numModules];
- var yaw = gyroYaw.plus(m_gyroOffset);
-
- var rotationMatrix = new SimpleMatrix(3, 3);
- var gyroZero = new Rotation2d(); // doing dist calcs robot relative
- rotationMatrix.setRow(
- 0,
- 0,
- gyroZero.getCos() * pitch.getCos(),
- gyroZero.getCos() * pitch.getSin() * roll.getSin() - gyroZero.getSin() * roll.getCos(),
- gyroZero.getCos() * pitch.getSin() * roll.getCos() + gyroZero.getSin() * roll.getSin());
-
- rotationMatrix.setRow(
- 1,
- 0,
- gyroZero.getSin() * pitch.getCos(),
- gyroZero.getSin() * pitch.getSin() * roll.getSin() + gyroZero.getCos() * roll.getCos(),
- gyroZero.getSin() * pitch.getSin() * roll.getCos() - gyroZero.getCos() * roll.getSin());
-
- rotationMatrix.setRow(
- 2, 0, -pitch.getSin(), pitch.getCos() * roll.getSin(), pitch.getCos() * roll.getCos());
-
- for (int index = 0; index < m_numModules; index++)
- {
- var current = modulePositions[index];
- var previous = m_previousModulePositions[index];
-
- var deltaDistanceInitial = current.distanceMeters - previous.distanceMeters;
- var changedAngle =
- current.angle.minus(
- m_zeroModuleStates[index]
- .angle); // does this return (-pi, pi)? shoudln't matter since Twist2d does sin
- // cos
-
- var deltaMatrix = new SimpleMatrix(3, 1);
- deltaMatrix.setColumn(
- 0,
- 0,
- changedAngle.getCos() * deltaDistanceInitial,
- changedAngle.getSin() * deltaDistanceInitial,
- 0);
-
- var rotatedDeltaMatrix = rotationMatrix.mult(deltaMatrix);
- var finalDeltaDistance =
- deltaDistanceInitial >= 0
- ? Math.sqrt(
- Math.pow(rotatedDeltaMatrix.get(0, 0), 2)
- + Math.pow(rotatedDeltaMatrix.get(1, 0), 2))
- : -Math.sqrt(
- Math.pow(rotatedDeltaMatrix.get(0, 0), 2)
- + Math.pow(rotatedDeltaMatrix.get(1, 0), 2));
-
- var deltaDistance =
- (roll.getDegrees() > 10 || pitch.getDegrees() > 10)
- ? finalDeltaDistance
- : deltaDistanceInitial;
- var updatedAngle =
- (roll.getDegrees() > 10 || pitch.getDegrees() > 10)
- ? Rotation2d.fromRadians(
- Math.atan2(rotatedDeltaMatrix.get(0, 0), rotatedDeltaMatrix.get(1, 0)))
- : current.angle;
-
- moduleDeltas[index] = new SwerveModulePosition(deltaDistance, updatedAngle);
- previous.distanceMeters = current.distanceMeters;
- }
-
- var twist = m_kinematics.toTwist2d(moduleDeltas);
- twist.dtheta = yaw.minus(m_previousAngle).getRadians();
-
- var newPose = m_poseMeters.exp(twist);
-
- m_previousAngle = yaw;
- m_poseMeters = new Pose2d(newPose.getTranslation(), yaw);
-
- return m_poseMeters;
- }
-}
diff --git a/swervelib/math/SwerveKinematics2.java b/swervelib/math/SwerveKinematics2.java
deleted file mode 100644
index 0a20372..0000000
--- a/swervelib/math/SwerveKinematics2.java
+++ /dev/null
@@ -1,366 +0,0 @@
-package swervelib.math;
-
-import edu.wpi.first.math.MathSharedStore;
-import edu.wpi.first.math.MathUsageId;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Twist2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.Timer;
-import java.util.Arrays;
-import java.util.Collections;
-import org.ejml.simple.SimpleMatrix;
-
-/**
- * Clone of WPI SwerveKinematics, which implements second order kinematics when calculating modules states from chassis
- * speed. Makes use of {@link SwerveModuleState2} to add the angular velocity that is required of the module as an
- * output.
- */
-public class SwerveKinematics2 extends SwerveDriveKinematics
-{
-
- /**
- * Inverse kinematics matrix.
- */
- private final SimpleMatrix m_inverseKinematics;
- /**
- * Forward kinematics matrix.
- */
- private final SimpleMatrix m_forwardKinematics;
- /**
- * Second order kinematics inverse matrix.
- */
- private final SimpleMatrix bigInverseKinematics;
- /**
- * Number of swerve modules.
- */
- private final int m_numModules;
- /**
- * Location of each swerve module in meters.
- */
- private final Translation2d[] m_modules;
- /**
- * Swerve module states.
- */
- private final SwerveModuleState2[] m_moduleStates;
- private final Timer m_moduleAccelTimer = new Timer();
- /**
- * Previous CoR
- */
- private Translation2d m_prevCoR = new Translation2d();
- private ChassisSpeeds m_prevChassisSpeeds = new ChassisSpeeds();
- private double m_prevModuleAccelTime = 0.0;
-
- /**
- * Constructs a swerve drive kinematics object. This takes in a variable number of wheel locations as Translation2ds.
- * The order in which you pass in the wheel locations is the same order that you will receive the module states when
- * performing inverse kinematics. It is also expected that you pass in the module states in the same order when
- * calling the forward kinematics methods.
- *
- * @param wheelsMeters The locations of the wheels relative to the physical center of the robot.
- */
- public SwerveKinematics2(Translation2d... wheelsMeters)
- {
- super(wheelsMeters);
- if (wheelsMeters.length < 2)
- {
- throw new IllegalArgumentException("A swerve drive requires at least two modules");
- }
- m_numModules = wheelsMeters.length;
- m_modules = Arrays.copyOf(wheelsMeters, m_numModules);
- m_moduleStates = new SwerveModuleState2[m_numModules];
- Arrays.fill(m_moduleStates, new SwerveModuleState2());
- m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
- bigInverseKinematics = new SimpleMatrix(m_numModules * 2, 4);
-
- for (int i = 0; i < m_numModules; i++)
- {
- m_inverseKinematics.setRow(i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getY());
- m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX());
- bigInverseKinematics.setRow(
- i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getX(), -m_modules[i].getY());
- bigInverseKinematics.setRow(
- i * 2 + 1, 0, /* Start Data */ 0, 1, -m_modules[i].getY(), +m_modules[i].getX());
- }
- m_forwardKinematics = m_inverseKinematics.pseudoInverse();
- m_moduleAccelTimer.start();
-
- MathSharedStore.reportUsage(MathUsageId.kKinematics_SwerveDrive, 1);
- }
-
- /**
- * Renormalizes the wheel speeds if any individual speed is above the specified maximum.
- *
- *
Sometimes, after inverse kinematics, the requested speed from one or more modules may be
- * above the max attainable speed for the driving motor on that module. To fix this issue, one can reduce all the
- * wheel speeds to make sure that all requested module speeds are at-or-below the absolute threshold, while
- * maintaining the ratio of speeds between modules.
- *
- * @param moduleStates Reference to array of module states. The array will be mutated with the
- * normalized speeds!
- * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a module can reach.
- */
- public static void desaturateWheelSpeeds(
- SwerveModuleState2[] moduleStates, double attainableMaxSpeedMetersPerSecond)
- {
- double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
- if (realMaxSpeed > attainableMaxSpeedMetersPerSecond)
- {
- for (SwerveModuleState moduleState : moduleStates)
- {
- moduleState.speedMetersPerSecond =
- moduleState.speedMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
- }
- }
- }
-
- /**
- * Renormalizes the wheel speeds if any individual speed is above the specified maximum, as well as getting rid of
- * joystick saturation at edges of joystick.
- *
- *
Sometimes, after inverse kinematics, the requested speed from one or more modules may be
- * above the max attainable speed for the driving motor on that module. To fix this issue, one can reduce all the
- * wheel speeds to make sure that all requested module speeds are at-or-below the absolute threshold, while
- * maintaining the ratio of speeds between modules.
- *
- * @param moduleStates Reference to array of module states. The array will be
- * mutated with the normalized speeds!
- * @param currentChassisSpeed The current speed of the robot
- * @param attainableMaxModuleSpeedMetersPerSecond The absolute max speed that a module can reach
- * @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot can reach while
- * translating
- * @param attainableMaxRotationalVelocityRadiansPerSecond The absolute max speed the robot can reach while rotating
- */
- public static void desaturateWheelSpeeds(
- SwerveModuleState2[] moduleStates,
- ChassisSpeeds currentChassisSpeed,
- double attainableMaxModuleSpeedMetersPerSecond,
- double attainableMaxTranslationalSpeedMetersPerSecond,
- double attainableMaxRotationalVelocityRadiansPerSecond)
- {
- double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
-
- if (attainableMaxTranslationalSpeedMetersPerSecond == 0
- || attainableMaxRotationalVelocityRadiansPerSecond == 0
- || realMaxSpeed == 0)
- {
- return;
- }
- double translationalK =
- Math.hypot(currentChassisSpeed.vxMetersPerSecond, currentChassisSpeed.vyMetersPerSecond)
- / attainableMaxTranslationalSpeedMetersPerSecond;
- double rotationalK =
- Math.abs(currentChassisSpeed.omegaRadiansPerSecond)
- / attainableMaxRotationalVelocityRadiansPerSecond;
- double k = Math.max(translationalK, rotationalK);
- double scale = Math.min(k * attainableMaxModuleSpeedMetersPerSecond / realMaxSpeed, 1);
- for (SwerveModuleState moduleState : moduleStates)
- {
- moduleState.speedMetersPerSecond *= scale;
- }
- }
-
- /**
- * Performs inverse kinematics to return the module states from a desired chassis velocity. This method is often used
- * to convert joystick values into module speeds and angles.
- *
- *
This function also supports variable centers of rotation. During normal operations, the
- * center of rotation is usually the same as the physical center of the robot; therefore, the argument is defaulted to
- * that use case. However, if you wish to change the center of rotation for evasive maneuvers, vision alignment, or
- * for any other use case, you can do so.
- *
- *
In the case that the desired chassis speeds are zero (i.e. the robot will be stationary),
- * the previously calculated module angle will be maintained.
- *
- * @param chassisSpeeds The desired chassis speed.
- * @param centerOfRotationMeters The center of rotation. For example, if you set the center of rotation at one corner
- * of the robot and provide a chassis speed that only has a dtheta component, the robot
- * will rotate around that corner.
- * @return An array containing the module states. Use caution because these module states are not normalized.
- * Sometimes, a user input may cause one of the module speeds to go above the attainable max velocity. Use the
- * {@link #desaturateWheelSpeeds(SwerveModuleState2[], double) DesaturateWheelSpeeds} function to rectify this issue.
- */
- @SuppressWarnings("PMD.MethodReturnsInternalArray")
- public SwerveModuleState2[] toSwerveModuleStates(
- ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters)
- {
- var time = m_moduleAccelTimer.get();
- var dt = time - m_prevModuleAccelTime;
- m_prevModuleAccelTime = time;
-
- var accelChassisSpeeds = new ChassisSpeeds(
- (chassisSpeeds.vxMetersPerSecond - m_prevChassisSpeeds.vxMetersPerSecond) / dt,
- (chassisSpeeds.vyMetersPerSecond - m_prevChassisSpeeds.vyMetersPerSecond) / dt,
- (chassisSpeeds.omegaRadiansPerSecond - m_prevChassisSpeeds.omegaRadiansPerSecond) / dt
- );
- m_prevChassisSpeeds = chassisSpeeds;
-
- if (chassisSpeeds.vxMetersPerSecond == 0.0
- && chassisSpeeds.vyMetersPerSecond == 0.0
- && chassisSpeeds.omegaRadiansPerSecond == 0.0)
- {
- for (int i = 0; i < m_numModules; i++)
- {
- m_moduleStates[i].speedMetersPerSecond = 0.0;
- }
-
- return m_moduleStates;
- }
-
- if (!centerOfRotationMeters.equals(m_prevCoR))
- {
- for (int i = 0; i < m_numModules; i++)
- {
- m_inverseKinematics.setRow(
- i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getY() + centerOfRotationMeters.getY());
- m_inverseKinematics.setRow(
- i * 2 + 1,
- 0, /* Start Data */
- 0,
- 1,
- +m_modules[i].getX() - centerOfRotationMeters.getX());
- bigInverseKinematics.setRow(
- i * 2,
- 0, /* Start Data */
- 1,
- 0,
- -m_modules[i].getX() + centerOfRotationMeters.getX(),
- -m_modules[i].getY() + centerOfRotationMeters.getY());
- bigInverseKinematics.setRow(
- i * 2 + 1,
- 0, /* Start Data */
- 0,
- 1,
- -m_modules[i].getY() + centerOfRotationMeters.getY(),
- +m_modules[i].getX() - centerOfRotationMeters.getX());
- }
- m_prevCoR = centerOfRotationMeters;
- }
-
- var chassisSpeedsVector = new SimpleMatrix(3, 1);
- chassisSpeedsVector.setColumn(
- 0,
- 0,
- chassisSpeeds.vxMetersPerSecond,
- chassisSpeeds.vyMetersPerSecond,
- chassisSpeeds.omegaRadiansPerSecond);
-
- var moduleVelocityStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
-
- var accelerationVector = new SimpleMatrix(4, 1);
- accelerationVector.setColumn(0, 0,
- accelChassisSpeeds.vxMetersPerSecond,
- accelChassisSpeeds.vyMetersPerSecond,
- chassisSpeeds.omegaRadiansPerSecond * chassisSpeeds.omegaRadiansPerSecond,
- accelChassisSpeeds.omegaRadiansPerSecond);
-
- var moduleAccelerationStatesMatrix = bigInverseKinematics.mult(accelerationVector);
-
- for (int i = 0; i < m_numModules; i++)
- {
- double x = moduleVelocityStatesMatrix.get(i * 2, 0);
- double y = moduleVelocityStatesMatrix.get(i * 2 + 1, 0);
-
- double ax = moduleAccelerationStatesMatrix.get(i * 2, 0);
- double ay = moduleAccelerationStatesMatrix.get(i * 2 + 1, 0);
-
- double speed = Math.hypot(x, y);
- Rotation2d angle = new Rotation2d(x, y);
-
- var trigThetaAngle = new SimpleMatrix(2, 2);
- trigThetaAngle.setColumn(0, 0, angle.getCos(), -angle.getSin());
- trigThetaAngle.setColumn(1, 0, angle.getSin(), angle.getCos());
-
- var accelVector = new SimpleMatrix(2, 1);
- accelVector.setColumn(0, 0, ax, ay);
-
- var omegaVector = trigThetaAngle.mult(accelVector);
-
- double omega = (omegaVector.get(1, 0) / speed) - chassisSpeeds.omegaRadiansPerSecond;
- m_moduleStates[i] = new SwerveModuleState2(speed, angle, omega);
- }
-
- return m_moduleStates;
- }
-
- /**
- * Performs inverse kinematics. See {@link #toSwerveModuleStates(ChassisSpeeds, Translation2d)} toSwerveModuleStates
- * for more information.
- *
- * @param chassisSpeeds The desired chassis speed.
- * @return An array containing the module states.
- */
- public SwerveModuleState2[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds)
- {
- return toSwerveModuleStates(chassisSpeeds, new Translation2d());
- }
-
- /**
- * Performs forward kinematics to return the resulting chassis state from the given module states. This method is
- * often used for odometry -- determining the robot's position on the field using data from the real-world speed and
- * angle of each module on the robot.
- *
- * @param wheelStates The state of the modules (as a SwerveModuleState type) as measured from respective encoders and
- * gyros. The order of the swerve module states should be same as passed into the constructor of
- * this class.
- * @return The resulting chassis speed.
- */
- public ChassisSpeeds toChassisSpeeds(SwerveModuleState2... wheelStates)
- {
- if (wheelStates.length != m_numModules)
- {
- throw new IllegalArgumentException(
- "Number of modules is not consistent with number of wheel locations provided in "
- + "constructor");
- }
- var moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1);
-
- for (int i = 0; i < m_numModules; i++)
- {
- var module = wheelStates[i];
- moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos());
- moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin());
- }
-
- var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix);
- return new ChassisSpeeds(
- chassisSpeedsVector.get(0, 0),
- chassisSpeedsVector.get(1, 0),
- chassisSpeedsVector.get(2, 0));
- }
-
- /**
- * Performs forward kinematics to return the resulting chassis state from the given module states. This method is
- * often used for odometry -- determining the robot's position on the field using data from the real-world speed and
- * angle of each module on the robot.
- *
- * @param wheelDeltas The latest change in position of the modules (as a SwerveModulePosition type) as measured from
- * respective encoders and gyros. The order of the swerve module states should be same as passed
- * into the constructor of this class.
- * @return The resulting Twist2d.
- */
- public Twist2d toTwist2d(SwerveModulePosition... wheelDeltas)
- {
- if (wheelDeltas.length != m_numModules)
- {
- throw new IllegalArgumentException(
- "Number of modules is not consistent with number of wheel locations provided in "
- + "constructor");
- }
- var moduleDeltaMatrix = new SimpleMatrix(m_numModules * 2, 1);
-
- for (int i = 0; i < m_numModules; i++)
- {
- var module = wheelDeltas[i];
- moduleDeltaMatrix.set(i * 2, 0, module.distanceMeters * module.angle.getCos());
- moduleDeltaMatrix.set(i * 2 + 1, module.distanceMeters * module.angle.getSin());
- }
-
- var chassisDeltaVector = m_forwardKinematics.mult(moduleDeltaMatrix);
- return new Twist2d(
- chassisDeltaVector.get(0, 0), chassisDeltaVector.get(1, 0), chassisDeltaVector.get(2, 0));
- }
-}
diff --git a/swervelib/math/SwerveMath.java b/swervelib/math/SwerveMath.java
index 4661348..e524ff3 100644
--- a/swervelib/math/SwerveMath.java
+++ b/swervelib/math/SwerveMath.java
@@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.List;
import swervelib.SwerveController;
@@ -365,16 +366,15 @@ public class SwerveMath
/**
* Perform anti-jitter within modules if the speed requested is too low.
*
- * @param moduleState Current {@link SwerveModuleState2} requested.
- * @param lastModuleState Previous {@link SwerveModuleState2} used.
+ * @param moduleState Current {@link SwerveModuleState} requested.
+ * @param lastModuleState Previous {@link SwerveModuleState} used.
* @param maxSpeed Maximum speed of the modules, should be in {@link SwerveDriveConfiguration#maxSpeed}.
*/
- public static void antiJitter(SwerveModuleState2 moduleState, SwerveModuleState2 lastModuleState, double maxSpeed)
+ public static void antiJitter(SwerveModuleState moduleState, SwerveModuleState lastModuleState, double maxSpeed)
{
if (Math.abs(moduleState.speedMetersPerSecond) <= (maxSpeed * 0.01))
{
moduleState.angle = lastModuleState.angle;
- moduleState.omegaRadPerSecond = lastModuleState.omegaRadPerSecond;
}
}
}
diff --git a/swervelib/math/SwerveModuleState2.java b/swervelib/math/SwerveModuleState2.java
deleted file mode 100644
index f4497dc..0000000
--- a/swervelib/math/SwerveModuleState2.java
+++ /dev/null
@@ -1,157 +0,0 @@
-package swervelib.math;
-
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-
-/**
- * Second order kinematics swerve module state.
- */
-public class SwerveModuleState2 extends SwerveModuleState
-{
-
- /**
- * Rad per sec
- */
- public double omegaRadPerSecond = 0;
-
- /**
- * Constructs a SwerveModuleState with zeros for speed and angle.
- */
- public SwerveModuleState2()
- {
- super();
- }
-
- /**
- * Constructs a SwerveModuleState.
- *
- * @param speedMetersPerSecond The speed of the wheel of the module.
- * @param angle The angle of the module.
- * @param omegaRadPerSecond The angular velocity of the module.
- */
- public SwerveModuleState2(
- double speedMetersPerSecond, Rotation2d angle, double omegaRadPerSecond)
- {
- super(speedMetersPerSecond, angle);
- this.omegaRadPerSecond = omegaRadPerSecond;
- }
-
- /**
- * Create a {@link SwerveModuleState2} based on the {@link SwerveModuleState} with the radians per second defined.
- *
- * @param state First order kinematic module state.
- * @param omegaRadPerSecond Module wheel angular rotation in radians per second.
- */
- public SwerveModuleState2(SwerveModuleState state, double omegaRadPerSecond)
- {
- super(state.speedMetersPerSecond, state.angle);
- this.omegaRadPerSecond = omegaRadPerSecond;
- }
-
- /**
- * Minimize the change in heading the desired swerve module state would require by potentially reversing the direction
- * the wheel spins. If this is used with the PIDController class's continuous input functionality, the furthest a
- * wheel will ever rotate is 90 degrees.
- *
- * @param desiredState The desired state.
- * @param currentAngle The current module angle.
- * @param lastState The last state of the module.
- * @param moduleSteerFeedForwardClosedLoop The module feed forward closed loop for the angle motor.
- * @return Optimized swerve module state.
- */
- public static SwerveModuleState2 optimize(SwerveModuleState2 desiredState, Rotation2d currentAngle,
- SwerveModuleState2 lastState, double moduleSteerFeedForwardClosedLoop)
- {
- SwerveModuleState2 optimized = new SwerveModuleState2(SwerveModuleState.optimize(desiredState, currentAngle),
- desiredState.omegaRadPerSecond);
- if (desiredState.angle.equals(currentAngle) || desiredState.angle.equals(
- optimized.angle.rotateBy(Rotation2d.fromDegrees(90))) || moduleSteerFeedForwardClosedLoop == 0)
- {
- optimized.omegaRadPerSecond = 0;
- }
- if (desiredState.angle.equals(optimized.angle.rotateBy(Rotation2d.fromDegrees(90))))
- {
- desiredState.omegaRadPerSecond = 0;
- return desiredState;
- }
- return optimized;
- /*
- // NEVER optimize if it's the same angle, it just doesn't make sense...
- if (currentAngle.equals(desiredState.angle.rotateBy(Rotation2d.fromDegrees(180))))
- {
- desiredState.invert();
- desiredState.omegaRadPerSecond = 0;
- return desiredState;
- } else if (currentAngle.equals(desiredState.angle))
- {
- desiredState.omegaRadPerSecond = 0;
- return desiredState;
- }
-
- SwerveModuleState2 optimized;
- if (moduleSteerFeedForwardClosedLoop == 0)
- {
-// desiredState.angle = desiredState.angle.plus(Rotation2d.fromRadians(
-// lastState.omegaRadPerSecond * moduleSteerFeedForwardClosedLoop * 0.065));
-// return new SwerveModuleState2(SwerveModuleState.optimize(desiredState, currentAngle),
-// desiredState.omegaRadPerSecond);
- optimized = new SwerveModuleState2(SwerveModuleState.optimize(desiredState, currentAngle), 0);
-// return desiredState;
- } else
- {
- Rotation2d delta = desiredState.angle.plus(Rotation2d.fromRadians(lastState.omegaRadPerSecond *
- moduleSteerFeedForwardClosedLoop *
- 0.065))
- .minus(currentAngle);
- if (Double.compare(Math.abs(delta.getDegrees()), 90.0) < 0)
- {
- optimized = desiredState.invert();
- optimized.angle = optimized.angle.plus(Rotation2d.fromRadians(lastState.omegaRadPerSecond *
- moduleSteerFeedForwardClosedLoop *
- 0.065));
- } else
- {
- optimized = desiredState;
- }
- }
-
- if (Double.compare(Math.abs(currentAngle.minus(optimized.angle).getDegrees()),
- Math.abs(currentAngle.minus(desiredState.angle)
- .getDegrees())) > 0)
- {
- desiredState.omegaRadPerSecond = 0;
- return desiredState;
- }
-
- // Maybe the omega rad per second will always be off when optimized?
-// optimized.omegaRadPerSecond = 0;
- return optimized;
- */
- }
-
- /**
- * Invert the swerve module state.
- *
- * @return Current inverted {@link SwerveModuleState2}
- */
- public SwerveModuleState2 invert()
- {
-// omegaRadPerSecond *= -1;
-// speedMetersPerSecond *= -1;
-// angle = angle.rotateBy(Rotation2d.fromDegrees(180));
-// return this;
- return new SwerveModuleState2(-speedMetersPerSecond,
- angle.rotateBy(Rotation2d.fromDegrees(180)),
- -omegaRadPerSecond);
- }
-
- /**
- * Convert to a {@link SwerveModuleState}.
- *
- * @return {@link SwerveModuleState} with the same angle and speed.
- */
- public SwerveModuleState toSwerveModuleState()
- {
- return new SwerveModuleState(this.speedMetersPerSecond, this.angle);
- }
-}
diff --git a/swervelib/math/SwervePoseEstimator2.java b/swervelib/math/SwervePoseEstimator2.java
deleted file mode 100644
index b61e3e6..0000000
--- a/swervelib/math/SwervePoseEstimator2.java
+++ /dev/null
@@ -1,466 +0,0 @@
-package swervelib.math;
-
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Twist2d;
-import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.math.numbers.N1;
-import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.util.WPIUtilJNI;
-import java.util.Arrays;
-import java.util.Map;
-import java.util.Objects;
-
-/**
- * Clone of {@link SwerveDrivePoseEstimator} which takes into account gyroscope pitch and roll to achieve a more
- * accurate estimation, originally made by Team 1466.
- */
-public class SwervePoseEstimator2 // extends SwerveDrivePoseEstimator
-{
-
- /**
- * Swerve drive kinematics.
- */
- private final SwerveDriveKinematics m_kinematics;
- /**
- * Enhanced swerve drive odometry.
- */
- private final SwerveDriveOdometry2 m_odometry;
- /**
- * Matrix quotient.
- */
- private final Matrix m_q = new Matrix<>(Nat.N3(), Nat.N1());
- /**
- * Number of swerve modules.
- */
- private final int m_numModules;
- /**
- * Interpolation buffer.
- */
- private final TimeInterpolatableBuffer m_poseBuffer = TimeInterpolatableBuffer.createBuffer(1.5);
- /**
- * Vision standard deviations.
- */
- private final Matrix m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
-
- /**
- * Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision measurements.
- *
- * The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y,
- * and 0.1 radians for heading. The default standard deviations of the vision measurements are 0.9 meters for x, 0.9
- * meters for y, and 0.9 radians for heading.
- *
- * @param kinematics A correctly-configured kinematics object for your drivetrain.
- * @param gyroAngle The current gyro angle.
- * @param modulePositions The current distance measurements and rotations of the swerve modules.
- * @param initialPoseMeters The starting pose estimate.
- */
- public SwervePoseEstimator2(
- SwerveDriveKinematics kinematics,
- Rotation2d gyroAngle,
- SwerveModulePosition[] modulePositions,
- Pose2d initialPoseMeters)
- {
- this(
- kinematics,
- gyroAngle,
- modulePositions,
- initialPoseMeters,
- VecBuilder.fill(0.1, 0.1, 0.1),
- VecBuilder.fill(0.9, 0.9, 0.9));
- }
-
- /**
- * Constructs a SwerveDrivePoseEstimator.
- *
- * @param kinematics A correctly-configured kinematics object for your drivetrain.
- * @param gyroAngle The current gyro angle.
- * @param modulePositions The current distance and rotation measurements of the swerve modules.
- * @param initialPoseMeters The starting pose estimate.
- * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position in
- * meters, and heading in radians). Increase these numbers to trust your state
- * estimate less.
- * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position in meters, y
- * position in meters, and heading in radians). Increase these numbers to trust the
- * vision pose measurement less.
- */
- public SwervePoseEstimator2(
- SwerveDriveKinematics kinematics,
- Rotation2d gyroAngle,
- SwerveModulePosition[] modulePositions,
- Pose2d initialPoseMeters,
- Matrix stateStdDevs,
- Matrix visionMeasurementStdDevs)
- {
-// super(kinematics, gyroAngle, modulePositions, initialPoseMeters);
- m_kinematics = kinematics;
- m_odometry =
- new SwerveDriveOdometry2(kinematics, gyroAngle, modulePositions, initialPoseMeters);
-
- for (int i = 0; i < 3; ++i)
- {
- m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
- }
-
- m_numModules = modulePositions.length;
-
- setVisionMeasurementStdDevs(visionMeasurementStdDevs);
- }
-
- /**
- * Sets the pose estimator's trust of global measurements. This might be used to change trust in vision measurements
- * after the autonomous period, or to change trust as distance to a vision target increases.
- *
- * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these numbers to trust
- * global measurements from vision less. This matrix is in the form [x, y, theta]^T,
- * with units in meters and radians.
- */
- public void setVisionMeasurementStdDevs(Matrix visionMeasurementStdDevs)
- {
-// super.setVisionMeasurementStdDevs(visionMeasurementStdDevs);
- var r = new double[3];
- for (int i = 0; i < 3; ++i)
- {
- r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
- }
-
- // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
- // and C = I. See wpimath/algorithms.md.
- for (int row = 0; row < 3; ++row)
- {
- if (m_q.get(row, 0) == 0.0)
- {
- m_visionK.set(row, row, 0.0);
- } else
- {
- m_visionK.set(
- row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
- }
- }
- }
-
- /**
- * Resets the robot's position on the field.
- *
- * The gyroscope angle does not need to be reset in the user's robot code. The library
- * automatically takes care of offsetting the gyro angle.
- *
- * @param gyroAngle The angle reported by the gyroscope.
- * @param modulePositions The current distance measurements and rotations of the swerve modules.
- * @param poseMeters The position on the field that your robot is at.
- */
- public void resetPosition(
- Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters)
- {
-// super.resetPosition(gyroAngle, modulePositions, poseMeters);
- // Reset state estimate and error covariance
- m_odometry.resetPosition(gyroAngle, modulePositions, poseMeters);
- m_poseBuffer.clear();
- }
-
- /**
- * Gets the estimated robot pose.
- *
- * @return The estimated robot pose in meters.
- */
- public Pose2d getEstimatedPosition()
- {
- return m_odometry.getPoseMeters();
- }
-
- /**
- * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate while still accounting
- * for measurement noise.
- *
- *
This method can be called as infrequently as you want, as long as you are calling {@link
- * SwerveDrivePoseEstimator#update} every loop.
- *
- *
To promote stability of the pose estimate and make it robust to bad vision data, we
- * recommend only adding vision measurements that are already within one meter or so of the current pose estimate.
- *
- * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
- * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you don't use your
- * own time source by calling
- * {@link SwerveDrivePoseEstimator#updateWithTime(double, Rotation2d,
- * SwerveModulePosition[])} then you must use a timestamp with an epoch since FPGA
- * startup (i.e., the epoch of this timestamp is the same epoch as
- * {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that you should
- * use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source or sync
- * the epochs.
- */
- public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds)
- {
-// super.addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
- // Step 1: Get the pose odometry measured at the moment the vision measurement was made.
- var sample = m_poseBuffer.getSample(timestampSeconds);
-
- if (sample.isEmpty())
- {
- return;
- }
-
- // Step 2: Measure the twist between the odometry pose and the vision pose.
- var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
-
- // Step 3: We should not trust the twist entirely, so instead we scale this twist by a Kalman
- // gain matrix representing how much we trust vision measurements compared to our current pose.
- var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
-
- // Step 4: Convert back to Twist2d.
- var scaledTwist =
- new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
-
- // Step 5: Reset Odometry to state at sample with vision adjustment.
- m_odometry.resetPosition(
- sample.get().gyroAngle,
- sample.get().modulePositions,
- sample.get().poseMeters.exp(scaledTwist));
-
- // Step 6: Record the current pose to allow multiple measurements from the same timestamp
-// m_poseBuffer.addSample(
-// timestampSeconds,
-// new InterpolationRecord(
-// getEstimatedPosition(), sample.get().gyroAngle, sample.get().gyroPitch, sample.get().gyroRoll,
-// sample.get().modulePositions));
-
- // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
- // pose buffer and correct odometry.
- for (Map.Entry entry :
- m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet())
- {
- updateWithTime(
- entry.getKey(),
- entry.getValue().gyroAngle,
- entry.getValue().gyroPitch,
- entry.getValue().gyroRoll,
- entry.getValue().modulePositions);
- }
- }
-
- /**
- * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate while still accounting
- * for measurement noise.
- *
- * This method can be called as infrequently as you want, as long as you are calling {@link
- * SwerveDrivePoseEstimator#update} every loop.
- *
- *
To promote stability of the pose estimate and make it robust to bad vision data, we
- * recommend only adding vision measurements that are already within one meter or so of the current pose estimate.
- *
- *
Note that the vision measurement standard deviations passed into this method will continue
- * to apply to future measurements until a subsequent call to
- * {@link SwerveDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
- *
- * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
- * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you don't use your
- * own time source by calling
- * {@link SwerveDrivePoseEstimator#updateWithTime(double, Rotation2d,
- * SwerveModulePosition[])}, then you must use a timestamp with an epoch since FPGA
- * startup (i.e., the epoch of this timestamp is the same epoch as
- * {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you should
- * use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source in
- * this case.
- * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position in meters, y
- * position in meters, and heading in radians). Increase these numbers to trust the
- * vision pose measurement less.
- */
- public void addVisionMeasurement(
- Pose2d visionRobotPoseMeters,
- double timestampSeconds,
- Matrix visionMeasurementStdDevs)
- {
- setVisionMeasurementStdDevs(visionMeasurementStdDevs);
- addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
- }
-
- /**
- * Updates the pose estimator with wheel encoder and gyro information. This should be called every loop.
- *
- * @param gyroAngle The current gyro angle.
- * @param gyroPitch The current gyro pitch.
- * @param gyroRoll The current gyro roll.
- * @param modulePositions The current distance measurements and rotations of the swerve modules.
- * @return The estimated pose of the robot in meters.
- */
- public Pose2d update(
- Rotation2d gyroAngle,
- Rotation2d gyroPitch,
- Rotation2d gyroRoll,
- SwerveModulePosition[] modulePositions)
- {
- return updateWithTime(
- WPIUtilJNI.now() * 1.0e-6, gyroAngle, gyroPitch, gyroRoll, modulePositions);
- }
-
- /**
- * Updates the pose estimator with wheel encoder and gyro information. This should be called every loop.
- *
- * @param currentTimeSeconds Time at which this method was called, in seconds.
- * @param gyroAngle The current gyro angle.
- * @param gyroPitch The current gyro pitch.
- * @param gyroRoll The current gyro roll.
- * @param modulePositions The current distance measurements and rotations of the swerve modules.
- * @return The estimated pose of the robot in meters.
- */
- public Pose2d updateWithTime(
- double currentTimeSeconds,
- Rotation2d gyroAngle,
- Rotation2d gyroPitch,
- Rotation2d gyroRoll,
- SwerveModulePosition[] modulePositions)
- {
-// super.updateWithTime(currentTimeSeconds, gyroAngle, modulePositions);
- if (modulePositions.length != m_numModules)
- {
- throw new IllegalArgumentException(
- "Number of modules is not consistent with number of wheel locations provided in "
- + "constructor");
- }
-
- var internalModulePositions = new SwerveModulePosition[m_numModules];
-
- for (int i = 0; i < m_numModules; i++)
- {
- internalModulePositions[i] =
- new SwerveModulePosition(modulePositions[i].distanceMeters, modulePositions[i].angle);
- }
-
- m_odometry.update(gyroAngle, gyroPitch, gyroRoll, internalModulePositions);
-
- m_poseBuffer.addSample(
- currentTimeSeconds,
- new InterpolationRecord(
- getEstimatedPosition(), gyroAngle, gyroPitch, gyroRoll, internalModulePositions));
-
- return getEstimatedPosition();
- }
-
- /**
- * Represents an odometry record. The record contains the inputs provided as well as the pose that was observed based
- * on these inputs, as well as the previous record and its inputs.
- */
- private class InterpolationRecord implements Interpolatable
- {
-
- // The pose observed given the current sensor inputs and the previous pose.
- private final Pose2d poseMeters;
-
- // The current gyro angle.
- private final Rotation2d gyroAngle;
-
- // The current gyro pitch.
- private final Rotation2d gyroPitch;
-
- // The current gyro roll.
- private final Rotation2d gyroRoll;
-
- // The distances and rotations measured at each module.
- private final SwerveModulePosition[] modulePositions;
-
- /**
- * Constructs an Interpolation Record with the specified parameters.
- *
- * @param poseMeters The pose observed given the current sensor inputs and the previous pose.
- * @param gyro The current gyro angle.
- * @param gyroPitch The current gyro pitch.
- * @param gyroRoll The current gyro roll.
- * @param modulePositions The distances and rotations measured at each wheel.
- */
- private InterpolationRecord(
- Pose2d poseMeters,
- Rotation2d gyro,
- Rotation2d gyroPitch,
- Rotation2d gyroRoll,
- SwerveModulePosition[] modulePositions)
- {
- this.poseMeters = poseMeters;
- this.gyroAngle = gyro;
- this.gyroPitch = gyroPitch;
- this.gyroRoll = gyroRoll;
- this.modulePositions = modulePositions;
- }
-
- /**
- * Return the interpolated record. This object is assumed to be the starting position, or lower bound.
- *
- * @param endValue The upper bound, or end.
- * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
- * @return The interpolated value.
- */
- @Override
- public InterpolationRecord interpolate(InterpolationRecord endValue, double t)
- {
- if (t < 0)
- {
- return this;
- } else if (t >= 1)
- {
- return endValue;
- } else
- {
- // Find the new wheel distances.
- var modulePositions = new SwerveModulePosition[m_numModules];
-
- // Find the distance travelled between this measurement and the interpolated measurement.
- var moduleDeltas = new SwerveModulePosition[m_numModules];
-
- for (int i = 0; i < m_numModules; i++)
- {
- double ds =
- MathUtil.interpolate(
- this.modulePositions[i].distanceMeters,
- endValue.modulePositions[i].distanceMeters,
- t);
- Rotation2d theta =
- this.modulePositions[i].angle.interpolate(endValue.modulePositions[i].angle, t);
- modulePositions[i] = new SwerveModulePosition(ds, theta);
- moduleDeltas[i] =
- new SwerveModulePosition(ds - this.modulePositions[i].distanceMeters, theta);
- }
-
- // Find the new gyro angle.
- var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
- var gyroPitch_lerp = gyroPitch.interpolate(endValue.gyroPitch, t);
- var gyroRoll_lerp = gyroRoll.interpolate(endValue.gyroRoll, t);
-
- // Create a twist to represent this change based on the interpolated sensor inputs.
- Twist2d twist = m_kinematics.toTwist2d(moduleDeltas);
- twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
-
- return new InterpolationRecord(
- poseMeters.exp(twist), gyro_lerp, gyroPitch_lerp, gyroRoll_lerp, modulePositions);
- }
- }
-
- @Override
- public boolean equals(Object obj)
- {
- if (this == obj)
- {
- return true;
- }
- if (!(obj instanceof InterpolationRecord))
- {
- return false;
- }
- InterpolationRecord record = (InterpolationRecord) obj;
- return Objects.equals(gyroAngle, record.gyroAngle)
- && Arrays.equals(modulePositions, record.modulePositions)
- && Objects.equals(poseMeters, record.poseMeters);
- }
-
- @Override
- public int hashCode()
- {
- return Objects.hash(gyroAngle, Arrays.hashCode(modulePositions), poseMeters);
- }
- }
-}
diff --git a/swervelib/motors/SparkMaxSwerve.java b/swervelib/motors/SparkMaxSwerve.java
index 8daece1..fadc733 100644
--- a/swervelib/motors/SparkMaxSwerve.java
+++ b/swervelib/motors/SparkMaxSwerve.java
@@ -269,11 +269,13 @@ public class SparkMaxSwerve extends SwerveMotor
* Save the configurations from flash to EEPROM.
*/
@Override
- public void burnFlash()
+ public void burnFlash()
{
- try {
+ try
+ {
Thread.sleep(200);
- } catch (Exception e) {
+ } catch (Exception e)
+ {
}
motor.burnFlash();
}
diff --git a/swervelib/parser/SwerveModuleConfiguration.java b/swervelib/parser/SwerveModuleConfiguration.java
index 1c5192f..4fd2a94 100644
--- a/swervelib/parser/SwerveModuleConfiguration.java
+++ b/swervelib/parser/SwerveModuleConfiguration.java
@@ -3,6 +3,7 @@ package swervelib.parser;
import static swervelib.math.SwerveMath.calculateDegreesPerSteeringRotation;
import static swervelib.math.SwerveMath.calculateMaxAcceleration;
import static swervelib.math.SwerveMath.calculateMetersPerRotation;
+
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Translation2d;
import swervelib.encoders.SwerveAbsoluteEncoder;
diff --git a/swervelib/simulation/SwerveIMUSimulation.java b/swervelib/simulation/SwerveIMUSimulation.java
index 949bda7..b3f2b62 100644
--- a/swervelib/simulation/SwerveIMUSimulation.java
+++ b/swervelib/simulation/SwerveIMUSimulation.java
@@ -4,11 +4,11 @@ import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import java.util.Optional;
-import swervelib.math.SwerveKinematics2;
-import swervelib.math.SwerveModuleState2;
/**
* Simulation for {@link swervelib.SwerveDrive} IMU.
@@ -93,17 +93,18 @@ public class SwerveIMUSimulation
* Update the odometry of the simulated {@link swervelib.SwerveDrive} and post the {@link swervelib.SwerveModule}
* states to the {@link Field2d}.
*
- * @param kinematics {@link SwerveKinematics2} of the swerve drive.
- * @param states {@link SwerveModuleState2} array of the module states.
+ * @param kinematics {@link SwerveDriveKinematics} of the swerve drive.
+ * @param states {@link SwerveModuleState} array of the module states.
* @param modulePoses {@link Pose2d} representing the swerve modules.
* @param field {@link Field2d} to update.
*/
public void updateOdometry(
- SwerveKinematics2 kinematics,
- SwerveModuleState2[] states,
+ SwerveDriveKinematics kinematics,
+ SwerveModuleState[] states,
Pose2d[] modulePoses,
Field2d field)
{
+
angle += kinematics.toChassisSpeeds(states).omegaRadiansPerSecond * (timer.get() - lastTime);
lastTime = timer.get();
field.getObject("XModules").setPoses(modulePoses);
diff --git a/swervelib/simulation/SwerveModuleSimulation.java b/swervelib/simulation/SwerveModuleSimulation.java
index fab142e..2fb06f8 100644
--- a/swervelib/simulation/SwerveModuleSimulation.java
+++ b/swervelib/simulation/SwerveModuleSimulation.java
@@ -2,8 +2,8 @@ package swervelib.simulation;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.Timer;
-import swervelib.math.SwerveModuleState2;
/**
* Class to hold simulation data for {@link swervelib.SwerveModule}
@@ -15,27 +15,27 @@ public class SwerveModuleSimulation
/**
* Main timer to simulate the passage of time.
*/
- private final Timer timer;
+ private final Timer timer;
/**
* Time delta since last update
*/
- private double dt;
+ private double dt;
/**
* Fake motor position.
*/
- private double fakePos;
+ private double fakePos;
/**
* The fake speed of the previous state, used to calculate {@link SwerveModuleSimulation#fakePos}.
*/
- private double fakeSpeed;
+ private double fakeSpeed;
/**
* Last time queried.
*/
- private double lastTime;
+ private double lastTime;
/**
* Current simulated swerve module state.
*/
- private SwerveModuleState2 state;
+ private SwerveModuleState state;
/**
* Create simulation class and initialize module at 0.
@@ -45,7 +45,7 @@ public class SwerveModuleSimulation
timer = new Timer();
timer.start();
lastTime = timer.get();
- state = new SwerveModuleState2(0, Rotation2d.fromDegrees(0), 0);
+ state = new SwerveModuleState(0, Rotation2d.fromDegrees(0));
fakeSpeed = 0;
fakePos = 0;
dt = 0;
@@ -57,7 +57,7 @@ public class SwerveModuleSimulation
*
* @param desiredState State the swerve module is set to.
*/
- public void updateStateAndPosition(SwerveModuleState2 desiredState)
+ public void updateStateAndPosition(SwerveModuleState desiredState)
{
dt = timer.get() - lastTime;
lastTime = timer.get();
@@ -76,16 +76,15 @@ public class SwerveModuleSimulation
public SwerveModulePosition getPosition()
{
- return new SwerveModulePosition(
- fakePos, state.angle.plus(new Rotation2d(state.omegaRadPerSecond * dt)));
+ return new SwerveModulePosition(fakePos, state.angle);
}
/**
- * Get the {@link SwerveModuleState2} of the simulated module.
+ * Get the {@link SwerveModuleState} of the simulated module.
*
- * @return {@link SwerveModuleState2} of the simulated module.
+ * @return {@link SwerveModuleState} of the simulated module.
*/
- public SwerveModuleState2 getState()
+ public SwerveModuleState getState()
{
return state;
}