diff --git a/.gitignore b/.gitignore index 778db45..79e9d55 100644 --- a/.gitignore +++ b/.gitignore @@ -171,6 +171,7 @@ out/ # Simulation GUI and other tools window save file networktables.json simgui.json +simgui-ds.json *-window.json # Simulation data log directory diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c0d5cd2..378df19 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,7 @@ package frc.robot; +import java.util.Optional; import java.util.concurrent.atomic.AtomicReference; import org.littletonrobotics.junction.LogFileUtil; @@ -13,6 +14,9 @@ import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.utils.logging.commands.CommandLogger; @@ -28,6 +32,13 @@ public class Robot extends LoggedRobot { private final RobotContainer robotContainer; private static final AtomicReference mode = new AtomicReference<>(RobotMode.DISABLED); + private static Character autonomousWinner; + + private static boolean hubActive; + private static Timer timer = new Timer(); + private static Alliance autoWinner; + + private static Optional allianceColor = Optional.empty(); /** * This function is run when the robot is first started up and should be used for any @@ -97,6 +108,14 @@ public void robotPeriodic() { if (Constants.ENABLE_LOGGING) { CommandLogger.get().log(); } + + // Gets the alliance color. + if (DriverStation.isDSAttached() && allianceColor.isEmpty()) { + allianceColor = DriverStation.getAlliance(); + if (allianceColor.isPresent()) { + robotContainer.getAutoChooser().getProvider().forceRefresh(); + } + } } /** This function is called once each time the robot enters Disabled mode. */ @@ -121,6 +140,9 @@ public void autonomousInit() { if (autonomousCommand != null) { autonomousCommand.schedule(); } + + // Hub is always active during autonomous. + hubActive = true; } /** This function is called periodically during autonomous. */ @@ -137,17 +159,45 @@ public void teleopInit() { if (autonomousCommand != null) { autonomousCommand.cancel(); } + + // Start the timer to keep track of shifts. + timer.start(); } /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + // Check who won autonomous. + if (autonomousWinner == null) { + autonomousWinner = DriverStation.getGameSpecificMessage().charAt(0); + if (autonomousWinner != null) { + autoWinner = switch (autonomousWinner) { + case 'R' -> Alliance.Red; + case 'B' -> Alliance.Blue; + default -> throw new AssertionError("Data is corrupt - winner must be red or blue."); + }; + } + } else { + // Determine whether the hub is active. + if (timer.hasElapsed(110)) { + hubActive = true; // end game + } else if (timer.hasElapsed(85)) { + hubActive = (allianceColor.get() == autoWinner); // shift 4 + } else if (timer.hasElapsed(60)) { + hubActive = (allianceColor.get() != autoWinner); // shift 3 + } else if (timer.hasElapsed(35)) { + hubActive = (allianceColor.get() == autoWinner); // shift 2 + } else if (timer.hasElapsed(10)) { + hubActive = (allianceColor.get() != autoWinner); // shift 1 + } else hubActive = true; // transition + } + } @Override public void testInit() { - mode.set(RobotMode.TEST); - // Cancels all running commands at the start of test mode. - CommandScheduler.getInstance().cancelAll(); } + mode.set(RobotMode.TEST); + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); } /** This function is called periodically during test mode. */ @Override @@ -160,4 +210,10 @@ public void simulationInit() {} /** This function is called periodically whilst in simulation. */ @Override public void simulationPeriodic() {} + + // getters + /** @return whether the hub is active */ + public static boolean hubActive() {return hubActive;} + /** @return the alliance color */ + public static Optional getAllianceColor() {return allianceColor;} } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f664f73..8d51908 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,6 +8,10 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.autochooser.AutoAction; +import frc.robot.autochooser.FieldLocation; +import frc.robot.autochooser.chooser.AutoChooser2026; +import frc.robot.autochooser.event.RealAutoEventProvider; import frc.robot.commands.roller.SpinRoller; import frc.robot.commands.tilt.TiltDown; import frc.robot.commands.tilt.TiltUp; @@ -26,6 +30,7 @@ public class RobotContainer { private final RollerSubsystem rollerSubsystem; private final TiltSubsystem tiltSubsystem; private RobotVisualizer robotVisualizer = null; + private final AutoChooser2026 autoChooser; // Replace with CommandPS4Controller or CommandJoystick if needed //new CommandXboxController(OperatorConstants.kDriverControllerPort); @@ -52,6 +57,7 @@ public RobotContainer() { } configureBindings(); putShuffleboardCommands(); + autoChooser = new AutoChooser2026(new RealAutoEventProvider(AutoAction.DO_NOTHING, FieldLocation.ZERO)); } /** @@ -99,4 +105,8 @@ public Command getAutonomousCommand() { public RobotVisualizer getRobotVisualizer() { return robotVisualizer; } + + public AutoChooser2026 getAutoChooser() { + return autoChooser; + } } diff --git a/src/main/java/frc/robot/autochooser/AutoAction.java b/src/main/java/frc/robot/autochooser/AutoAction.java new file mode 100644 index 0000000..cbb8da9 --- /dev/null +++ b/src/main/java/frc/robot/autochooser/AutoAction.java @@ -0,0 +1,37 @@ +package frc.robot.autochooser; + +import java.util.Arrays; +import java.util.HashMap; +import java.util.function.Function; +import java.util.stream.Collectors; + +public enum AutoAction { + DO_NOTHING("Do Nothing"), + TWO_PIECE_HIGH("2 Piece L4"), + TWO_PIECE_LOW("2 Piece L2"), + ONE_PIECE("1 Piece"), + CROSS_THE_LINE("Cross The Line"), + INVALID("INVALID"); + private final String name; + private static final HashMap nameMap = + new HashMap<>( + Arrays.stream(AutoAction.values()) + .collect(Collectors.toMap(AutoAction::getName, Function.identity()))); + + AutoAction(String name) { + this.name = name; + } + + public String getName() { + return name; + } + + @Override + public String toString() { + return getName(); + } + + public static AutoAction fromName(String name) { + return nameMap.get(name); + } +} diff --git a/src/main/java/frc/robot/autochooser/FieldLocation.java b/src/main/java/frc/robot/autochooser/FieldLocation.java new file mode 100644 index 0000000..bb8ae60 --- /dev/null +++ b/src/main/java/frc/robot/autochooser/FieldLocation.java @@ -0,0 +1,59 @@ +package frc.robot.autochooser; + +import static edu.wpi.first.wpilibj.DriverStation.Alliance; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import frc.robot.Robot; +import java.util.Arrays; +import java.util.HashMap; +import java.util.function.Function; +import java.util.stream.Collectors; + +public enum FieldLocation { + ZERO(0, 0, 0, "Zero"), + INVALID(-1, -1, -1, "INVALID"), + LEFT(7.150, 7.000, 180, "NON Processor Side"), + MIDDLE(7.150, 4.500, 180, "Middle"), + RIGHT(7.150, 2.000, 180, "Processor Side"); + + private static final double RED_X_POS = 2.3876; // meters + public static final double HEIGHT_OF_FIELD = 8.05; + public static final double LENGTH_OF_FIELD = 17.548225; + private final double yPose; + private final double xPose; + private final double angle; + private final String name; + private static final HashMap nameMap = + new HashMap<>( + Arrays.stream(FieldLocation.values()) + .collect(Collectors.toMap(FieldLocation::getShuffleboardName, Function.identity()))); + + FieldLocation(double xPos, double yPose, double angle, String name) { + this.xPose = xPos; + this.yPose = yPose; + this.angle = angle; + this.name = name; + } + + public static FieldLocation fromName(String string) { + return nameMap.get(string); + } + + public Pose2d getLocation() { + Alliance alliance = Robot.getAllianceColor().orElse(null); + if (alliance == null) { + return new Pose2d(INVALID.xPose, INVALID.yPose, Rotation2d.fromDegrees(INVALID.angle)); + } + double x = (alliance == Alliance.Red) ? xPose + RED_X_POS + Units.inchesToMeters(36) : xPose; + double y = (alliance == Alliance.Red) ? yPose - 2 * (yPose - (HEIGHT_OF_FIELD / 2)) : yPose; + double radian = + (alliance == Alliance.Red) ? Math.toRadians(180 - angle) : Math.toRadians(angle); + return new Pose2d(x, y, Rotation2d.fromRadians(radian)); + } + + public String getShuffleboardName() { + return name; + } +} diff --git a/src/main/java/frc/robot/autochooser/chooser/AutoChooser.java b/src/main/java/frc/robot/autochooser/chooser/AutoChooser.java new file mode 100644 index 0000000..f2edea8 --- /dev/null +++ b/src/main/java/frc/robot/autochooser/chooser/AutoChooser.java @@ -0,0 +1,16 @@ +package frc.robot.autochooser.chooser; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.autochooser.event.AutoEvent; + +/** interface for taking in a {@link AutoEvent} and returning the corresponding {@link Command} */ +public interface AutoChooser { + /** + * @return Command that corresponds to the selected {@link AutoEvent} from the {@link + * frc.robot.autochooser.event.AutoEventProvider} + */ + Command getAutoCommand(); + + Pose2d getStartingPosition(); +} diff --git a/src/main/java/frc/robot/autochooser/chooser/AutoChooser2026.java b/src/main/java/frc/robot/autochooser/chooser/AutoChooser2026.java new file mode 100644 index 0000000..ce47343 --- /dev/null +++ b/src/main/java/frc/robot/autochooser/chooser/AutoChooser2026.java @@ -0,0 +1,59 @@ +package frc.robot.autochooser.chooser; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.autochooser.AutoAction; +import frc.robot.autochooser.FieldLocation; +import frc.robot.autochooser.event.AutoEvent; +import frc.robot.autochooser.event.AutoEventProvider; +import frc.robot.autochooser.event.AutoEventProviderIO; +import frc.robot.utils.logging.commands.DoNothingCommand; +import java.util.Map; + +public class AutoChooser2026 extends SubsystemBase implements AutoChooser { + private final Map commandMap; + private final AutoEventProvider provider; + // private final SwerveDrivetrain drivetrain; + + public AutoChooser2026( + AutoEventProviderIO providerIO + // SwerveDrivetrain drivetrain, + ) { + provider = new AutoEventProvider(providerIO, this::isValid); + // this.drivetrain = drivetrain; + commandMap = + Map.ofEntries( + Map.entry( + new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.LEFT), new DoNothingCommand()), + Map.entry( + new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.MIDDLE), new DoNothingCommand()), + Map.entry( + new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.RIGHT), new DoNothingCommand()) + ); + } + + @Override + public Command getAutoCommand() { + return commandMap.get( + new AutoEvent(provider.getSelectedAction(), provider.getSelectedLocation())); + } + + @Override + public Pose2d getStartingPosition() { + return provider.getSelectedLocation().getLocation(); + } + + protected boolean isValid(AutoAction action, FieldLocation location) { + return commandMap.containsKey(new AutoEvent(action, location)); + } + + @Override + public void periodic() { + provider.updateInputs(); + } + + public AutoEventProvider getProvider() { + return provider; + } +} diff --git a/src/main/java/frc/robot/autochooser/event/AutoChooserInputs.java b/src/main/java/frc/robot/autochooser/event/AutoChooserInputs.java new file mode 100644 index 0000000..11934d4 --- /dev/null +++ b/src/main/java/frc/robot/autochooser/event/AutoChooserInputs.java @@ -0,0 +1,35 @@ +package frc.robot.autochooser.event; + +import frc.robot.autochooser.AutoAction; +import frc.robot.autochooser.FieldLocation; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +public class AutoChooserInputs implements LoggableInputs { + AutoAction action = AutoAction.INVALID; + FieldLocation location = FieldLocation.INVALID; + AutoAction defaultAction = AutoAction.INVALID; + FieldLocation defaultLocation = FieldLocation.INVALID; + AutoAction feedbackAction = AutoAction.INVALID; + FieldLocation feedbackLocation = FieldLocation.INVALID; + + @Override + public void toLog(LogTable table) { + table.put("action", action); + table.put("location", location); + table.put("defaultAction", defaultAction); + table.put("defaultLocation", defaultLocation); + table.put("feedbackAction", feedbackAction); + table.put("feedbackLocation", feedbackLocation); + } + + @Override + public void fromLog(LogTable table) { + action = table.get("action", action); + location = table.get("location", location); + defaultAction = table.get("defaultAction", defaultAction); + defaultLocation = table.get("defaultLocation", defaultLocation); + feedbackAction = table.get("feedbackAction", feedbackAction); + feedbackLocation = table.get("feedbackLocation", feedbackLocation); + } +} diff --git a/src/main/java/frc/robot/autochooser/event/AutoEvent.java b/src/main/java/frc/robot/autochooser/event/AutoEvent.java new file mode 100644 index 0000000..977f323 --- /dev/null +++ b/src/main/java/frc/robot/autochooser/event/AutoEvent.java @@ -0,0 +1,40 @@ +package frc.robot.autochooser.event; + +import frc.robot.autochooser.AutoAction; +import frc.robot.autochooser.FieldLocation; +import java.util.Objects; + +/** + * Wrapper Class, that Contains a {@link frc.robot.autochooser.AutoAction} and a {@link + * frc.robot.autochooser.FieldLocation} + */ +public class AutoEvent { + private final AutoAction action; + private final FieldLocation location; + + public AutoEvent(AutoAction action, FieldLocation location) { + this.action = action; + this.location = location; + } + + public AutoAction getAction() { + return action; + } + + public FieldLocation getLocation() { + return location; + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (o == null || getClass() != o.getClass()) return false; + AutoEvent autoEvent = (AutoEvent) o; + return action.equals(autoEvent.action) && location.equals(autoEvent.location); + } + + @Override + public int hashCode() { + return Objects.hash(action, location); + } +} diff --git a/src/main/java/frc/robot/autochooser/event/AutoEventProvider.java b/src/main/java/frc/robot/autochooser/event/AutoEventProvider.java new file mode 100644 index 0000000..b34f9a8 --- /dev/null +++ b/src/main/java/frc/robot/autochooser/event/AutoEventProvider.java @@ -0,0 +1,76 @@ +package frc.robot.autochooser.event; + +import frc.robot.Robot; +import frc.robot.autochooser.AutoAction; +import frc.robot.autochooser.FieldLocation; +import java.util.function.BiFunction; +import java.util.function.Consumer; + +/** + * Superclass of {@link AutoEventProviderIO} that uses Network Tables to get {@link AutoAction + * AutoActions} and {@link FieldLocation fieldLocations}
+ */ +public class AutoEventProvider { + private final AutoEventProviderIO io; + private final AutoChooserInputs inputs; + private final BiFunction validator; + private boolean changed = false; + + public AutoEventProvider( + AutoEventProviderIO providerIO, BiFunction validator) { + this.io = providerIO; + this.inputs = new AutoChooserInputs(); + this.validator = validator; + setOnActionChangeListener((a) -> changed = true); + setOnLocationChangeListener((l) -> changed = true); + } + + public AutoAction getSelectedAction() { + return inputs.action == null + ? inputs.defaultAction + : inputs.action; + } + + public FieldLocation getSelectedLocation() { + return inputs.location == null + ? inputs.defaultLocation + : inputs.location; + } + + public void updateInputs() { + FieldLocation lastsLoc = inputs.location; + AutoAction lastsAct = inputs.action; + if (!Robot.isReal() + && (!lastsLoc.equals(inputs.location) + || !lastsAct.equals(inputs.action))) { + changed = true; + } + if (changed) { + forceRefresh(); + changed = false; + } + } + + public void setOnActionChangeListener(Consumer listener) { + io.setOnActionChangeListener(listener); + } + + public void setOnLocationChangeListener(Consumer listener) { + io.setOnLocationChangeListener(listener); + } + + public void forceRefresh() { + if (validator.apply(getSelectedAction(), getSelectedLocation())) { + io.setFeedbackAction(getSelectedAction()); + io.setFeedbackLocation(getSelectedLocation()); + io.runValidCommands(); + } else { + io.setFeedbackAction(AutoAction.INVALID); + io.setFeedbackLocation(FieldLocation.INVALID); + } + } + + public void addOnValidationCommand(Runnable c) { + io.addOnValidationCommand(c); + } +} diff --git a/src/main/java/frc/robot/autochooser/event/AutoEventProviderIO.java b/src/main/java/frc/robot/autochooser/event/AutoEventProviderIO.java new file mode 100644 index 0000000..61f7f06 --- /dev/null +++ b/src/main/java/frc/robot/autochooser/event/AutoEventProviderIO.java @@ -0,0 +1,25 @@ +package frc.robot.autochooser.event; + +import frc.robot.autochooser.AutoAction; +import frc.robot.autochooser.FieldLocation; +import java.util.function.Consumer; + +/** + * interface that outlines the necessary methods to provide {@link FieldLocation FieldLocations} and + * {@link AutoAction AutoActions} + */ +public interface AutoEventProviderIO { + void setOnActionChangeListener(Consumer listener); + + void setOnLocationChangeListener(Consumer listener); + + void addOnValidationCommand(Runnable consumer); + + void runValidCommands(); + + void setFeedbackAction(AutoAction action); + + void setFeedbackLocation(FieldLocation location); + + void updateInputs(AutoChooserInputs inputs); +} diff --git a/src/main/java/frc/robot/autochooser/event/RealAutoEventProvider.java b/src/main/java/frc/robot/autochooser/event/RealAutoEventProvider.java new file mode 100644 index 0000000..946a3bb --- /dev/null +++ b/src/main/java/frc/robot/autochooser/event/RealAutoEventProvider.java @@ -0,0 +1,121 @@ +package frc.robot.autochooser.event; + +import edu.wpi.first.networktables.NetworkTableValue; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import frc.robot.autochooser.AutoAction; +import frc.robot.autochooser.FieldLocation; +import frc.robot.utils.shuffleboard.SmartShuffleboard; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import java.util.function.Consumer; + +public class RealAutoEventProvider implements AutoEventProviderIO { + + public static final String AUTO_TAB_NAME = "Auto"; + public static final String ACTION_FIELD_NAME = "Auto Action"; + public static final String LOCATION_FIELD_NAME = "Location Chooser"; + public static final String AUTO_LOCATION_FEEDBACK_NAME = "FieldLocationFeedback"; + public static final String AUTO_ACTION_FEEDBACK_NAME = "AutoActionFeedback"; + + private final SendableChooser actionChooser; + private final SendableChooser locationChooser; + private final AutoAction defaultAutoAction; + private final FieldLocation defaultFieldLocation; + public final List onValidEvents = new ArrayList<>(); + + public RealAutoEventProvider(AutoAction defaultAutoAction, FieldLocation defaultFieldLocation) { + this.defaultAutoAction = defaultAutoAction; + this.defaultFieldLocation = defaultFieldLocation; + this.actionChooser = new SendableChooser<>(); + this.locationChooser = new SendableChooser<>(); + Arrays.stream(AutoAction.values()).forEach(a -> actionChooser.addOption(a.getName(), a)); + Arrays.stream(FieldLocation.values()) + .forEach(l -> locationChooser.addOption(l.getShuffleboardName(), l)); + actionChooser.setDefaultOption(defaultAutoAction.getName(), defaultAutoAction); + locationChooser.setDefaultOption( + defaultFieldLocation.getShuffleboardName(), defaultFieldLocation); + ShuffleboardTab autoTab = Shuffleboard.getTab(AUTO_TAB_NAME); + autoTab + .add(ACTION_FIELD_NAME, actionChooser) + .withWidget(BuiltInWidgets.kComboBoxChooser) + .withPosition(0, 0) + .withSize(4, 1); + autoTab.add(LOCATION_FIELD_NAME, locationChooser).withPosition(0, 1).withSize(4, 1); + SmartShuffleboard.put(AUTO_TAB_NAME, AUTO_ACTION_FEEDBACK_NAME, defaultAutoAction.toString()) + .withPosition(2, 2) + .withSize(2, 1); + SmartShuffleboard.put( + AUTO_TAB_NAME, AUTO_LOCATION_FEEDBACK_NAME, defaultFieldLocation.getShuffleboardName()) + .withPosition(0, 2) + .withSize(2, 1); + } + + /** + * @param listener function to be called when the value in {@link #actionChooser} changes + */ + @Override + public void setOnActionChangeListener(Consumer listener) { + actionChooser.onChange(listener); + } + + /** + * @param listener function to be called when the value in {@link #locationChooser} changes + */ + @Override + public void setOnLocationChangeListener(Consumer listener) { + locationChooser.onChange(listener); + } + + @Override + public void addOnValidationCommand(Runnable consumer) { + onValidEvents.add(consumer); + } + + @Override + public void runValidCommands() { + onValidEvents.forEach( + c -> { + try { + c.run(); + } catch (Exception e) { + throw new RuntimeException(e); + } + }); + } + + @Override + public void setFeedbackAction(AutoAction action) { + SmartShuffleboard.put(AUTO_TAB_NAME, AUTO_ACTION_FEEDBACK_NAME, action.toString()); + } + + @Override + public void setFeedbackLocation(FieldLocation location) { + SmartShuffleboard.put( + AUTO_TAB_NAME, AUTO_LOCATION_FEEDBACK_NAME, location.getShuffleboardName()); + } + + @Override + public void updateInputs(AutoChooserInputs inputs) { + inputs.action = actionChooser.getSelected(); + inputs.location = locationChooser.getSelected(); + inputs.defaultAction = defaultAutoAction; + inputs.defaultLocation = defaultFieldLocation; + NetworkTableValue feedbackLocation = + SmartShuffleboard.getValue(AUTO_TAB_NAME, AUTO_LOCATION_FEEDBACK_NAME); + if (feedbackLocation == null || feedbackLocation.getString().isBlank()) { + inputs.feedbackLocation = FieldLocation.INVALID; + } else { + inputs.feedbackLocation = FieldLocation.fromName(feedbackLocation.getString()); + } + NetworkTableValue action = SmartShuffleboard.getValue(AUTO_TAB_NAME, AUTO_ACTION_FEEDBACK_NAME); + if (action == null || action.getString().isBlank()) { + inputs.feedbackAction = AutoAction.INVALID; + } else { + inputs.feedbackAction = AutoAction.fromName(action.toString()); + } + } +}