diff --git a/src/main/java/frc/lib/FaultLogger.java b/src/main/java/frc/lib/FaultLogger.java index 418ba9e..507029a 100644 --- a/src/main/java/frc/lib/FaultLogger.java +++ b/src/main/java/frc/lib/FaultLogger.java @@ -19,7 +19,6 @@ import java.util.Set; import java.util.function.BooleanSupplier; import java.util.function.Supplier; - import org.photonvision.PhotonCamera; // (from team 1155 but slightly modified) @@ -242,10 +241,13 @@ public static void register(Pigeon2 pigeon) { /** * Registers a new PhotonCamera (more detailed logs on the web ui). - * + * * @param photonCamera The PhotonCamera. */ public static void register(PhotonCamera photonCamera) { - register(() -> !photonCamera.isConnected(), photonCamera.getName() + ": Disconnected.", FaultType.ERROR); + register( + () -> !photonCamera.isConnected(), + photonCamera.getName() + ": Disconnected.", + FaultType.ERROR); } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 41938b6..09df4ca 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -45,16 +45,20 @@ public class Robot extends TimedRobot { private Command _autonomousCommand = Autos.none(); + private boolean _fileOnlySet = false; + /** * This function is run when the robot is first started up and should be used for any * initialization code. */ public Robot() { // set up loggers - DogLog.setOptions(new DogLogOptions().withNtPublish(true)); + DogLog.setOptions(new DogLogOptions().withCaptureDs(true)); Epilogue.bind(this); SignalLogger.start(); + setFileOnly(false); // file-only once connected to fms + DriverStation.silenceJoystickConnectionWarning(isSimulation()); FaultLogger.setup(); @@ -72,6 +76,16 @@ public Robot() { addPeriodic(FaultLogger::update, 1); } + // set logging to be file only or not + private void setFileOnly(boolean fileOnly) { + DogLog.setOptions(DogLog.getOptions().withNtPublish(!fileOnly)); + + Epilogue.configure( + config -> { + // TODO + }); + } + private void configureBindings() { _swerve.setDefaultCommand( _swerve.drive( @@ -103,6 +117,12 @@ public void robotPeriodic() { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + + if (DriverStation.isFMSAttached() && !_fileOnlySet) { + setFileOnly(true); + + _fileOnlySet = true; + } } /** This autonomous runs the autonomous command. */ diff --git a/src/main/java/frc/robot/utils/VisionPoseEstimator.java b/src/main/java/frc/robot/utils/VisionPoseEstimator.java index f4698e7..6ff0035 100644 --- a/src/main/java/frc/robot/utils/VisionPoseEstimator.java +++ b/src/main/java/frc/robot/utils/VisionPoseEstimator.java @@ -12,9 +12,9 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.networktables.NetworkTableInstance; +import frc.lib.FaultLogger; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.VisionConstants; -import frc.lib.FaultLogger; import frc.robot.Robot; import java.util.ArrayList; import java.util.Comparator;