Phoenix6 swerve

src/main/java/frc/robot/Robot.java

@@ -4,9 +4,7 @@
 
 package frc.robot;
 
-import com.ctre.phoenix.sensors.CANCoder;
-import com.ctre.phoenix.sensors.Pigeon2;
-import com.ctre.phoenixpro.hardware.TalonFX;
+import com.ctre.phoenix6.hardware.Pigeon2;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import edu.wpi.first.wpilibj.PowerDistribution;
@@ -29,11 +27,11 @@
 import frc.robot.intake.IntakeSubsystem;
 import frc.robot.localization.LocalizationSubsystem;
 import frc.robot.managers.AutoRotate;
-import frc.robot.swerve.SwerveModule;
-import frc.robot.swerve.SwerveSubsystem;
 import frc.robot.util.scheduling.LifecycleSubsystemManager;
 import frc.robot.wrist.Positions;
 import frc.robot.wrist.WristSubsystem;
+import frc.swerve.SwerveSubsystem;
+
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.NT4Publisher;
@@ -55,35 +53,11 @@ public class Robot extends LoggedRobot {
   private final RumbleControllerSubsystem rumbleController =
       new RumbleControllerSubsystem(new XboxController(1));
 
-  private final SwerveModule frontLeft =
-      new SwerveModule(
-          Config.SWERVE_FL_CONSTANTS,
-          new TalonFX(Config.SWERVE_FL_DRIVE_MOTOR_ID),
-          new TalonFX(Config.SWERVE_FL_STEER_MOTOR_ID),
-          new CANCoder(Config.SWERVE_FL_CANCODER_ID));
-  private final SwerveModule frontRight =
-      new SwerveModule(
-          Config.SWERVE_FR_CONSTANTS,
-          new TalonFX(Config.SWERVE_FR_DRIVE_MOTOR_ID),
-          new TalonFX(Config.SWERVE_FR_STEER_MOTOR_ID),
-          new CANCoder(Config.SWERVE_FR_CANCODER_ID));
-  private final SwerveModule backLeft =
-      new SwerveModule(
-          Config.SWERVE_BL_CONSTANTS,
-          new TalonFX(Config.SWERVE_BL_DRIVE_MOTOR_ID),
-          new TalonFX(Config.SWERVE_BL_STEER_MOTOR_ID),
-          new CANCoder(Config.SWERVE_BL_CANCODER_ID));
-  private final SwerveModule backRight =
-      new SwerveModule(
-          Config.SWERVE_BR_CONSTANTS,
-          new TalonFX(Config.SWERVE_BR_DRIVE_MOTOR_ID),
-          new TalonFX(Config.SWERVE_BR_STEER_MOTOR_ID),
-          new CANCoder(Config.SWERVE_BR_CANCODER_ID));
+
 
   private final FmsSubsystem fmsSubsystem = new FmsSubsystem();
   private final ImuSubsystem imu = new ImuSubsystem(new Pigeon2(Config.PIGEON_ID));
-  private final SwerveSubsystem swerve =
-      new SwerveSubsystem(imu, frontRight, frontLeft, backRight, backLeft);
+  private final SwerveSubsystem swerve = new SwerveSubsystem(imu);
   private final IntakeSubsystem intake =
       new IntakeSubsystem(new CANSparkMax(Config.INTAKE_MOTOR_ID, MotorType.kBrushless));
   private final WristSubsystem wrist =
@@ -194,8 +168,7 @@ private void configureButtonBindings() {
             () ->
                 driveController.getSidewaysPercentage() == 0
                     && driveController.getForwardPercentage() == 0
-                    && driveController.getThetaPercentage() == 0)
-        .onFalse(swerve.disableXSwerveCommand());
+                    && driveController.getThetaPercentage() == 0);
 
     // operator home wrist
     operatorController.back().onTrue(wrist.getHomeCommand());

src/main/java/frc/robot/autos/Autobalance.java

@@ -13,9 +13,9 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.fms.FmsSubsystem;
 import frc.robot.imu.ImuSubsystem;
-import frc.robot.swerve.SwerveSubsystem;
 import frc.robot.util.scheduling.LifecycleSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
+import frc.swerve.SwerveSubsystem;
 
 public class Autobalance extends LifecycleSubsystem {
   private final SwerveSubsystem swerve;

src/main/java/frc/robot/autos/Autos.java

@@ -21,9 +21,10 @@
 import frc.robot.intake.IntakeState;
 import frc.robot.intake.IntakeSubsystem;
 import frc.robot.localization.LocalizationSubsystem;
-import frc.robot.swerve.SwerveSubsystem;
 import frc.robot.wrist.Positions;
 import frc.robot.wrist.WristSubsystem;
+import frc.swerve.SwerveSubsystem;
+
 import java.lang.ref.WeakReference;
 import java.util.EnumMap;
 import java.util.EnumSet;
@@ -125,7 +126,7 @@ public Autos(
             SwerveSubsystem.KINEMATICS,
             Config.SWERVE_TRANSLATION_PID,
             Config.SWERVE_ROTATION_PID,
-            (states) -> swerve.setModuleStates(states, false, false),
+            (states) -> swerve.getModuleStates(states, false, false),
             eventMap,
             false,
             swerve);
@@ -190,7 +191,7 @@ private Command buildAutoCommand(AutoKind auto) {
     }
 
     String autoName = "Auto" + auto.toString();
-    Command autoCommand = Commands.runOnce(() -> swerve.driveTeleop(0, 0, 0, true, true), swerve);
+    Command autoCommand = Commands.runOnce(() -> swerve.driveTeleop(0.0, 0.0, 0.0), swerve);
 
     if (auto == AutoKind.DO_NOTHING) {
       return autoCommand

src/main/java/frc/robot/config/Config.java

@@ -5,12 +5,9 @@
 package frc.robot.config;
 
 import com.pathplanner.lib.auto.PIDConstants;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
-import frc.robot.swerve.SwerveCorner;
-import frc.robot.swerve.SwerveModuleConstants;
 
 public class Config {
   public static final String SERIAL_NUMBER = System.getenv("serialnum");
@@ -38,27 +35,18 @@ public class Config {
   public static final int SWERVE_FL_DRIVE_MOTOR_ID = 2;
   public static final int SWERVE_FL_STEER_MOTOR_ID = 3;
   public static final int SWERVE_FL_CANCODER_ID = 10;
-  public static final SwerveModuleConstants SWERVE_FL_CONSTANTS =
-      new SwerveModuleConstants(
-          Rotation2d.fromDegrees(284.68), SwerveCorner.FRONT_LEFT, false, false);
+
   public static final int SWERVE_FR_DRIVE_MOTOR_ID = 4;
   public static final int SWERVE_FR_STEER_MOTOR_ID = 5;
   public static final int SWERVE_FR_CANCODER_ID = 11;
-  public static final SwerveModuleConstants SWERVE_FR_CONSTANTS =
-      new SwerveModuleConstants(
-          Rotation2d.fromDegrees(259.89), SwerveCorner.FRONT_RIGHT, false, false);
+
   public static final int SWERVE_BL_DRIVE_MOTOR_ID = 6;
   public static final int SWERVE_BL_STEER_MOTOR_ID = 7;
   public static final int SWERVE_BL_CANCODER_ID = 12;
-  public static final SwerveModuleConstants SWERVE_BL_CONSTANTS =
-      new SwerveModuleConstants(
-          Rotation2d.fromDegrees(35.60), SwerveCorner.BACK_LEFT, false, false);
+
   public static final int SWERVE_BR_DRIVE_MOTOR_ID = 8;
   public static final int SWERVE_BR_STEER_MOTOR_ID = 9;
   public static final int SWERVE_BR_CANCODER_ID = 13;
-  public static final SwerveModuleConstants SWERVE_BR_CONSTANTS =
-      new SwerveModuleConstants(
-          Rotation2d.fromDegrees(116.37), SwerveCorner.BACK_RIGHT, false, false);
 
   public static final int WRIST_MOTOR_ID = 16;
   public static final double WRIST_GEARING = 1.0 / 96.0;

src/main/java/frc/robot/imu/ImuSubsystem.java

@@ -4,14 +4,15 @@
 
 package frc.robot.imu;
 
-import com.ctre.phoenix.sensors.Pigeon2;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.util.scheduling.LifecycleSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
 import org.littletonrobotics.junction.Logger;
 
+import com.ctre.phoenix6.hardware.Pigeon2;
+
 public class ImuSubsystem extends LifecycleSubsystem {
   private final Pigeon2 imu;
 

src/main/java/frc/robot/localization/LocalizationSubsystem.java

@@ -11,9 +11,10 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.fms.FmsSubsystem;
 import frc.robot.imu.ImuSubsystem;
-import frc.robot.swerve.SwerveSubsystem;
 import frc.robot.util.scheduling.LifecycleSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
+import frc.swerve.SwerveSubsystem;
+
 import org.littletonrobotics.junction.Logger;
 
 public class LocalizationSubsystem extends LifecycleSubsystem {

src/main/java/frc/robot/managers/AutoRotate.java

@@ -7,9 +7,10 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.fms.FmsSubsystem;
-import frc.robot.swerve.SwerveSubsystem;
 import frc.robot.util.scheduling.LifecycleSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
+import frc.swerve.SwerveSubsystem;
+
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;