added robot mode triggers for logging isOpenLoop and field oriented

Team334 · Nov 17, 2024 · e0c7ec2 · e0c7ec2
1 parent 94b1884
commit e0c7ec2
Showing 1 changed file with 17 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
@@ -25,6 +25,7 @@
 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 frc.lib.InputStream;
 import frc.robot.Constants;
 import frc.robot.Constants.SwerveConstants;
@@ -90,6 +91,22 @@ public CommandSwerveDrivetrain(
           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;
+                }));
+
     if (RobotBase.isSimulation()) startSimThread();
   }
 
@@ -181,9 +198,6 @@ public void drive(double velX, double velY, double velOmega) {
    */
   public void drive(
       ChassisSpeeds speeds, double[] wheelForceFeedforwardsX, double[] wheelForceFeedforwardsY) {
-    _isFieldOriented = false;
-    _isOpenLoop = false;
-
     setControl(
         _robotSpeedsRequest
             .withSpeeds(speeds)