diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 4c5a3d0..cf9defd 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -19,13 +19,18 @@ import java.util.ArrayList; import java.util.List; +import java.util.Optional; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + import java.util.function.BooleanSupplier; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -72,7 +77,7 @@ -public class RobotContainer { +public class RobotContainer implements Sendable { public final GenericHID driverController = new GenericHID(Driver.port); public final XboxController driverRumble = new XboxController(Driver.port); //For rumbling the controller @@ -86,21 +91,26 @@ public class RobotContainer { private SendableChooser autoChooser = new SendableChooser<>(); - public boolean alignOverride = true; public boolean autoScoring = true; - public static int intakeCounter; + private boolean assumeAutoWin = false; + private String gameData; public RobotContainer() { + + + //#region AutoRegistration RegisterAutoCommands(); + // + // + + autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); - SmartDashboard.putBoolean("AlignOverride", alignOverride); SmartDashboard.putBoolean("AutoScoring", autoScoring); - SmartDashboard.putBoolean("AlignOverride", true); //#endregion setDefaultCommands(); setBindingsDriver(); @@ -112,6 +122,7 @@ public RobotContainer() { SmartDashboard.putString("Location", DriverStation.getLocation().toString()); SmartDashboard.putBoolean("Connected to FMS?", DriverStation.isFMSAttached()); SmartDashboard.putString("Station", DriverStation.getAlliance().toString() + " " + DriverStation.getLocation().toString()); + SmartDashboard.putData("Hub", this); } @@ -160,6 +171,195 @@ private void setDefaultCommands() { public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } + public boolean isHubActive() { + Optional alliance = DriverStation.getAlliance(); + // If we have no alliance, we cannot be enabled, therefore no hub. + if (alliance.isEmpty()) { + return false; + } + // Hub is always enabled in autonomous. + if (DriverStation.isAutonomousEnabled()) { + return true; + } + //Hub is technically on during disabled period + if (!DriverStation.isTeleopEnabled()) { + return true; + } + + // We're teleop enabled, compute. + double matchTime = DriverStation.getMatchTime(); + gameData = DriverStation.getGameSpecificMessage(); + boolean shift1Active; + // If we have no game data, we cannot compute, assume hub is active, as its likely early in teleop. + if (gameData.isEmpty()) { + shift1Active = !assumeAutoWin; + } + else { + boolean redInactiveFirst = false; + switch (gameData.charAt(0)) { + case 'R' -> { + redInactiveFirst = true; + // Shift was is active for blue if red won auto, or red if blue won auto. + shift1Active = switch (alliance.get()) { + case Red -> !redInactiveFirst; + case Blue -> redInactiveFirst; + }; + } + case 'B' -> { + redInactiveFirst = false; + // Shift was is active for blue if red won auto, or red if blue won auto. + shift1Active = switch (alliance.get()) { + case Red -> !redInactiveFirst; + case Blue -> redInactiveFirst; + }; + } + default -> { + // If we have invalid game data, assume hub is active. + shift1Active = !assumeAutoWin; + } + } + } + + if (matchTime > 130) { + // Transition shift, hub is active. + return true; + } + else if (matchTime > 105) { + // Shift 1 + return shift1Active; + } + else if (matchTime > 80) { + // Shift 2 + return !shift1Active; + } + else if (matchTime > 55) { + // Shift 3 + return shift1Active; + } + else if (matchTime > 30) { + // Shift 4 + return !shift1Active; + } + else { + // End game, hub always active. + return true; + } + } + public double hubTimeLeft(){ //Gets time left until hub is active/inactive + double matchTime = DriverStation.getMatchTime(); + Optional alliance = DriverStation.getAlliance(); + if (alliance.isEmpty()) { + return -1; + } + if (DriverStation.isAutonomous()) { + return assumeAutoWin ? matchTime + 10 : matchTime + 35; + } + gameData = DriverStation.getGameSpecificMessage(); + boolean shift1Active; + // If we have no game data, we cannot compute, assume hub is active, as its likely early in teleop. + if (gameData.isEmpty()) { + shift1Active = !assumeAutoWin; + } + else { + boolean redInactiveFirst = false; + switch (gameData.charAt(0)) { + case 'R' -> { + redInactiveFirst = true; + // Shift was is active for blue if red won auto, or red if blue won auto. + shift1Active = switch (alliance.get()) { + case Red -> !redInactiveFirst; + case Blue -> redInactiveFirst; + }; + } + case 'B' -> { + redInactiveFirst = false; + // Shift was is active for blue if red won auto, or red if blue won auto. + shift1Active = switch (alliance.get()) { + case Red -> !redInactiveFirst; + case Blue -> redInactiveFirst; + }; + } + default -> { + // If we have invalid game data, assume hub is active. + shift1Active = !assumeAutoWin; + } + } + } + if (matchTime > 130) { + // Transition shift, hub is active. + return shift1Active ? matchTime - 105 : matchTime - 130; + } + else if (matchTime > 105) { + // Shift 1 + return matchTime - 105; + } + else if (matchTime > 80) { + // Shift 2 + return matchTime - 80; + } + else if (matchTime > 55) { + // Shift 3 + return matchTime - 55; + } + else if (matchTime > 30) { + // Shift 4 + return shift1Active ? matchTime - 30 : matchTime; + } + else { + // End game, hub always active. + return matchTime; + } + } + + public boolean getAssumeAutoWin() { + return assumeAutoWin; + } + + public void setAssumeAutoWin(boolean assumption) { + assumeAutoWin = assumption; + } + + private int shiftNumber() { + if (DriverStation.isAutonomous()) { + return 0; + } + if (DriverStation.isTeleop()) { + double matchTime = DriverStation.getMatchTime(); + if (matchTime > 130) { + // Transition shift + return 0; + } + else if (matchTime > 105) { + // Shift 1 + return 1; + } + else if (matchTime > 80) { + // Shift 2 + return 2; + } + else if (matchTime > 55) { + // Shift 3 + return 3; + } + else if (matchTime > 30) { + // Shift 4 + return 4; + } + else { + // End game + return 5; + } + } + return 0; + } + + @Override + public void initSendable(SendableBuilder builder){ + builder.addBooleanProperty("Hub Active (T/F)", this::isHubActive, null); + builder.addDoubleProperty("Hub Time Left", this::hubTimeLeft, null); + builder.addBooleanProperty("Assume Won Auto", this::getAssumeAutoWin, this::setAssumeAutoWin); + builder.addDoubleProperty("Active shift", this::shiftNumber, null); + } //#endregion //#region HelpfulMethods //TODO: integrate these methods into lib199