diff --git a/src/main/java/frc/team2412/robot/util/AutoLogic.java b/src/main/java/frc/team2412/robot/util/AutoLogic.java index 7c7852d7..8067393b 100644 --- a/src/main/java/frc/team2412/robot/util/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/AutoLogic.java @@ -81,7 +81,7 @@ public void registerCommands() { "MidSpeakerCenterLineN5N4N3", AutoPaths.midSpeakerCenterLineN3N2N1); NamedCommands.registerCommand( "LowSpeakerCenterLineN5N4N3", AutoPaths.lowSpeakerCenterLineN5N4N3); - NamedCommands.registerCommand("LowSpeakerCenterLineN5N4N3", AutoPaths.lowSpeakerCenterLineN5N4); + NamedCommands.registerCommand("LowSpeakerCenterLineN5N4", AutoPaths.lowSpeakerCenterLineN5N4); NamedCommands.registerCommand( "TopSpeakerCenterLineN1N2AutoLine1", AutoPaths.TopSpeakerCenterLineN1N2AutoLine1); NamedCommands.registerCommand( diff --git a/src/main/java/frc/team2412/robot/util/AutoPaths.java b/src/main/java/frc/team2412/robot/util/AutoPaths.java index 039c5fb4..e1dc7da2 100644 --- a/src/main/java/frc/team2412/robot/util/AutoPaths.java +++ b/src/main/java/frc/team2412/robot/util/AutoPaths.java @@ -11,115 +11,152 @@ import frc.team2412.robot.subsystems.LauncherSubsystem; public class AutoPaths { + private static Command registerAuto( + String autoName, Command command, String... primaryPathNames) { + PathPlannerAutos.registerAuto(autoName, primaryPathNames); + return command.withName(autoName); + } // Test Auto - public static SequentialCommandGroup testAuto = - new SequentialCommandGroup( - AutoLogic.getAutoCommand("TestPath"), - new ConditionalCommand( - AutoLogic.getAutoCommand("TestPathTrue"), - AutoLogic.getAutoCommand("TestPathFalse"), - () -> false)); + public static Command testAuto = + registerAuto( + "AutoLogicTest", + new SequentialCommandGroup( + AutoLogic.getAutoCommand("TestPath"), + new ConditionalCommand( + AutoLogic.getAutoCommand("TestPathTrue"), + AutoLogic.getAutoCommand("TestPathFalse"), + () -> false)), + "TestPath", + "TestPathTrue"); // Complex Autos public static Command TopSpeakerCenterLineN1N2AutoLine1 = - Commands.sequence( - SubwooferLaunchCommand(), - getAutoCommand("TopSpeakerQCenterLineN1"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN1LCenterLineN1"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN1QCenterLineN2")), - Commands.sequence( - getAutoCommand("QCenterLineN1QCenterLineN2"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN2LCenterLineN2"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN2LAutoLineN1")), - Commands.sequence(getAutoCommand("QCenterLineN2LAutoLineN1")), - () -> true)), - () -> true), - VisionLaunchCommand()); + registerAuto( + "TopSpeakerCenterLineN1N2AutoLine1", + Commands.sequence( + SubwooferLaunchCommand(), + getAutoCommand("TopSpeakerQCenterLineN1"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN1LCenterLineN1"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN1QCenterLineN2")), + Commands.sequence( + getAutoCommand("QCenterLineN1QCenterLineN2"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN2LCenterLineN2"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN2LAutoLineN1")), + Commands.sequence(getAutoCommand("QCenterLineN2LAutoLineN1")), + () -> true)), + () -> true), + VisionLaunchCommand()), + "TopSpeakerQCenterLineN1", + "QCenterLineN1LCenterLineN1", + "LCenterLineN1QCenterLineN2"); public static Command TopSpeakerCenterLineN1N2N3 = - Commands.sequence( - SubwooferLaunchCommand(), - getAutoCommand("TopSpeakerQCenterLineN1"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN1LCenterLineN1"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN1QCenterLineN2")), - Commands.sequence( - getAutoCommand("QCenterLineN1QCenterLineN2"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN2LCenterLineN2"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN2LCenterLineN3")), - Commands.sequence(getAutoCommand("QCenterLineN2LCenterLineN3")), - () -> true)), - () -> true), - VisionLaunchCommand()); + registerAuto( + "TopSpeakerCenterLineN1N2N3", + Commands.sequence( + SubwooferLaunchCommand(), + getAutoCommand("TopSpeakerQCenterLineN1"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN1LCenterLineN1"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN1QCenterLineN2")), + Commands.sequence( + getAutoCommand("QCenterLineN1QCenterLineN2"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN2LCenterLineN2"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN2LCenterLineN3")), + Commands.sequence(getAutoCommand("QCenterLineN2LCenterLineN3")), + () -> true)), + () -> true), + VisionLaunchCommand()), + "TopSpeakerQCenterLineN1", + "QCenterLineN1LCenterLineN1", + "LCenterLineN1QCenterLineN2"); public static Command midSpeakerCenterLineN3N2N1 = - Commands.sequence( - SubwooferLaunchCommand(), - getAutoCommand("MidSpeakerQCenterLineN3"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN3LCenterLineN3"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN3QCenterLineN2"), - Commands.either( - Commands.sequence( - getAutoCommand("N3QCenterLineN2LCenterLineN2"), - VisionLaunchCommand(), - getAutoCommand("N3LCenterLineN2LCenterLineN1")), - getAutoCommand("QCenterLineN2LCenterLineN1"), - () -> true)), - Commands.sequence( - getAutoCommand("QCenterLineN3QCenterLineN2"), - getAutoCommand("QCenterLineN2LCenterLineN2")), - () -> true), - VisionLaunchCommand()); + registerAuto( + "MidSpeakerCenterLineN5N4N3", + Commands.sequence( + SubwooferLaunchCommand(), + getAutoCommand("MidSpeakerQCenterLineN3"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN3LCenterLineN3"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN3QCenterLineN2"), + Commands.either( + Commands.sequence( + getAutoCommand("N3QCenterLineN2LCenterLineN2"), + VisionLaunchCommand(), + getAutoCommand("N3LCenterLineN2LCenterLineN1")), + getAutoCommand("QCenterLineN2LCenterLineN1"), + () -> true)), + Commands.sequence( + getAutoCommand("QCenterLineN3QCenterLineN2"), + getAutoCommand("QCenterLineN2LCenterLineN2")), + () -> true), + VisionLaunchCommand()), + "MidSpeakerQCenterLineN3", + "QCenterLineN3LCenterLineN3", + "LCenterLineN3QCenterLineN2", + "N3QCenterLineN2LCenterLineN2", + "N3LCenterLineN2LCenterLineN1"); public static Command lowSpeakerCenterLineN5N4 = - Commands.sequence( - SubwooferLaunchCommand(), - getAutoCommand("LowSpeakerQCenterLineN5"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN5LCenterLineN5"), - Commands.waitSeconds(0.5), - getAutoCommand("LCenterLineN5LCenterLineN4")), - Commands.sequence(getAutoCommand("QCenterLineN5LCenterLineN4")), - () -> false)); + registerAuto( + "LowSpeakerCenterLineN5N4", + Commands.sequence( + SubwooferLaunchCommand(), + getAutoCommand("LowSpeakerQCenterLineN5"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN5LCenterLineN5"), + Commands.waitSeconds(0.5), + getAutoCommand("LCenterLineN5LCenterLineN4")), + Commands.sequence(getAutoCommand("QCenterLineN5LCenterLineN4")), + () -> false)), + "LowSpeakerQCenterLineN5", + "QCenterLineN5LCenterLineN5", + "LCenterLineN5LCenterLineN4"); public static Command lowSpeakerCenterLineN5N4N3 = - Commands.sequence( - SubwooferLaunchCommand(), - getAutoCommand("LowSpeakerQCenterLineN5"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN5QCenterLineN4"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN4LCenterLineN4"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN4LCenterLineN3")), - Commands.sequence(getAutoCommand("QCenterLineN4LCenterLineN3")), - () -> true)), - Commands.sequence( - getAutoCommand("QCenterLineN5LCenterLineN4"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN4LCenterLineN3")), - () -> false), - VisionLaunchCommand()); + registerAuto( + "LowSpeakerCenterLineN5N4N3", + Commands.sequence( + SubwooferLaunchCommand(), + getAutoCommand("LowSpeakerQCenterLineN5"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN5QCenterLineN4"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN4LCenterLineN4"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN4LCenterLineN3")), + Commands.sequence(getAutoCommand("QCenterLineN4LCenterLineN3")), + () -> true)), + Commands.sequence( + getAutoCommand("QCenterLineN5LCenterLineN4"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN4LCenterLineN3")), + () -> false), + VisionLaunchCommand()), + "LowSpeakerQCenterLineN5", + "QCenterLineN5QCenterLineN4", + "QCenterLineN4LCenterLineN4", + "LCenterLineN4LCenterLineN3"); // new command getters diff --git a/src/main/java/frc/team2412/robot/util/AutonomousField.java b/src/main/java/frc/team2412/robot/util/AutonomousField.java index 9496cfda..451dba90 100644 --- a/src/main/java/frc/team2412/robot/util/AutonomousField.java +++ b/src/main/java/frc/team2412/robot/util/AutonomousField.java @@ -5,7 +5,9 @@ import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StringSubscriber; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.Watchdog; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.Field2d; @@ -39,7 +41,19 @@ public static void configureShuffleboardTab( .subscribe(""); var autonomousField = new AutonomousField(() -> speedMultiplier.getDouble(DEFAULT_PLAYBACK_SPEED)); - addPeriodic.accept(() -> autonomousField.update(activeAutoSub.get()), UPDATE_RATE); + var watchdog = + new Watchdog(0.001, () -> DriverStation.reportWarning("auto field loop overrun", false)); + addPeriodic.accept( + () -> { + watchdog.reset(); + autonomousField.update(activeAutoSub.get()); + watchdog.addEpoch("AutonomousField.update()"); + watchdog.disable(); + if (watchdog.isExpired()) { + watchdog.printEpochs(); + } + }, + UPDATE_RATE); tab.add("Selected auto", autonomousField.getField()) .withPosition(columnIndex, rowIndex) .withSize(2, 2); @@ -49,6 +63,7 @@ public static void configureShuffleboardTab( private final Field2d field = new Field2d(); // Keeping track of the current trajectory + private PathPlannerAutos.Auto auto; private List trajectories; private int trajectoryIndex = 0; @@ -92,12 +107,16 @@ public Pose2d getUpdatedPose(String autoName) { double fpgaTime = Timer.getFPGATimestamp(); if (lastName.isEmpty() || !lastName.get().equals(autoName)) { lastName = Optional.of(autoName); - trajectories = PathPlannerAutos.getAutoTrajectories(autoName); + auto = PathPlannerAutos.getAuto(autoName); + trajectories = auto.trajectories; trajectoryIndex = 0; lastFPGATime = fpgaTime; lastTrajectoryTimeOffset = 0; } if (trajectories.isEmpty()) { + if (auto.startingPose != null) { + return auto.startingPose; + } return new Pose2d(); } lastTrajectoryTimeOffset += (fpgaTime - lastFPGATime) * speed; @@ -115,11 +134,16 @@ public Pose2d getUpdatedPose(String autoName) { } /** - * Updates the {@link Field2d} robot pose. + * Updates the {@link Field2d} robot pose. If the robot is enabled, does nothing and the + * trajectory will restart when the robot is disabled. * * @param autoName The name of the selected PathPlanner autonomous routine. */ public void update(String autoName) { + if (DriverStation.isEnabled()) { + lastName = Optional.empty(); + return; + } field.setRobotPose(getUpdatedPose(autoName)); } } diff --git a/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java b/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java index c9c77f96..f5d924bc 100644 --- a/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java +++ b/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java @@ -4,6 +4,7 @@ import com.fasterxml.jackson.databind.ObjectMapper; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.path.PathPlannerTrajectory; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; @@ -17,6 +18,37 @@ import java.util.function.Function; public class PathPlannerAutos { + public static final class Auto { + public final Pose2d startingPose; + public final List trajectories; + + public Auto() { + this.startingPose = null; + this.trajectories = List.of(); + } + + public Auto(Pose2d startingPose, List trajectories) { + this.startingPose = startingPose; + this.trajectories = trajectories; + } + } + + // Registering named commands + + private static final Map> namedCommandPathsCache = new HashMap<>(); + + public static void registerAuto(String autoName, List paths) { + namedCommandPathsCache.put(autoName, paths); + } + + public static void registerAuto(String autoName, String... pathNames) { + List paths = new ArrayList<>(pathNames.length); + for (String pathName : pathNames) { + paths.add(getPathPlannerPath(pathName)); + } + registerAuto(autoName, paths); + } + // Individual paths private static PathPlannerPath loadPathPlannerPath(String name) { @@ -47,7 +79,8 @@ private static void addPathsFromCommand( case "wait": break; case "named": - // TODO Handle conditional paths (after those are added) + String commandName = commandJson.get("data").get("name").asText(); + paths.addAll(namedCommandPathsCache.getOrDefault(commandName, List.of())); break; case "path": String pathName = commandJson.get("data").get("pathName").asText(); @@ -69,10 +102,6 @@ private static List getPaths(JsonNode autoJson) { return paths; } - private static Rotation2d getStartingRotation(JsonNode autoJson) { - return Rotation2d.fromDegrees(autoJson.get("startingPose").get("rotation").asDouble()); - } - private static ChassisSpeeds speedsFromState(PathPlannerTrajectory.State state) { return new ChassisSpeeds( state.heading.getCos() * state.velocityMps, @@ -80,23 +109,14 @@ private static ChassisSpeeds speedsFromState(PathPlannerTrajectory.State state) state.headingAngularVelocityRps); } - private static List loadAutoTrajectories(String autoName) { - File autoFile = - new File(Filesystem.getDeployDirectory(), "pathplanner/autos/" + autoName + ".auto"); - if (!autoFile.exists()) { - DriverStation.reportWarning( - "Attempted to load non-existent auto \"" + autoName + "\"", false); + private static List trajectoriesFromPaths( + List paths, Rotation2d startingRotation) { + if (paths.isEmpty()) { return List.of(); } - JsonNode autoJson; - try { - autoJson = new ObjectMapper().readTree(autoFile); - } catch (IOException e) { - DriverStation.reportWarning("Could not load auto \"" + autoName + "\"", e.getStackTrace()); - return List.of(); + if (startingRotation == null) { + startingRotation = paths.get(0).getPreviewStartingHolonomicPose().getRotation(); } - List paths = getPaths(autoJson); - Rotation2d startingRotation = getStartingRotation(autoJson); ChassisSpeeds startingSpeeds = new ChassisSpeeds(); List trajectories = new ArrayList<>(paths.size()); for (var path : paths) { @@ -108,11 +128,44 @@ private static List loadAutoTrajectories(String autoName) return List.copyOf(trajectories); } - private static final Map> autoTrajectoriesCache = - new HashMap<>(); + private static Pose2d getStartingPose(JsonNode autoJson) { + JsonNode startingPose = autoJson.get("startingPose"); + if (startingPose.isNull()) { + return null; + } + JsonNode translation = startingPose.get("position"); + return new Pose2d( + translation.get("x").asDouble(), + translation.get("y").asDouble(), + Rotation2d.fromDegrees(startingPose.get("rotation").asDouble())); + } + + private static Auto loadAuto(String autoName) { + File autoFile = + new File(Filesystem.getDeployDirectory(), "pathplanner/autos/" + autoName + ".auto"); + if (!autoFile.exists()) { + DriverStation.reportWarning( + "Attempted to load non-existent auto \"" + autoName + "\"", false); + return new Auto(); + } + JsonNode autoJson; + try { + autoJson = new ObjectMapper().readTree(autoFile); + } catch (IOException e) { + DriverStation.reportWarning("Could not load auto \"" + autoName + "\"", e.getStackTrace()); + return new Auto(); + } + Pose2d startingPose = getStartingPose(autoJson); + Rotation2d startingRotation = startingPose == null ? null : startingPose.getRotation(); + List trajectories = + trajectoriesFromPaths(getPaths(autoJson), startingRotation); + return new Auto(startingPose, trajectories); + } + + private static final Map autosCache = new HashMap<>(); - public static List getAutoTrajectories(String autoName) { - return autoTrajectoriesCache.computeIfAbsent(autoName, PathPlannerAutos::loadAutoTrajectories); + public static Auto getAuto(String autoName) { + return autosCache.computeIfAbsent(autoName, PathPlannerAutos::loadAuto); } // Pre-loading everything @@ -157,10 +210,6 @@ public static void loadAllChoreoTrajectories() { } public static void loadAllAutoTrajectories() { - loadAll( - "pathplanner/autos", - ".auto", - autoTrajectoriesCache, - PathPlannerAutos::loadAutoTrajectories); + loadAll("pathplanner/autos", ".auto", autosCache, PathPlannerAutos::loadAuto); } }