diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 60f86cb..f6ddde7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -200,14 +200,15 @@ public void teleopInit() { } /** This function is called periodically during operator control. */ - @Override - public void teleopPeriodic() { - // Check who won autonomous. - if (autonomousWinner == null) { - determineAutonomousWinner(); - } else { - determineHubActive(); - } + @Override + public void teleopPeriodic() { + // Check who won autonomous. + if (autonomousWinner == null) { + determineAutonomousWinner(); + } else { + determineHubActive(); + determineHubCountdown(); + } } @@ -245,9 +246,30 @@ private void determineHubActive() { hubActive = (allianceColor.get() == autoWinner); } else if (timeLeft <= Constants.SHIFT_1_START) { hubActive = (allianceColor.get() != autoWinner); + } else hubActive = true; // transition + } - } else hubActive = true; // transition - } + private void determineHubCountdown(){ + int hubCountdown = 0; + // Determines time until next hub shift + int timeLeft = (int) DriverStation.getMatchTime(); + if (timeLeft < 0) + hubCountdown = 0; // Match has not started. + if (0 <= timeLeft && timeLeft <= Constants.ENDGAME_START) { + hubCountdown = timeLeft; + // Calculates the time between current time and next shift for every shift of the match + } else if (Constants.ENDGAME_START < timeLeft && timeLeft <= Constants.SHIFT_4_START) { + hubCountdown = timeLeft - Constants.ENDGAME_START; + } else if (Constants.SHIFT_4_START < timeLeft && timeLeft <= Constants.SHIFT_3_START) { + hubCountdown = timeLeft - Constants.SHIFT_4_START; + } else if (Constants.SHIFT_3_START < timeLeft && timeLeft <= Constants.SHIFT_2_START) { + hubCountdown = timeLeft - Constants.SHIFT_3_START; + } else if (Constants.SHIFT_2_START < timeLeft && timeLeft <= Constants.SHIFT_1_START) { + hubCountdown = timeLeft - Constants.SHIFT_2_START; + } else {hubCountdown = timeLeft - Constants.SHIFT_1_START;} + SmartDashboard.putNumber("Countdown Until Next Hub Shift", hubCountdown); + // Puts the countdown on dashboard so driver and operator can see the time until the next shift + } @Override public void testInit() {