idk

TitaniumTigers4829 · Nov 30, 2024 · 6529e41 · 6529e41
1 parent 51afa7e
commit 6529e41
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 22 deletions.
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java
@@ -48,7 +48,7 @@ public class SwerveDrive extends SubsystemBase {
   private final SwerveModulePosition[] lastModulePositions;
   private final SwerveDrivePoseEstimator poseEstimator;
 
-  private SwerveSetpoint setpoint;
+  private SwerveSetpoint setpoint = SwerveSetpoint.zeroed();
   private final SwerveSetpointGenerator setpointGenerator =
       new SwerveSetpointGenerator(
           DriveConstants.MODULE_TRANSLATIONS,
@@ -94,10 +94,10 @@ public SwerveDrive(
 
     swerveModules =
         new SwerveModule[] {
-          new SwerveModule(frontLeftModuleIO, "FrontLeft", 0),
-          new SwerveModule(frontRightModuleIO, "FrontRight", 1),
-          new SwerveModule(backLeftModuleIO, "BackLeft",2),
-          new SwerveModule(backRightModuleIO, "BackRight", 3)
+          new SwerveModule(frontLeftModuleIO, "FrontLeft"),
+          new SwerveModule(frontRightModuleIO, "FrontRight"),
+          new SwerveModule(backLeftModuleIO, "BackLeft"),
+          new SwerveModule(backRightModuleIO, "BackRight")
         };
 
     lastModulePositions =
@@ -122,7 +122,7 @@ public SwerveDrive(
 
     gyroDisconnectedAlert.set(false);
 
-    setpoint = SwerveSetpoint.zeroed();
+    // setpoint = SwerveSetpoint.zeroed();
   }
 
   /**
@@ -250,8 +250,8 @@ public double getAllianceAngleOffset() {
    *     frontRight, backLeft, backRight (should be the same as the kinematics).
    */
   public void setModuleStates(AdvancedSwerveModuleState[] desiredStates) {
-    for (SwerveModule module : swerveModules) {
-      module.runSetPoint(AdvancedSwerveModuleState.fromBase(desiredStates[module.getNumber()]));
+    for (int i = 0; i < 4; i ++) {
+      swerveModules[i].runSetPoint(AdvancedSwerveModuleState.fromBase(desiredStates[i]));
     }
   }
 

diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java
@@ -19,18 +19,14 @@
 public class SwerveModule extends SubsystemBase {
   private final ModuleInterface io;
   private final String name;
-  private final int number;
   private final ModuleInputsAutoLogged inputs = new ModuleInputsAutoLogged();
 
-  private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
-
   private final Alert hardwareFaultAlert;
 
-  public SwerveModule(ModuleInterface io, String name, int number) {
+  public SwerveModule(ModuleInterface io, String name) {
     super("Module-" + name);
     this.io = io;
     this.name = name;
-    this.number = number;
     this.hardwareFaultAlert =
         new Alert("Module-" + name + " Hardware Fault", Alert.AlertType.kError);
     this.hardwareFaultAlert.set(false);
@@ -62,10 +58,10 @@ public double getCharacterizationVelocity() {
 
   /** Runs the module with the specified setpoint state. Returns optimized setpoint */
   public void runSetPoint(AdvancedSwerveModuleState state) {
-    state.optimize(getTurnRotation());
-    if (Math.abs(state.speedMetersPerSecond) < 0.01) {
-      io.stopModule();
-    }
+    // state.optimize(getTurnRotation());
+    // if (Math.abs(state.speedMetersPerSecond) < 0.01) {
+    //   io.stopModule();
+    // }
 
     io.setDesiredState(state);
   }
@@ -103,8 +99,4 @@ public SwerveModuleState getMeasuredState() {
   public SwerveModulePosition getPosition() {
     return new SwerveModulePosition(getDrivePositionMeters(), getTurnRotation());
   }
-
-  public int getNumber() {
-    return number;
-  }
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java
@@ -199,7 +199,7 @@ public SwerveSetpoint generateSetpoint(final SwerveSetpoint prevSetpoint, Chassi
             return generateSetpoint(prevSetpoint, ZERO_SPEEDS, dt);
         }
 
-        // solveSteering(vars);
+        solveSteering(vars);
         // solveDriving(vars);
 
         ChassisSpeeds retSpeeds = new ChassisSpeeds(