From fa409fac81df997dacc92490f301d5031816c53f Mon Sep 17 00:00:00 2001
From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com>
Date: Sun, 28 Jan 2024 13:29:53 -0800
Subject: [PATCH] switched degrees to rotations

---
 .../robot/subsystems/drive/module/Module.java |  4 +--
 .../subsystems/drive/module/ModuleIO.java     |  2 +-
 .../subsystems/drive/module/ModuleIOSim.java  |  2 +-
 .../drive/module/ModuleIOTalonFX.java         | 26 ++++++-------------
 4 files changed, 12 insertions(+), 22 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/drive/module/Module.java b/src/main/java/frc/robot/subsystems/drive/module/Module.java
index 7ecf28b4..0358b95e 100644
--- a/src/main/java/frc/robot/subsystems/drive/module/Module.java
+++ b/src/main/java/frc/robot/subsystems/drive/module/Module.java
@@ -90,7 +90,7 @@ public void periodic() {
 
     // Run closed loop turn control
     if (m_angleSetpoint != null) {
-      m_io.setTurnPositionDegs(m_angleSetpoint.minus(m_turnRelativeOffset).getRotations());
+      m_io.setTurnPositionRots(m_angleSetpoint.minus(m_turnRelativeOffset).getRotations());
 
       // Run closed loop drive control
       // Only allowed if closed loop turn control is running
@@ -149,7 +149,7 @@ public void runCharacterization(double volts) {
 
   /** Disables all outputs to motors. */
   public void stop() {
-    m_io.setTurnPositionDegs(0.0);
+    m_io.setTurnPositionRots(0.0);
     m_io.setDriveVelocityMPS(0.0);
 
     // Disable closed loop control for turn and drive
diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java
index 6b5bac51..d79dab59 100644
--- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java
+++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java
@@ -129,7 +129,7 @@ default void updateInputs(ModuleIOInputs inputs) {}
   default void setDriveVelocityMPS(double mps) {}
 
   /** Run the turn motor at the specified voltage. */
-  default void setTurnPositionDegs(double degrees) {}
+  default void setTurnPositionRots(double degrees) {}
 
   /** Enable or disable brake mode on the drive motor. */
   default void setDriveBrakeMode(boolean enable) {}
diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java
index b3a15eb7..1a7ab440 100644
--- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java
+++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java
@@ -104,7 +104,7 @@ public void setDriveVelocityMPS(double mps) {
   }
 
   @Override
-  public void setTurnPositionDegs(double degrees) {
+  public void setTurnPositionRots(double degrees) {
     double rots = degrees / 360;
     double volts = m_turnPid.calculate(m_turnSim.getAngularPositionRotations(), rots);
     turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0);
diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java
index 583332c1..bfc53659 100644
--- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java
@@ -199,9 +199,9 @@ public void updateInputs(ModuleIOInputs inputs) {
     m_turnPid.updateIfChanged();
 
     inputs.drivePositionRad =
-        Units.rotationsToRadians(m_drivePosition.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO();
+        Units.rotationsToRadians(m_drivePosition.getValueAsDouble());
     inputs.driveVelocityRadPerSec =
-        Units.rotationsToRadians(m_driveVelocity.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO();
+        Units.rotationsToRadians(m_driveVelocity.getValueAsDouble());
     inputs.driveAppliedVolts = m_driveAppliedVolts.getValueAsDouble();
     inputs.driveCurrentAmps = new double[] {m_driveCurrent.getValueAsDouble()};
 
@@ -210,28 +210,19 @@ public void updateInputs(ModuleIOInputs inputs) {
     inputs.setTurnPosition(Rotation2d.fromRotations((
             m_turnPosition.getValueAsDouble())));
     inputs.setTurnVelocityRadPerSec(
-            Units.rotationsToRadians(m_turnVelocity.getValueAsDouble()) / m_moduleConstants.TURNING_GEAR_RATIO());
+            Units.rotationsToRadians(m_turnVelocity.getValueAsDouble()));
     inputs.setTurnAppliedVolts(m_turnAppliedVolts.getValueAsDouble());
     inputs.setTurnCurrentAmps(new double[] {m_turnCurrent.getValueAsDouble()});
 
     inputs.setOdometryDrivePositionsRad(m_drivePositionQueue.stream()
-        .mapToDouble((Double value) -> Units.rotationsToRadians(value) / m_moduleConstants.DRIVE_GEAR_RATIO())
+        .mapToDouble(Units::rotationsToRadians)
         .toArray());
     inputs.setOdometryTurnPositions(m_turnPositionQueue.stream()
-        .map((Double value) -> Rotation2d.fromRotations(value / m_moduleConstants.DRIVE_GEAR_RATIO()))
+        .map(Rotation2d::fromRotations)
         .toArray(Rotation2d[]::new));
+
     m_drivePositionQueue.clear();
     m_turnPositionQueue.clear();
-
-    SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() +
-            "/Closed Loop Output", m_turnClosedLoopOutput.getValueAsDouble());
-    SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() +
-            "/Closed Loop Error", m_turnClosedLoopError.getValueAsDouble());
-    SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() +
-            "/Closed Loop Reference", m_turnClosedLoopReference.getValueAsDouble());
-
-    SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() +
-            "/Recorded kP", m_turnPid.getSlotConfigs().kP);
   }
 
   @Override
@@ -242,9 +233,8 @@ public void setDriveVelocityMPS(double mps) {
   }
 
   @Override
-  public void setTurnPositionDegs(double degrees) {
-//    double rots = degrees / 360;
-    m_turnTalon.setControl(m_posRequest.withPosition(degrees));
+  public void setTurnPositionRots(double rotations) {
+    m_turnTalon.setControl(m_posRequest.withPosition(rotations));
   }
 
   @Override