diff --git a/src/main/java/ca/team2706/frc/robot/Robot.java b/src/main/java/ca/team2706/frc/robot/Robot.java index 79a1aa6b..0839b252 100644 --- a/src/main/java/ca/team2706/frc/robot/Robot.java +++ b/src/main/java/ca/team2706/frc/robot/Robot.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj.command.Scheduler; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.lang.reflect.Field; import java.util.ArrayList; @@ -52,6 +53,8 @@ public void robotInit() { setOnStateChange((state) -> Log.i("Robot State: " + state.name())); setOnConnectionChange((state) -> Log.i("Connection State: " + state.name())); setOnConnectionChange(Log::setupFMS); + // Adding the match time to SmartDashboard for use by other processors. + SmartDashboard.putNumber(Config.MATCH_TIME_NT_KEY, 0); onStateChange(RobotState.ROBOT_INIT); isInitialized = true; @@ -136,6 +139,33 @@ public void robotPeriodic() { driverStationConnected = DriverStation.getInstance().isDSAttached(); onConnectionChange(driverStationConnected ? ConnectionState.DRIVERSTATION_CONNECT : ConnectionState.DRIVERSTATION_DISCONNECT); } + + // If it's a real match, output the match time for use by others. + if (fmsConnected) { + SmartDashboard.putNumber(Config.MATCH_TIME_NT_KEY, getMatchTime()); + } + } + + /** + * Gets the match time counting autonomous and the teleop period together. Will count down from 150 to 0, + * 150 being start of the match at auto, 0 being end of teleop. + * This may not return accurate match times if it is not a real match. + * + * @return The current match time in seconds. + */ + public static double getMatchTime() { + final double time; + if (DriverStation.getInstance().isDisabled()) { + time = 0; + } else if (DriverStation.getInstance().isAutonomous()) { + time = Timer.getMatchTime() + 135; + } else if (DriverStation.getInstance().isOperatorControl()) { + time = Timer.getMatchTime(); + } else { + time = 0; + } + + return time; } /** diff --git a/src/main/java/ca/team2706/frc/robot/config/Config.java b/src/main/java/ca/team2706/frc/robot/config/Config.java index 612663c0..676bb029 100644 --- a/src/main/java/ca/team2706/frc/robot/config/Config.java +++ b/src/main/java/ca/team2706/frc/robot/config/Config.java @@ -42,6 +42,11 @@ public static void init() { private static int robotId = -1; + /** + * Networktables key for the current match time. + */ + public static final String MATCH_TIME_NT_KEY = "Match Time"; + // Values for driving robot with joystick public static final boolean TELEOP_SQUARE_JOYSTICK_INPUTS = true,