diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 529a7cf..1a7c65b 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -188,6 +188,17 @@ public Autos( LoggedNetworkTablesBuilder.createLoggedAutoResettingButton("Autos/Reset Odometry") .onTrue(new InstantCommand(this::resetAutoOdometry).ignoringDisable(true))); + autoChooser.onChange( + (Auto auto) -> { + try { + // Construct this auto to warm up the JSON parser and get all the paths into + // PathPlanner's path cache. + PathPlannerAuto pp_auto = new PathPlannerAuto(auto.autoName); + DriverStation.reportWarning("Preloaded auto " + pp_auto.getName(), false); + } catch (Exception e) { + DriverStation.reportError("Error preloading auto " + auto.autoName, false); + } + }); bindNamedCommands(); bindEventMarkers(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bceb541..1957493 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,5 +1,6 @@ package frc.robot; +import com.pathplanner.lib.commands.FollowPathCommand; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.controls.Controls; import frc.robot.generated.TunerConstants; @@ -97,6 +98,7 @@ public RobotContainer() { leds = new Leds(new LedsReal()); deploy = new Deploy(new DeployIO() {}); hood = new Hood(new HoodIOReal()); + break; case SIM: @@ -156,6 +158,8 @@ public RobotContainer() { controls = new Controls(drive, intake, shooter, indexer, booster, hood, fuelDetection, leds, deploy); autos = new Autos(drive, indexer, intake, shooter, booster, hood, leds, deploy); + // Warm up the PathPlaner system. + FollowPathCommand.warmupCommand(); } public Command getAutonomousCommand() {