diff --git a/src/main/java/org/littletonrobotics/frc2024/Robot.java b/src/main/java/org/littletonrobotics/frc2024/Robot.java index 1bd0ba37..f7465454 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Robot.java +++ b/src/main/java/org/littletonrobotics/frc2024/Robot.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.button.Trigger; import java.io.File; import java.io.FileWriter; import java.io.IOException; @@ -60,6 +61,8 @@ public class Robot extends LoggedRobot { private final Timer canInitialErrorTimer = new Timer(); private final Timer canErrorTimer = new Timer(); private final Timer canivoreErrorTimer = new Timer(); + private double teleStart; + private static double teleElapsedTime = 0.0; private final Alert canErrorAlert = new Alert("CAN errors detected, robot may not be controllable.", AlertType.ERROR); @@ -72,6 +75,14 @@ public class Robot extends LoggedRobot { private final Alert sameBatteryAlert = new Alert("The battery has not been changed since the last match.", AlertType.WARNING); + public static Trigger createTeleopTimeTrigger(double teleElapsedTime) { + return new Trigger( + () -> + DriverStation.isFMSAttached() + && DriverStation.isTeleopEnabled() + && Robot.teleElapsedTime > teleElapsedTime); + } + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -325,11 +336,15 @@ public void teleopInit() { } NoteVisualizer.clearAutoNotes(); NoteVisualizer.showAutoNotes(); + + teleStart = Timer.getFPGATimestamp(); } /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + teleElapsedTime = Timer.getFPGATimestamp() - teleStart; + } /** This function is called once when test mode is enabled. */ @Override