misc swerve stuff

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

@@ -29,7 +29,6 @@
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
-import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.lib.FaultLogger;
 import frc.lib.FaultsTable;
@@ -163,22 +162,6 @@ public Swerve(
           DogLog.log("Swerve/Odometry Period", state.OdometryPeriod);
         });
 
-    // TODO: idk if this will conflict with the actual scheduled commands
-    RobotModeTriggers.autonomous()
-        .onTrue(
-            runOnce(
-                () -> {
-                  _isFieldOriented = false;
-                  _isOpenLoop = false;
-                }));
-    RobotModeTriggers.teleop()
-        .onTrue(
-            runOnce(
-                () -> {
-                  _isFieldOriented = true;
-                  _isOpenLoop = true;
-                }));
-
     // display all sysid routines
     SysId.displayRoutine("Swerve Translation", _sysIdRoutineTranslation);
     SysId.displayRoutine("Swerve Steer", _sysIdRoutineSteer);
@@ -288,6 +271,11 @@ public Command drive(InputStream velX, InputStream velY, InputStream velOmega) {
     return run(() -> {
           drive(velX.get(), velY.get(), velOmega.get());
         })
+        .beforeStarting(
+            () -> {
+              _isFieldOriented = true;
+              _isOpenLoop = false;
+            })
         .withName("Drive");
   }
 
@@ -396,4 +384,11 @@ public Command selfCheck(BiConsumer<String, FaultType> faults) {
         selfCheckModule("Back Left", getModule(2)),
         selfCheckModule("Back Right", getModule(3)));
   }
+
+  @Override
+  public void close() {
+    super.close();
+
+    _simNotifier.close();
+  }
 }

src/test/java/frc/robot/SwerveTest.java

@@ -0,0 +1,27 @@
+package frc.robot;
+
+import static frc.lib.UnitTestingUtil.reset;
+import static frc.lib.UnitTestingUtil.setupTests;
+
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.Swerve;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+
+public class SwerveTest {
+  static final double DELTA = 1e-2; // acceptable deviation range
+
+  private Swerve _swerve;
+
+  @BeforeEach
+  public void setup() {
+    setupTests();
+
+    _swerve = TunerConstants.createDrivetrain();
+  }
+
+  @AfterEach
+  public void close() throws Exception {
+    reset(_swerve);
+  }
+}