use units lib when getting angle from abs encoder (#4)

TripleHelixProgramming · Oct 18, 2024 · 3a03f52 · 3a03f52
1 parent 0d9111b
commit 3a03f52
Showing 1 changed file with 5 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/drivetrain/SwerveModule.java b/src/main/java/frc/robot/drivetrain/SwerveModule.java
@@ -15,6 +15,7 @@
 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.units.measure.Angle;
 import edu.wpi.first.wpilibj.Preferences;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.ModuleConstants;
@@ -143,9 +144,9 @@ public void setDesiredState(SwerveModuleState desiredState) {
     desiredState.cosineScale(encoderRotation);
 
     m_drivePIDController.setReference(
-        state.speedMetersPerSecond, CANSparkMax.ControlType.kVelocity);
+        desiredState.speedMetersPerSecond, CANSparkMax.ControlType.kVelocity);
     m_turningPIDController.setReference(
-        state.angle.getRotations(), CANSparkMax.ControlType.kPosition);
+        desiredState.angle.getRotations(), CANSparkMax.ControlType.kPosition);
   }
 
   public void resetDriveEncoder() {
@@ -172,7 +173,7 @@ public Rotation2d getRelativeTurningPosition() {
    * @return The absolute turning angle of the module
    */
   public Rotation2d getAbsTurningPosition(double waitPeriod) {
-    double absPositonRotations;
+    Angle absPositonRotations;
 
     if (waitPeriod > 0.0) {
       absPositonRotations =
@@ -181,7 +182,7 @@ public Rotation2d getAbsTurningPosition(double waitPeriod) {
       absPositonRotations = m_turningAbsEncoder.getAbsolutePosition().getValue();
     }
 
-    return Rotation2d.fromRotations(absPositonRotations);
+    return new Rotation2d(absPositonRotations);
   }
 
   /**