Rewrote module class for drive to implement monologue; Removed Akit v…

…endor dep

build.gradle

@@ -80,9 +80,6 @@ dependencies {
     testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
 
     implementation "com.google.guava:guava:33.4.0-jre"
-
-    def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
-    annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
 }
 
 test {
@@ -114,12 +111,6 @@ tasks.withType(JavaCompile) {
     options.compilerArgs.add '-XDstringConcat=inline'
 }
 
-// Akit replay task
-task(replayWatch, type: JavaExec) {
-    mainClass = "org.littletonrobotics.junction.ReplayWatch"
-    classpath = sourceSets.main.runtimeClasspath
-}
-
 // Build Constants generation
 project.compileJava.dependsOn(createVersionFile)
 gversion {

src/main/java/wmironpatriots/Robot.java

@@ -11,41 +11,37 @@
 import edu.wpi.first.wpilibj.PS5Controller;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.Threads;
+import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import java.util.function.BiConsumer;
 import monologue.Logged;
 import monologue.Monologue;
-import org.littletonrobotics.junction.LoggedRobot;
-import wmironpatriots.subsystems.swerve.SwerveSubsystem;
-import wmironpatriots.subsystems.swerve.constants.CompBotSwerveConfigs;
-import wmironpatriots.util.ControllerUtil;
+import org.frc6423.frc2025.BuildConstants;
 import wmironpatriots.util.deviceUtil.TalonFXHandler;
 
-public class Robot extends LoggedRobot implements Logged {
+public class Robot extends TimedRobot implements Logged {
   private final CommandScheduler m_scheduler = CommandScheduler.getInstance();
 
   private final PS5Controller m_driveController;
 
   public static final TalonFXHandler talonHandler = new TalonFXHandler();
 
-  private final SwerveSubsystem m_swerveSubsystem;
-
   public Robot() {
     startupMonologue();
 
     RobotController.setBrownoutVoltage(6.0);
 
     m_driveController = new PS5Controller(0);
     // Subsystem init
-    m_swerveSubsystem = new SwerveSubsystem(new CompBotSwerveConfigs());
+    // m_swerveSubsystem = new SwerveSubsystem(new CompBotSwerveConfigs());
 
     // Default Commands
-    m_swerveSubsystem.setDefaultCommand(
-        m_swerveSubsystem.teleopSwerveCommmand(
-            ControllerUtil.applyDeadband(m_driveController::getLeftY, false),
-            ControllerUtil.applyDeadband(m_driveController::getLeftX, false),
-            ControllerUtil.applyDeadband(m_driveController::getRightX, false)));
+    // m_swerveSubsystem.setDefaultCommand(
+    //     m_swerveSubsystem.teleopSwerveCommmand(
+    //         ControllerUtil.applyDeadband(m_driveController::getLeftY, false),
+    //         ControllerUtil.applyDeadband(m_driveController::getLeftX, false),
+    //         ControllerUtil.applyDeadband(m_driveController::getRightX, false)));
   }
 
   @Override

src/main/java/wmironpatriots/subsystems/elevator/Elevator.java

@@ -33,9 +33,9 @@ public abstract class Elevator extends SubsystemBase {
   public static final double kL3PoseMeters = 10.81;
   public static final double kL4PoseMeters = 24;
 
-
   /** LOGGED VALUES */
   @Log protected boolean LMotorOk = false;
+
   @Log protected boolean RMotorOk = false;
 
   @Log protected double poseMeters;
@@ -56,10 +56,12 @@ public abstract class Elevator extends SubsystemBase {
   @Log protected double RMotorTorqueCurrentAmps;
   @Log protected double RMotorTempCelsius;
 
-
   /** VARIABLES */
-  private final PositionVoltage m_motorPoseOutReq = new PositionVoltage(0.0).withEnableFOC(true).withUpdateFreqHz(0.0);
-  private final VoltageOut m_motorVoltOutReq = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0.0);
+  private final PositionVoltage m_motorPoseOutReq =
+      new PositionVoltage(0.0).withEnableFOC(true).withUpdateFreqHz(0.0);
+
+  private final VoltageOut m_motorVoltOutReq =
+      new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0.0);
 
   /** Run target position meters from current zeroed pose */
   public Command runTargetPoseCommand(double poseMeters) {
@@ -100,7 +102,6 @@ private void resetPose() {
     setEncoderPose(0.0);
   }
 
-
   /** HARDWARE METHODS */
   /** Run elevator motor with control request */
   protected abstract void runMotorControl(ControlRequest request);

src/main/java/wmironpatriots/subsystems/swerve/SwerveSubsystem.java

@@ -25,14 +25,11 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import java.util.Arrays;
 import java.util.function.DoubleSupplier;
-import org.frc6423.frc2025.subsystems.swerve.gyro.GyroIOInputsAutoLogged;
-import org.littletonrobotics.junction.Logger;
-import wmironpatriots.subsystems.swerve.gyro.GyroIO;
-import wmironpatriots.subsystems.swerve.gyro.GyroIONavX;
-import wmironpatriots.subsystems.swerve.gyro.GyroIOPigeon;
+import wmironpatriots.Robot;
 import wmironpatriots.subsystems.swerve.module.Module;
+import wmironpatriots.subsystems.swerve.module.ModuleIOComp;
+import wmironpatriots.subsystems.swerve.module.ModuleIOSim;
 import wmironpatriots.util.swerveUtil.SwerveConfig;
-import wmironpatriots.util.swerveUtil.SwerveConfig.GyroType;
 
 public class SwerveSubsystem extends SubsystemBase {
   private final SwerveConfig m_config;
@@ -42,23 +39,22 @@ public class SwerveSubsystem extends SubsystemBase {
   private SwerveDriveKinematics m_kinematics;
   private SwerveDrivePoseEstimator m_odo;
 
-  private final GyroIO m_gyro;
-  private final GyroIOInputsAutoLogged m_gyroInputs;
   private Rotation2d m_simHeading;
 
   private final Field2d m_f2d;
 
   public SwerveSubsystem(SwerveConfig config) {
     // Create modules
-    var moduleConfigs = config.getModuleConfigs();
-    m_modules = new Module[moduleConfigs.length];
-    Arrays.stream(moduleConfigs).forEach((c) -> m_modules[c.kIndex - 1] = new Module(c));
-
-    m_gyro =
-        config.getGyroType() == GyroType.PIGEON
-            ? new GyroIOPigeon(config.getGyroID())
-            : new GyroIONavX();
-    m_gyroInputs = new GyroIOInputsAutoLogged();
+    if (Robot.isReal()) {
+      var moduleConfigs = config.getModuleConfigs();
+      m_modules = new Module[moduleConfigs.length];
+      Arrays.stream(moduleConfigs).forEach((c) -> m_modules[c.kIndex - 1] = new ModuleIOComp(c));
+    } else {
+      var moduleConfigs = config.getModuleConfigs();
+      m_modules = new Module[moduleConfigs.length];
+      Arrays.stream(moduleConfigs).forEach((c) -> m_modules[c.kIndex - 1] = new ModuleIOSim(c));
+    }
+
     m_simHeading = new Rotation2d();
 
     // Create math objects
@@ -74,17 +70,17 @@ public SwerveSubsystem(SwerveConfig config) {
   @Override
   public void periodic() {
     // Update gyro and all modules
-    Arrays.stream(m_modules).forEach((m) -> m.updateInputs());
-    m_gyro.updateInputs(m_gyroInputs);
+    Arrays.stream(m_modules).forEach((m) -> m.periodic());
+    // m_gyro.updateInputs(m_gyroInputs);
 
     // Odo update
     updateOdometry();
     m_f2d.setRobotPose(getPose());
     SmartDashboard.putData(m_f2d);
 
     // Log swerve data
-    Logger.recordOutput("Swerve/ActualOutput", getVelocitiesRobotRelative());
-    Logger.recordOutput("Swerve/ActualStates", getModuleStates());
+    // Logger.recordOutput("Swerve/ActualOutput", getVelocitiesRobotRelative());
+    // Logger.recordOutput("Swerve/ActualStates", getModuleStates());
 
     // Stop module input when driverstation is disabled
     if (DriverStation.isDisabled()) {
@@ -103,7 +99,7 @@ public void simulationPeriodic() {
             m_config.getMaxAngularSpeedRadsPerSec() / 10000);
     m_simHeading = m_simHeading.rotateBy(Rotation2d.fromRadians(clamped));
 
-    Logger.recordOutput("Swerve/simRotation", m_simHeading.getDegrees());
+    // Logger.recordOutput("Swerve/simRotation", m_simHeading.getDegrees());
   }
 
   /**
@@ -152,8 +148,8 @@ public void runVelocities(ChassisSpeeds velocity, boolean openloopEnabled) {
     SwerveDriveKinematics.desaturateWheelSpeeds(
         desiredStates, m_config.getMaxLinearSpeedMetersPerSec());
 
-    Logger.recordOutput("Swerve/desiredVelocity", velocity);
-    Logger.recordOutput("Swerve/desiredSetpoints", desiredStates);
+    // Logger.recordOutput("Swerve/desiredVelocity", velocity);
+    // Logger.recordOutput("Swerve/desiredSetpoints", desiredStates);
 
     for (int i = 0; i < desiredStates.length; i++) {
       if (openloopEnabled) {

src/main/java/wmironpatriots/subsystems/swerve/module/Module.java

@@ -6,23 +6,22 @@
 
 package wmironpatriots.subsystems.swerve.module;
 
+import com.ctre.phoenix6.controls.ControlRequest;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.system.plant.DCMotor;
 import monologue.Annotations.Log;
-
-import com.ctre.phoenix6.controls.ControlRequest;
-import com.ctre.phoenix6.controls.PositionVoltage;
-import com.ctre.phoenix6.controls.VelocityVoltage;
-import com.ctre.phoenix6.controls.VoltageOut;
-
 import wmironpatriots.util.swerveUtil.ModuleConfig;
 
 public abstract class Module {
   /** LOGGED VALUES */
   @Log protected boolean pivotOk = false;
+
   @Log protected boolean driveOk = false;
 
   @Log protected Rotation2d pivotABSPose = new Rotation2d();
@@ -38,11 +37,14 @@ public abstract class Module {
   @Log protected double driveSupplyCurrent;
   @Log protected double driveTorqueCurrent;
 
-
   /** VARIABLES */
-  private final VoltageOut m_motorVoltsOutReq = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0.0);
-  private final VelocityVoltage m_motorVelocityOutReq = new VelocityVoltage(0.0).withEnableFOC(true).withUpdateFreqHz(0.0);
-  private final PositionVoltage m_motorPositionOutReq = new PositionVoltage(0.0).withEnableFOC(true).withUpdateFreqHz(0.0);
+  private final VoltageOut m_motorVoltsOutReq =
+      new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0.0);
+
+  private final VelocityVoltage m_motorVelocityOutReq =
+      new VelocityVoltage(0.0).withEnableFOC(true).withUpdateFreqHz(0.0);
+  private final PositionVoltage m_motorPositionOutReq =
+      new PositionVoltage(0.0).withEnableFOC(true).withUpdateFreqHz(0.0);
 
   private final SimpleMotorFeedforward m_driveFF = new SimpleMotorFeedforward(0.14, 0.134);
 
@@ -52,22 +54,23 @@ public Module(ModuleConfig config) {
     m_config = config;
   }
 
+  /** Periodically ran logic */
+  public void periodic() {}
+
   /** Run setpoint module state */
   public SwerveModuleState runSetpoint(SwerveModuleState setpoint) {
     setpoint.optimize(pivotABSPose);
-    setpoint.speedMetersPerSecond *=
-      Math.cos(setpoint.angle.minus(pivotABSPose).getRadians());
+    setpoint.speedMetersPerSecond *= Math.cos(setpoint.angle.minus(pivotABSPose).getRadians());
 
     double speedMPS = setpoint.speedMetersPerSecond;
     runPivotControl(m_motorPositionOutReq.withPosition(setpoint.angle.getRotations()));
     runDriveControl(
-      m_motorVelocityOutReq
-        .withVelocity(speedMPS)
-        .withFeedForward(m_driveFF.calculate(
-            (speedMPS / m_config.kWheelRadiusMeters)
-                * (m_config.kDriveReduction
-                    / DCMotor.getKrakenX60Foc(1)
-                        .KtNMPerAmp))));
+        m_motorVelocityOutReq
+            .withVelocity(speedMPS)
+            .withFeedForward(
+                m_driveFF.calculate(
+                    (speedMPS / m_config.kWheelRadiusMeters)
+                        * (m_config.kDriveReduction / DCMotor.getKrakenX60Foc(1).KtNMPerAmp))));
 
     return new SwerveModuleState();
   }
@@ -79,7 +82,8 @@ public SwerveModuleState runSetpoint(SwerveModuleState setpointState, double dri
   }
 
   /** Run setpoint module state but with open loop drive control */
-  public SwerveModuleState runSetpointOpenloop(double voltage, Rotation2d angle, boolean FOCEnabled) {
+  public SwerveModuleState runSetpointOpenloop(
+      double voltage, Rotation2d angle, boolean FOCEnabled) {
     SwerveModuleState setpointState = new SwerveModuleState(voltage, angle);
 
     setpointState.optimize(pivotABSPose);
@@ -92,6 +96,10 @@ public SwerveModuleState runSetpointOpenloop(double voltage, Rotation2d angle, b
     return new SwerveModuleState();
   }
 
+  public void stop() {
+    stopMotors();
+  }
+
   /** Enable coast mode to move robot easier */
   public void moduleCoasting(boolean enabled) {
     motorCoasting(enabled);
@@ -100,7 +108,7 @@ public void moduleCoasting(boolean enabled) {
   /** Returns module angle */
   public Rotation2d getModuleAngle() {
     return pivotPose;
-  } 
+  }
 
   /** Converts drive motor speed to mps */
   public double getModuleSpeedMPS() {
@@ -117,7 +125,6 @@ public SwerveModuleState getModuleState() {
     return new SwerveModuleState(getModuleSpeedMPS(), pivotPose);
   }
 
-
   /** HARDWARE METHODS */
   /** Run Pivot motor with control request */
   protected abstract void runPivotControl(ControlRequest request);