From e04dec9ce159fda0feafd3f5d2a55e104b42d396 Mon Sep 17 00:00:00 2001
From: Ishan <98932677+Ishan1522@users.noreply.github.com>
Date: Tue, 24 Dec 2024 20:15:57 -0500
Subject: [PATCH] refactor SwerveDrive and SwerveModule; adjust motor
 configurations and clean up commented code

---
 .../robot/subsystems/swerve/SwerveDrive.java  |  4 +--
 .../robot/subsystems/swerve/SwerveModule.java | 25 +++----------------
 .../swerve/module/SimulatedModule.java        |  6 ++---
 3 files changed, 9 insertions(+), 26 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java
index 4e2823c..c988e70 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java
@@ -45,7 +45,7 @@ public class SwerveDrive extends SubsystemBase {
       new SwerveSetpointGenerator(
           DriveConstants.MODULE_TRANSLATIONS,
           DCMotor.getKrakenX60(1).withReduction(ModuleConstants.DRIVE_GEAR_RATIO),
-          DCMotor.getFalcon500(1).withReduction(11),
+          DCMotor.getFalcon500(1).withReduction(1),
           60,
           58,
           7,
@@ -214,7 +214,7 @@ xSpeed, ySpeed, rotationSpeed, getOdometryAllianceRelativeRotation2d())
    * @return Value is Counter-clockwise positive.
    */
   public double getHeading() {
-    return -gyroInputs.yawDegrees;
+    return gyroInputs.yawDegrees;
   }
 
   /**
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java
index 8dbc1ec..b3ab1eb 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java
@@ -43,23 +43,6 @@ public void periodic() {
     // updateOdometryPositions();
   }
 
-  // private void updateOdometryPositions() {
-  //   odometryPositions = new SwerveModulePosition[inputs.odometryDriveWheelRevolutions.length];
-  //   for (int i = 0; i < odometryPositions.length; i++) {
-  //     double positionMeters = inputs.odometryDriveWheelRevolutions[i];
-  //     Rotation2d angle = inputs.odometrySteerPositions[i];
-  //     odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
-
-  //     SmartDashboard.putNumber("updated drive position", positionMeters);
-  //     SmartDashboard.putNumber("updated angle", angle.getDegrees());
-  //   }
-  // }
-
-  // private double driveRevolutionsToMeters(double driveWheelRevolutions) {
-  //   return Rotations.of(driveWheelRevolutions).in(Radians)
-  //       * SimulationConstants.WHEEL_RADIUS_METERS;
-  // }
-
   public void setVoltage(Voltage volts) {
     io.setDriveVoltage(volts);
     io.setTurnVoltage(Volts.zero());
@@ -75,10 +58,10 @@ public double getCharacterizationVelocity() {
 
   /** Runs the module with the specified setpoint state. Returns optimized setpoint */
   public void runSetPoint(SwerveModuleState state) {
-    // state.optimize(getTurnRotation());
-    // if (state.speedMetersPerSecond < 0.01) {
-    //   io.stopModule();
-    // }
+    state.optimize(getTurnRotation());
+    if (state.speedMetersPerSecond < 0.01) {
+      io.stopModule();
+    }
     io.setDesiredState(state);
   }
 
diff --git a/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java
index 2105c49..10a8ec5 100644
--- a/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java
+++ b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java
@@ -24,12 +24,12 @@ public class SimulatedModule implements ModuleInterface {
           ModuleConstants.MAX_ANGULAR_SPEED_ROTATIONS_PER_SECOND,
           ModuleConstants.MAX_ANGULAR_ACCELERATION_ROTATIONS_PER_SECOND_SQUARED);
   private final ProfiledPIDController turnPID =
-      new ProfiledPIDController(3.5, 0, 0, turnConstraints);
-  private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(4, 10, 0);
+      new ProfiledPIDController(5, 0, 0, turnConstraints);
+  private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(6, 8, 0);
 
   public SimulatedModule(SwerveModuleSimulation moduleSimulation) {
     this.moduleSimulation = moduleSimulation;
-    // turnPID.enableContinuousInput(-Math.PI, Math.PI);
+    turnPID.enableContinuousInput(-Math.PI, Math.PI);
   }
 
   @Override