diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a8b52d4..9cf8907 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.arm.ArmSubsystem; import frc.robot.autos.Autos; +import frc.robot.autos.trailblazer.Trailblazer; import frc.robot.config.RobotConfig; import frc.robot.fms.FmsSubsystem; import frc.robot.generated.BuildConstants; @@ -52,7 +53,8 @@ public class Robot extends TimedRobot { new RobotManager(arm, shooter, localization, vision, imu, intake, queuer, swerve); private final RobotCommands robotCommands = new RobotCommands(robotManager); - private final Autos autos = new Autos(robotCommands, robotManager, swerve, localization); + private final Trailblazer trailblazer = new Trailblazer(swerve, localization); + private final Autos autos = new Autos(robotManager, trailblazer); private final LightsSubsystem lights = new LightsSubsystem(robotManager, hardware.candle); diff --git a/src/main/java/frc/robot/autos/AutoChooser.java b/src/main/java/frc/robot/autos/AutoChooser.java index 350a409..ba7fd93 100644 --- a/src/main/java/frc/robot/autos/AutoChooser.java +++ b/src/main/java/frc/robot/autos/AutoChooser.java @@ -1,83 +1,27 @@ package frc.robot.autos; -import com.pathplanner.lib.auto.AutoBuilder; -import dev.doglog.DogLog; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.fms.FmsSubsystem; +import frc.robot.autos.trailblazer.Trailblazer; +import frc.robot.robot_manager.RobotManager; import java.util.EnumSet; -import java.util.HashSet; -import java.util.Optional; -import java.util.Set; public class AutoChooser { - private final SendableChooser chooser = new SendableChooser<>(); - private final Set brokenAutoNames = new HashSet<>(); - private final Command doNothingAuto; - private Optional cachedCommand = Optional.empty(); - private String cachedAutoName = ""; + private final SendableChooser chooser = new SendableChooser<>(); - public AutoChooser(AutoCommands commands) { + public AutoChooser(RobotManager robotManager, Trailblazer trailblazer) { SmartDashboard.putData("Autos/SelectedAuto", chooser); - chooser.setDefaultOption(AutoSelection.DO_NOTHING.toString(), AutoSelection.DO_NOTHING); for (AutoSelection selection : EnumSet.allOf(AutoSelection.class)) { - chooser.addOption(selection.toString(), selection); + chooser.addOption(selection.toString(), selection.auto.apply(robotManager, trailblazer)); } - this.doNothingAuto = commands.doNothingCommand(); + chooser.setDefaultOption( + AutoSelection.DO_NOTHING.toString(), + AutoSelection.DO_NOTHING.auto.apply(robotManager, trailblazer)); } - public Command getAutoCommand() { - var selection = getAutoSelection(); - - String autoName = FmsSubsystem.isRedAlliance() ? selection.redAutoName : selection.blueAutoName; - - // When the name of the auto changes (either from changing alliance color or from making a new - // selection), clear out the cached command - if (!autoName.equals(cachedAutoName)) { - cachedCommand = Optional.empty(); - } - - cachedAutoName = autoName; - - DogLog.log( - "Autos/BrokenAutoNames", brokenAutoNames.toArray(new String[brokenAutoNames.size()])); - DogLog.log("Autos/SelectedAutoName", selection); - DogLog.log("Autos/SelectedAutoFileName", autoName.equals("") ? "(none)" : autoName); - - if (cachedCommand.isPresent()) { - return cachedCommand.get(); - } - - if (autoName.equals("")) { - var command = Commands.print("No auto path provided, running do nothing auto"); - cachedCommand = Optional.of(command.andThen(doNothingAuto)); - return command; - } - - try { - var command = - AutoBuilder.buildAuto(autoName) - .withName("Auto__" + selection.toString() + "__" + autoName); - - cachedCommand = Optional.of(command); - return command; - } catch (Exception e) { - var alliance = FmsSubsystem.isRedAlliance() ? "red" : "blue"; - brokenAutoNames.add(selection.toString() + " - " + alliance + ": " + autoName); - } - - var command = Commands.print("Auto failed to load, check Autos/BrokenAutoNames"); - cachedCommand = Optional.of(command); - return command; - } - - public AutoSelection getAutoSelection() { - var selected = Optional.ofNullable(chooser.getSelected()); - - return selected.orElse(AutoSelection.DO_NOTHING); + public BaseAuto getSelectedAuto() { + return chooser.getSelected(); } } diff --git a/src/main/java/frc/robot/autos/AutoSelection.java b/src/main/java/frc/robot/autos/AutoSelection.java index 52d28c8..7e3e757 100644 --- a/src/main/java/frc/robot/autos/AutoSelection.java +++ b/src/main/java/frc/robot/autos/AutoSelection.java @@ -1,19 +1,18 @@ package frc.robot.autos; +import frc.robot.autos.amp.Op345Auto; +import frc.robot.autos.trailblazer.Trailblazer; +import frc.robot.robot_manager.RobotManager; +import java.util.function.BiFunction; + public enum AutoSelection { - DO_NOTHING("", ""), + DO_NOTHING(DoNothingAuto::new), - AMP_5_PIECE("Red Amp 5 Piece", "Blue Amp 5 Piece"), - SOURCE_4_PIECE("Red Source 4 Piece", "Blue Source 4 Piece"), - SOURCE_OFFSET("Red Source Offset", "Blue Source Offset"), - MID_5_PIECE("Red 5 Piece 6 to 1", "Blue 5 Piece 6 to 1"), - OP("Red OP", "Blue OP"); + OP(Op345Auto::new); - public final String redAutoName; - public final String blueAutoName; + public final BiFunction auto; - private AutoSelection(String redAutoName, String blueAutoName) { - this.redAutoName = redAutoName; - this.blueAutoName = blueAutoName; + private AutoSelection(BiFunction auto) { + this.auto = auto; } } diff --git a/src/main/java/frc/robot/autos/Autos.java b/src/main/java/frc/robot/autos/Autos.java index 0c2e7e9..4b05c71 100644 --- a/src/main/java/frc/robot/autos/Autos.java +++ b/src/main/java/frc/robot/autos/Autos.java @@ -1,90 +1,21 @@ package frc.robot.autos; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PIDConstants; -import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.localization.LocalizationSubsystem; -import frc.robot.robot_manager.RobotCommands; +import frc.robot.autos.trailblazer.Trailblazer; import frc.robot.robot_manager.RobotManager; -import frc.robot.swerve.SwerveSubsystem; import frc.robot.util.scheduling.LifecycleSubsystem; import frc.robot.util.scheduling.SubsystemPriority; public class Autos extends LifecycleSubsystem { - - private static Command wrapAutoEvent(String commandName, Command command) { - return Commands.sequence( - Commands.print("[COMMANDS] Starting auto event " + commandName), - command.deadlineWith( - Commands.waitSeconds(5) - .andThen( - Commands.print( - "[COMMANDS] Auto event " - + commandName - + " has been running for 5+ seconds!"))), - Commands.print("[COMMANDS] Finished auto event " + commandName)) - .handleInterrupt(() -> System.out.println("[COMMANDS] Cancelled auto event " + commandName)) - .withName(commandName); - } - - private static void registerCommand(String eventName, Command command) { - NamedCommands.registerCommand(eventName, wrapAutoEvent("Auto_" + eventName, command)); - } - - private final RobotCommands robotCommands; private final AutoChooser autoChooser; - private final AutoCommands autoCommands; - private final SwerveSubsystem swerve; - private final LocalizationSubsystem localization; - public Autos( - RobotCommands robotCommands, - RobotManager robotManager, - SwerveSubsystem swerve, - LocalizationSubsystem localization) { + public Autos(RobotManager robotManager, Trailblazer trailblazer) { super(SubsystemPriority.AUTOS); - this.robotCommands = robotCommands; - this.swerve = swerve; - this.localization = localization; - - autoCommands = new AutoCommands(robotCommands, robotManager); - - autoChooser = new AutoChooser(autoCommands); - - AutoBuilder.configureHolonomic( - localization::getPose, - localization::resetPose, - swerve::getRobotRelativeSpeeds, - (robotRelativeSpeeds) -> { - swerve.setRobotRelativeAutoSpeeds(robotRelativeSpeeds); - }, - new HolonomicPathFollowerConfig( - new PIDConstants(4.0, 0.0, 0.0), - new PIDConstants(2.5, 0.0, 0.0), - 4.4, - 0.387, - new ReplanningConfig(true, true)), - () -> false, - swerve); - - registerCommand("speakerShot", autoCommands.speakerShotWithTimeout()); - registerCommand("intakeAssist", robotCommands.intakeAssistCommand()); - registerCommand("dynamic5Piece", autoCommands.dynamicAmp5PieceCommand()); - registerCommand("waitingSpeakerCommand", robotCommands.waitSpeakerCommand()); + autoChooser = new AutoChooser(robotManager, trailblazer); } public Command getAutoCommand() { - return autoChooser.getAutoCommand(); - } - - @Override - public void disabledPeriodic() { - // Constantly load the selected auto to avoid lag on auto init - getAutoCommand(); + return autoChooser.getSelectedAuto().getAutoCommand(); } } diff --git a/src/main/java/frc/robot/autos/BaseAuto.java b/src/main/java/frc/robot/autos/BaseAuto.java index 94cbbae..0f8cba1 100644 --- a/src/main/java/frc/robot/autos/BaseAuto.java +++ b/src/main/java/frc/robot/autos/BaseAuto.java @@ -1,7 +1,9 @@ package frc.robot.autos; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.autos.trailblazer.Trailblazer; +import frc.robot.fms.FmsSubsystem; import frc.robot.robot_manager.RobotCommands; import frc.robot.robot_manager.RobotManager; @@ -18,5 +20,11 @@ protected BaseAuto(RobotManager robotManager, Trailblazer trailblazer) { this.autoCommands = new AutoCommands(actions, robotManager); } - public abstract Command getAutoCommand(); + protected abstract Command getRedAutoCommand(); + + protected abstract Command getBlueAutoCommand(); + + public Command getAutoCommand() { + return Commands.either(getRedAutoCommand(), getBlueAutoCommand(), FmsSubsystem::isRedAlliance); + } } diff --git a/src/main/java/frc/robot/autos/DoNothingAuto.java b/src/main/java/frc/robot/autos/DoNothingAuto.java new file mode 100644 index 0000000..a1a0411 --- /dev/null +++ b/src/main/java/frc/robot/autos/DoNothingAuto.java @@ -0,0 +1,22 @@ +package frc.robot.autos; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.autos.trailblazer.Trailblazer; +import frc.robot.robot_manager.RobotManager; + +public class DoNothingAuto extends BaseAuto { + public DoNothingAuto(RobotManager robotManager, Trailblazer trailblazer) { + super(robotManager, trailblazer); + } + + @Override + protected Command getBlueAutoCommand() { + return Commands.none(); + } + + @Override + protected Command getRedAutoCommand() { + return Commands.none(); + } +} diff --git a/src/main/java/frc/robot/autos/red/amp/RedOp345Auto.java b/src/main/java/frc/robot/autos/amp/Op345Auto.java similarity index 86% rename from src/main/java/frc/robot/autos/red/amp/RedOp345Auto.java rename to src/main/java/frc/robot/autos/amp/Op345Auto.java index e705ad9..1de27d9 100644 --- a/src/main/java/frc/robot/autos/red/amp/RedOp345Auto.java +++ b/src/main/java/frc/robot/autos/amp/Op345Auto.java @@ -1,4 +1,4 @@ -package frc.robot.autos.red.amp; +package frc.robot.autos.amp; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -8,15 +8,20 @@ import frc.robot.autos.trailblazer.AutoPoint; import frc.robot.autos.trailblazer.AutoSegment; import frc.robot.autos.trailblazer.Trailblazer; -import frc.robot.robot_manager.RobotCommands; import frc.robot.robot_manager.RobotManager; -public class RedOp345Auto extends BaseAuto { - public RedOp345Auto(RobotManager robotManager, Trailblazer trailblazer, RobotCommands actions) { +public class Op345Auto extends BaseAuto { + public Op345Auto(RobotManager robotManager, Trailblazer trailblazer) { super(robotManager, trailblazer); } - public Command getAutoCommand() { + @Override + protected Command getBlueAutoCommand() { + return Commands.none(); + } + + @Override + protected Command getRedAutoCommand() { return Commands.sequence( Commands.print("example command on auto start"), trailblazer.followSegment(