done - need to test

src/main/java/frc/lib/util/swerve/SwerveModule.java

@@ -16,7 +16,7 @@
 public class SwerveModule {
     public int moduleNumber;
     private Rotation2d angleOffset;
-    private double lastAngle;
+    // private double lastAngle;
 
     private SwerveModuleIO io;
     private SwerveModuleInputsAutoLogged inputs = new SwerveModuleInputsAutoLogged();
@@ -49,7 +49,7 @@ public SwerveModule(int moduleNumber, int driveMotorID, int angleMotorID, int ca
 
         this.angleOffset = angleOffset;
 
-        lastAngle = getState().angle.getDegrees();
+        // lastAngle = getState().angle.getDegrees();
 
     }
 
@@ -61,7 +61,6 @@ public SwerveModule(int moduleNumber, int driveMotorID, int angleMotorID, int ca
      */
     public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) {
         desiredState = SwerveModuleState.optimize(desiredState, getState().angle);
-        // mAngleMotor.setControl(anglePosition.withPosition(desiredState.angle.getRotations()));
         io.setAngleMotor(anglePosition.withPosition(desiredState.angle.getRotations()));
         setSpeed(desiredState, isOpenLoop);
     }
@@ -75,14 +74,12 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop)
     private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) {
         if (isOpenLoop) {
             driveDutyCycle.Output = desiredState.speedMetersPerSecond / Constants.Swerve.maxSpeed;
-            // mDriveMotor.setControl(driveDutyCycle);
             io.setDriveMotor(driveDutyCycle);
         } else {
             driveVelocity.Velocity = Conversions.MPSToRPS(desiredState.speedMetersPerSecond,
                 Constants.Swerve.wheelCircumference);
             driveVelocity.FeedForward =
                 driveFeedForward.calculate(desiredState.speedMetersPerSecond);
-            // mDriveMotor.setControl(driveVelocity);
             io.setDriveMotor(driveDutyCycle);
         }
     }
@@ -101,7 +98,6 @@ public Rotation2d getCANcoder() {
      */
     public void resetToAbsolute() {
         double absolutePosition = getCANcoder().getRotations() - angleOffset.getRotations();
-        // mAngleMotor.setPosition(absolutePosition);
         io.setPositionAngleMotor(absolutePosition);
     }
 
@@ -112,11 +108,8 @@ public void resetToAbsolute() {
      */
     public SwerveModuleState getState() {
         return new SwerveModuleState(
-            // Conversions.RPSToMPS(mDriveMotor.getVelocity().getValue(),
-            // Constants.Swerve.wheelCircumference),
             Conversions.RPSToMPS(io.getVelocityDriveMotor().getValue(),
                 Constants.Swerve.wheelCircumference),
-            // Rotation2d.fromRotations(mAngleMotor.getPosition().getValue()));
             Rotation2d.fromRotations(io.getPositionAngleMotor().getValue()));
     }
 
@@ -127,11 +120,8 @@ public SwerveModuleState getState() {
      */
     public SwerveModulePosition getPosition() {
         return new SwerveModulePosition(
-            // Conversions.rotationsToMeters(mDriveMotor.getPosition().getValue(),
-            // Constants.Swerve.wheelCircumference),
             Conversions.rotationsToMeters(io.getPositionDriveMotor().getValue(),
                 Constants.Swerve.wheelCircumference),
-            // Rotation2d.fromRotations(mAngleMotor.getPosition().getValue()));
             Rotation2d.fromRotations(io.getPositionAngleMotor().getValue()));
 
     }

src/main/java/frc/lib/util/swerve/SwerveModuleReal.java

@@ -18,7 +18,7 @@ public class SwerveModuleReal implements SwerveModuleIO {
     private CANcoderConfiguration swerveCANcoderConfig = new CANcoderConfiguration();
 
     public SwerveModuleReal(int moduleNumber, int driveMotorID, int angleMotorID, int cancoderID,
-        Rotation2d angleOffset, SwerveModuleIO io) {
+        Rotation2d angleOffset) {
 
         angleEncoder = new CANcoder(cancoderID);
         mDriveMotor = new TalonFX(driveMotorID);
@@ -100,30 +100,37 @@ private void configAngleEncoder() {
         angleEncoder.getConfigurator().apply(swerveCANcoderConfig);
     }
 
+    @Override
     public void setAngleMotor(ControlRequest request) {
         mAngleMotor.setControl(request);
     }
 
+    @Override
     public void setDriveMotor(ControlRequest request) {
         mDriveMotor.setControl(request);
     }
 
+    @Override
     public StatusSignal<Double> getAbsolutePositionAngleEncoder() {
         return angleEncoder.getAbsolutePosition();
     }
 
+    @Override
     public void setPositionAngleMotor(double absolutePosition) {
         mAngleMotor.setPosition(absolutePosition);
     }
 
+    @Override
     public StatusSignal<Double> getVelocityDriveMotor() {
         return mDriveMotor.getVelocity();
     }
 
+    @Override
     public StatusSignal<Double> getPositionAngleMotor() {
         return mAngleMotor.getPosition();
     }
 
+    @Override
     public StatusSignal<Double> getPositionDriveMotor() {
         return mDriveMotor.getPosition();
     }

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

@@ -59,7 +59,7 @@ public void robotInit() {
             // Instantiate our RobotContainer. This will perform all our button bindings, and put
             // our
             // autonomous chooser on the dashboard.
-            m_robotContainer = new RobotContainer();
+            m_robotContainer = new RobotContainer(isReal());
         }
     }
 

src/main/java/frc/robot/RobotContainer.java

@@ -18,6 +18,8 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.TeleopSwerve;
 import frc.robot.subsystems.swerve.Swerve;
+import frc.robot.subsystems.swerve.SwerveIO;
+import frc.robot.subsystems.swerve.SwerveReal;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -32,10 +34,15 @@ public class RobotContainer {
     private final SendableChooser<String> autoChooser = new SendableChooser<>();
 
     /* Subsystems */
-    private final Swerve s_Swerve = new Swerve();
+    private final Swerve s_Swerve;
 
     /** The container for the robot. Contains subsystems, OI devices, and commands. */
-    public RobotContainer() {
+    public RobotContainer(boolean isReal) {
+        if (isReal) {
+            s_Swerve = new Swerve(new SwerveReal());
+        } else {
+            s_Swerve = new Swerve(new SwerveIO() {});
+        }
         s_Swerve.setDefaultCommand(new TeleopSwerve(s_Swerve, driver,
             Constants.Swerve.isFieldRelative, Constants.Swerve.isOpenLoop));
         // autoChooser.addOption(resnickAuto, new ResnickAuto(s_Swerve));

src/main/java/frc/robot/commands/TeleopSwerve.java

@@ -17,7 +17,7 @@ public class TeleopSwerve extends Command {
     private CommandXboxController controller;
 
     /**
-     * Creates an command for driving the swerve drive during tele-op
+     * Creates a command for driving the swerve drive during tele-op
      *
      * @param swerveDrive The instance of the swerve drive subsystem
      * @param fieldRelative Whether the movement is relative to the field or absolute

src/main/java/frc/robot/commands/ZeroMotorsWaitCommand.java

@@ -5,6 +5,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
 import frc.robot.subsystems.swerve.Swerve;
+import frc.robot.subsystems.swerve.SwerveIO;
 
 /**
  * Zeros motors then executes normal wait command.
@@ -19,10 +20,10 @@ public class ZeroMotorsWaitCommand extends Command {
      *
      * @param seconds how long the wait command should run
      */
-    public ZeroMotorsWaitCommand(double seconds) {
+    public ZeroMotorsWaitCommand(double seconds, SwerveIO swerveIO) {
         this.m_duration = seconds;
         SendableRegistry.setName(this, getName() + ": " + seconds + " seconds");
-        this.s_Swerve = new Swerve();
+        this.s_Swerve = new Swerve(swerveIO);
     }
 
     @Override

src/main/java/frc/robot/subsystems/swerve/Swerve.java

@@ -16,7 +16,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.lib.util.swerve.SwerveModule;
-import frc.lib.util.swerve.SwerveModuleIO;
 import frc.robot.Constants;
 
 /**
@@ -31,20 +30,20 @@ public class Swerve extends SubsystemBase {
     /**
      * Swerve Subsystem
      */
-    public Swerve(SwerveIO swerveIO, SwerveModuleIO swerveModuleIO) {
+    public Swerve(SwerveIO swerveIO) {
         swerveMods = new SwerveModule[] {
-            new SwerveModule(0, Constants.Swerve.Mod0.driveMotorID,
+            swerveIO.createSwerveModule(0, Constants.Swerve.Mod0.driveMotorID,
                 Constants.Swerve.Mod0.angleMotorID, Constants.Swerve.Mod0.canCoderID,
-                Constants.Swerve.Mod0.angleOffset, swerveModuleIO),
-            new SwerveModule(1, Constants.Swerve.Mod1.driveMotorID,
+                Constants.Swerve.Mod0.angleOffset),
+            swerveIO.createSwerveModule(1, Constants.Swerve.Mod1.driveMotorID,
                 Constants.Swerve.Mod1.angleMotorID, Constants.Swerve.Mod1.canCoderID,
-                Constants.Swerve.Mod1.angleOffset, swerveModuleIO),
-            new SwerveModule(2, Constants.Swerve.Mod2.driveMotorID,
+                Constants.Swerve.Mod1.angleOffset),
+            swerveIO.createSwerveModule(2, Constants.Swerve.Mod2.driveMotorID,
                 Constants.Swerve.Mod2.angleMotorID, Constants.Swerve.Mod2.canCoderID,
-                Constants.Swerve.Mod2.angleOffset, swerveModuleIO),
-            new SwerveModule(3, Constants.Swerve.Mod3.driveMotorID,
+                Constants.Swerve.Mod2.angleOffset),
+            swerveIO.createSwerveModule(3, Constants.Swerve.Mod3.driveMotorID,
                 Constants.Swerve.Mod3.angleMotorID, Constants.Swerve.Mod3.canCoderID,
-                Constants.Swerve.Mod3.angleOffset, swerveModuleIO)};
+                Constants.Swerve.Mod3.angleOffset)};
 
         swerveOdometry = new SwerveDriveOdometry(Constants.Swerve.swerveKinematics, getGyroYaw(),
             getModulePositions());

src/main/java/frc/robot/subsystems/swerve/SwerveIO.java

@@ -1,6 +1,7 @@
 package frc.robot.subsystems.swerve;
 
 import org.littletonrobotics.junction.AutoLog;
+import edu.wpi.first.math.geometry.Rotation2d;
 import frc.lib.util.swerve.SwerveModule;
 import frc.lib.util.swerve.SwerveModuleIO;
 
@@ -16,9 +17,12 @@ public static class SwerveInputs {
 
     public default void updateInputs(SwerveInputs inputs) {}
 
-    public default SwerveModule createSwerveModule(int moduleNumber,
-        frc.lib.util.swerve.SwerveModuleConstants constants) {
-        return new SwerveModule(moduleNumber, constants, new SwerveModuleIO() {});
+    public default SwerveModule createSwerveModule(int moduleNumber, int driveMotorID,
+        int angleMotorID, int cancoderID, Rotation2d angleOffset) {
+        return new SwerveModule(moduleNumber, driveMotorID, angleMotorID, cancoderID, angleOffset,
+            new SwerveModuleIO() {
+
+            });
     }
 
 }

src/main/java/frc/robot/subsystems/swerve/SwerveReal.java

@@ -1,8 +1,9 @@
 package frc.robot.subsystems.swerve;
 
-import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants;
 import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.math.geometry.Rotation2d;
 import frc.lib.util.swerve.SwerveModule;
+import frc.lib.util.swerve.SwerveModuleReal;
 import frc.robot.Constants;
 
 /** Real Class for Swerve */
@@ -21,8 +22,11 @@ public void updateInputs(SwerveInputs inputs) {
     }
 
     @Override
-    public SwerveModule createSwerveModule(int moduleNumber, SwerveModuleConstants constants) {
-        return new SwerveModule(moduleNumber, constants, new SwerveModuleReal(constants));
+    public SwerveModule createSwerveModule(int moduleNumber, int driveMotorID, int angleMotorID,
+        int cancoderID, Rotation2d angleOffset) {
+        return new SwerveModule(moduleNumber, driveMotorID, angleMotorID, cancoderID, angleOffset,
+            new SwerveModuleReal(moduleNumber, driveMotorID, angleMotorID, cancoderID,
+                angleOffset));
     }
 
 }