From ab0507ec1ee6ee22b24da1383e32628f80bc0494 Mon Sep 17 00:00:00 2001 From: jace rodgers Date: Tue, 14 Jan 2025 13:54:05 -0600 Subject: [PATCH] changed swerve module conversions --- src/main/java/frc/lib/util/swerve/SwerveModule.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index 1ed3a02..957d781 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.lib.math.Conversions; import frc.robot.Constants; /** @@ -97,8 +98,9 @@ public Rotation2d getCANcoder() { */ public SwerveModuleState getState() { return new SwerveModuleState( - inputs.driveMotorSelectedSensorVelocity.in(RotationsPerSecond) - * Constants.Swerve.wheelCircumference.in(Meters), + Conversions.rotationPerSecondToMetersPerSecond( + inputs.driveMotorSelectedSensorVelocity.in(RotationsPerSecond), + Constants.Swerve.wheelCircumference.in(Meters)), Rotation2d.fromRotations(inputs.angleMotorSelectedPosition.in(Rotations))); } @@ -109,8 +111,9 @@ public SwerveModuleState getState() { */ public SwerveModulePosition getPosition() { return new SwerveModulePosition( - inputs.driveMotorSelectedPosition.in(Rotations) - * Constants.Swerve.wheelCircumference.in(Meters), + Conversions.rotationsToMeters( + inputs.driveMotorSelectedSensorVelocity.in(RotationsPerSecond), + Constants.Swerve.wheelCircumference.in(Meters)), Rotation2d.fromRotations(inputs.angleMotorSelectedPosition.in(Rotations))); } }