From 84c3a8d3c6bfc6112387e8c2ef764a320b502c91 Mon Sep 17 00:00:00 2001 From: Trigon5990 Date: Sun, 22 Feb 2026 21:26:19 +0200 Subject: [PATCH 1/2] started implementing Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- src/main/java/frc/trigon/lib | 2 +- .../autonomous/AutonomousGenerator.java | 60 +++++++++++++------ 2 files changed, 44 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib index 33193d5..1a05839 160000 --- a/src/main/java/frc/trigon/lib +++ b/src/main/java/frc/trigon/lib @@ -1 +1 @@ -Subproject commit 33193d5c42b4f36076569d714d255f85c1acc381 +Subproject commit 1a058398175c36b6aba72c10ae84f5b151468b4d diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/AutonomousGenerator.java b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/AutonomousGenerator.java index 259b745..7516a6d 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/AutonomousGenerator.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/AutonomousGenerator.java @@ -1,12 +1,14 @@ package frc.trigon.robot.commands.commandfactories.autonomous; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.trigon.lib.utilities.flippable.FlippablePose2d; +import frc.trigon.lib.utilities.flippable.FlippableTranslation2d; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.FieldConstants; @@ -16,6 +18,8 @@ import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; +import java.util.function.Supplier; + public class AutonomousGenerator { public static final LoggedDashboardChooser FIRST_AUTONOMOUS_CHOOSER = new LoggedDashboardChooser<>("FirstAutonomousChooser", new SendableChooser<>()), @@ -45,23 +49,23 @@ public static Command getAutonomousCommand() { private static Command getAutonomousStateSequenceCommand() { return new SequentialCommandGroup( - getCommandFromState(FIRST_AUTONOMOUS_CHOOSER.get(), null, SECOND_AUTONOMOUS_CHOOSER.get()), - getCommandFromState(SECOND_AUTONOMOUS_CHOOSER.get(), FIRST_AUTONOMOUS_CHOOSER.get(), THIRD_AUTONOMOUS_CHOOSER.get()), - getCommandFromState(THIRD_AUTONOMOUS_CHOOSER.get(), SECOND_AUTONOMOUS_CHOOSER.get(), FOURTH_AUTONOMOUS_CHOOSER.get()), + getCommandFromState(FIRST_AUTONOMOUS_CHOOSER.get(), null, SECOND_AUTONOMOUS_CHOOSER.get(), THIRD_AUTONOMOUS_CHOOSER.get(), FOURTH_AUTONOMOUS_CHOOSER.get(), FIFTH_AUTONOMOUS_CHOOSER.get()), + getCommandFromState(SECOND_AUTONOMOUS_CHOOSER.get(), FIRST_AUTONOMOUS_CHOOSER.get(), THIRD_AUTONOMOUS_CHOOSER.get(), FOURTH_AUTONOMOUS_CHOOSER.get(), FIFTH_AUTONOMOUS_CHOOSER.get()), + getCommandFromState(THIRD_AUTONOMOUS_CHOOSER.get(), SECOND_AUTONOMOUS_CHOOSER.get(), FOURTH_AUTONOMOUS_CHOOSER.get(), FIFTH_AUTONOMOUS_CHOOSER.get()), getCommandFromState(FOURTH_AUTONOMOUS_CHOOSER.get(), THIRD_AUTONOMOUS_CHOOSER.get(), FIFTH_AUTONOMOUS_CHOOSER.get()), - getCommandFromState(FIFTH_AUTONOMOUS_CHOOSER.get(), FOURTH_AUTONOMOUS_CHOOSER.get(), null) + getCommandFromState(FIFTH_AUTONOMOUS_CHOOSER.get(), FOURTH_AUTONOMOUS_CHOOSER.get()) ); } - private static Command getCommandFromState(AutonomousState state, AutonomousState previousState, AutonomousState nextState) { + private static Command getCommandFromState(AutonomousState state, AutonomousState previousState, AutonomousState... nextStates) { if (state == null) return Commands.none(); return switch (state) { case DELIVERY -> - GeneralAutonomousCommands.getDeliveryCommand(previousState, AutonomousConstants.DELIVERY_TIMEOUT_SECONDS::get); + GeneralAutonomousCommands.getDeliveryCommand(previousState, () -> getDeliveryTimeout(nextStates == null ? new AutonomousState[]{} : nextStates)); case SCORE -> - GeneralAutonomousCommands.getScoreCommand(nextState, AutonomousConstants.SCORING_TIMEOUT_SECONDS); + GeneralAutonomousCommands.getScoreCommand(nextStates == null ? null : nextStates[0], AutonomousConstants.SCORING_TIMEOUT_SECONDS); case COLLECT_FROM_DEPOT -> GeneralAutonomousCommands.getCollectFromDepotCommand(true, AutonomousConstants.DEPOT_COLLECTION_TIMEOUT_SECONDS); case COLLECT_FROM_NEUTRAL_ZONE -> @@ -69,6 +73,23 @@ private static Command getCommandFromState(AutonomousState state, AutonomousStat }; } + private static double getDeliveryTimeout(AutonomousState[] nextStates) { + double timeToLeave = 0; + for (AutonomousState nextState : nextStates) { + switch (nextState) { + case SCORE -> timeToLeave += AutonomousConstants.SCORING_TIMEOUT_SECONDS; + case COLLECT_FROM_DEPOT -> timeToLeave += AutonomousConstants.DEPOT_COLLECTION_TIMEOUT_SECONDS; + case COLLECT_FROM_NEUTRAL_ZONE -> timeToLeave += AutonomousConstants.NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS; + } + } + + if (shouldClimb()) + timeToLeave += calculateTimeToLeaveForClimbSeconds(nextStates.length == 0 ? AutonomousState.DELIVERY.expectedRobotPose.get().get() : nextStates[nextStates.length - 1].expectedRobotPose.get().get()) + 0.5; + + // implement logic to deal with earlier states time + return 0; + } + private static Command getLogCommand() { return new RunCommand(AutonomousGenerator::log); } @@ -81,15 +102,18 @@ private static void log() { } private static boolean shouldStartDrivingToClimb() { + final double timeToLeaveForClimbSeconds = calculateTimeToLeaveForClimbSeconds(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation()); + return MatchTracker.getMatchTimeSeconds() <= AutonomousConstants.TOTAL_MATCH_TIME_SECONDS - AutonomousConstants.AUTONOMOUS_TIME_SECONDS + timeToLeaveForClimbSeconds; + } + + private static double calculateTimeToLeaveForClimbSeconds(Translation2d robotPose) { if (!shouldClimb() || !IS_AUTONOMOUS_CLIMB_HIGHEST_PRIORITY.get()) - return false; + return 0; - final Pose2d currentRobotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); final Pose2d targetClimbPose = CLIMB_POSITION_CHOOSER.get().climbPose.get(); - final double distanceToClimbPoseMeters = currentRobotPose.getTranslation().getDistance(targetClimbPose.getTranslation()); + final double distanceToClimbPoseMeters = robotPose.getDistance(targetClimbPose.getTranslation()); final double estimatedDriveTimeSeconds = distanceToClimbPoseMeters / AutonomousConstants.ROBOT_AVERAGE_SPEED_METERS_PER_SECOND; - final double timeToLeaveForClimbSeconds = AutonomousConstants.ESTIMATED_CLIMBING_TIME_SECONDS + estimatedDriveTimeSeconds + AutonomousConstants.CLIMB_DRIVE_TIME_SAFETY_MARGIN_SECONDS; - return MatchTracker.getMatchTimeSeconds() <= AutonomousConstants.TOTAL_MATCH_TIME_SECONDS - AutonomousConstants.AUTONOMOUS_TIME_SECONDS + timeToLeaveForClimbSeconds; + return AutonomousConstants.ESTIMATED_CLIMBING_TIME_SECONDS + estimatedDriveTimeSeconds + AutonomousConstants.CLIMB_DRIVE_TIME_SAFETY_MARGIN_SECONDS; } public static boolean shouldClimb() { @@ -112,15 +136,17 @@ private static void configureClimbPositionChooser(LoggedDashboardChooser SafeAutonomousDriveCommands.isRight() ? FieldConstants.RIGHT_DELIVERY_POSITION : FieldConstants.LEFT_DELIVERY_POSITION), + SCORE(true, () -> SafeAutonomousDriveCommands.isRight() ? FieldConstants.RIGHT_IDEAL_SHOOTING_POSITION.getTranslation() : FieldConstants.LEFT_IDEAL_SHOOTING_POSITION.getTranslation()), + COLLECT_FROM_DEPOT(true, FieldConstants.DEPOT_POSITION::getTranslation), + COLLECT_FROM_NEUTRAL_ZONE(false, () -> SafeAutonomousDriveCommands.isRight() ? FieldConstants.RIGHT_INTAKE_POSITION.getTranslation() : FieldConstants.LEFT_INTAKE_POSITION.getTranslation()); final boolean isInAllianceZone; + final Supplier expectedRobotPose; - AutonomousState(boolean isInAllianceZone) { + AutonomousState(boolean isInAllianceZone, Supplier expectedRobotPose) { this.isInAllianceZone = isInAllianceZone; + this.expectedRobotPose = expectedRobotPose; } } From 5b931448609a28b12e309d18ddb66bbfb9e87b16 Mon Sep 17 00:00:00 2001 From: Trigon5990 Date: Sun, 22 Feb 2026 21:56:13 +0200 Subject: [PATCH 2/2] did stuff, haven't tested and prob doesn't work Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../autonomous/AutonomousGenerator.java | 58 +++++++++++++------ .../autonomous/GeneralAutonomousCommands.java | 1 - .../robot/constants/AutonomousConstants.java | 1 - 3 files changed, 39 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/AutonomousGenerator.java b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/AutonomousGenerator.java index 98d9fdf..7ff6579 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/AutonomousGenerator.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/AutonomousGenerator.java @@ -36,7 +36,7 @@ public static void init() { configureAutonomousChooser(THIRD_AUTONOMOUS_CHOOSER); configureAutonomousChooser(FOURTH_AUTONOMOUS_CHOOSER); configureAutonomousChooser(FIFTH_AUTONOMOUS_CHOOSER); - configureClimbPositionChooser(CLIMB_POSITION_CHOOSER); + configureClimbPositionChooser(); } public static Command getAutonomousCommand() { @@ -49,31 +49,44 @@ public static Command getAutonomousCommand() { private static Command getAutonomousStateSequenceCommand() { return new SequentialCommandGroup( - getCommandFromState(FIRST_AUTONOMOUS_CHOOSER.get(), null, SECOND_AUTONOMOUS_CHOOSER.get(), THIRD_AUTONOMOUS_CHOOSER.get(), FOURTH_AUTONOMOUS_CHOOSER.get(), FIFTH_AUTONOMOUS_CHOOSER.get()), - getCommandFromState(SECOND_AUTONOMOUS_CHOOSER.get(), FIRST_AUTONOMOUS_CHOOSER.get(), THIRD_AUTONOMOUS_CHOOSER.get(), FOURTH_AUTONOMOUS_CHOOSER.get(), FIFTH_AUTONOMOUS_CHOOSER.get()), - getCommandFromState(THIRD_AUTONOMOUS_CHOOSER.get(), SECOND_AUTONOMOUS_CHOOSER.get(), FOURTH_AUTONOMOUS_CHOOSER.get(), FIFTH_AUTONOMOUS_CHOOSER.get()), - getCommandFromState(FOURTH_AUTONOMOUS_CHOOSER.get(), THIRD_AUTONOMOUS_CHOOSER.get(), FIFTH_AUTONOMOUS_CHOOSER.get()), - getCommandFromState(FIFTH_AUTONOMOUS_CHOOSER.get(), FOURTH_AUTONOMOUS_CHOOSER.get()) + getCommandFromState(1), + getCommandFromState(2), + getCommandFromState(3), + getCommandFromState(4), + getCommandFromState(5) ); } - private static Command getCommandFromState(AutonomousState state, AutonomousState previousState, AutonomousState... nextStates) { + private static Command getCommandFromState(int index) { + AutonomousState state = getStateFromIndex(index); + if (state == null) return Commands.none(); return switch (state) { case DELIVERY -> - GeneralAutonomousCommands.getDeliveryCommand(previousState, () -> getDeliveryTimeout(nextStates == null ? new AutonomousState[]{} : nextStates)); + GeneralAutonomousCommands.getDeliveryCommand(getStateFromIndex(index - 1), () -> getDeliveryTimeout(getStateFromIndex(index + 1) == null ? null : getStateFromIndex(index + 1), getStateFromIndex(index + 2), getStateFromIndex(index + 3), getStateFromIndex(index + 4))); case SCORE -> - GeneralAutonomousCommands.getScoreCommand(nextStates == null ? null : nextStates[0], AutonomousConstants.SCORING_TIMEOUT_SECONDS); + GeneralAutonomousCommands.getScoreCommand(getStateFromIndex(index + 1) == null ? null : getStateFromIndex(index + 1), AutonomousConstants.SCORING_TIMEOUT_SECONDS); case COLLECT_FROM_DEPOT -> GeneralAutonomousCommands.getCollectFromDepotCommand(true, AutonomousConstants.DEPOT_COLLECTION_TIMEOUT_SECONDS); case COLLECT_FROM_NEUTRAL_ZONE -> - GeneralAutonomousCommands.getCollectFromNeutralZoneCommand(previousState, AutonomousConstants.NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS); + GeneralAutonomousCommands.getCollectFromNeutralZoneCommand(getStateFromIndex(index - 1), AutonomousConstants.NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS); + }; + } + + private static AutonomousState getStateFromIndex(int index) { + return switch (index) { + case 0 -> FIRST_AUTONOMOUS_CHOOSER.get(); + case 1 -> SECOND_AUTONOMOUS_CHOOSER.get(); + case 2 -> THIRD_AUTONOMOUS_CHOOSER.get(); + case 3 -> FOURTH_AUTONOMOUS_CHOOSER.get(); + case 4 -> FIFTH_AUTONOMOUS_CHOOSER.get(); + default -> null; }; } - private static double getDeliveryTimeout(AutonomousState[] nextStates) { + private static double getDeliveryTimeout(AutonomousState... nextStates) { double timeToLeave = 0; for (AutonomousState nextState : nextStates) { switch (nextState) { @@ -84,10 +97,17 @@ private static double getDeliveryTimeout(AutonomousState[] nextStates) { } if (shouldClimb()) - timeToLeave += calculateTimeToLeaveForClimbSeconds(nextStates.length == 0 ? AutonomousState.DELIVERY.expectedRobotPose.get().get() : nextStates[nextStates.length - 1].expectedRobotPose.get().get()) + 0.5; + timeToLeave += calculateTimeToLeaveForClimbSeconds(getLastValidPoseOrDefault(nextStates)) + 0.5; + + return timeToLeave; + } + + private static Translation2d getLastValidPoseOrDefault(AutonomousState[] nextStates) { + for (int i = nextStates.length - 1; i >= 0; i--) + if (nextStates[i] != null) + return nextStates[i].expectedRobotPose.get().get(); - // implement logic to deal with earlier states time - return 0; + return AutonomousState.DELIVERY.expectedRobotPose.get().get(); } private static Command getLogCommand() { @@ -129,11 +149,11 @@ private static void configureAutonomousChooser(LoggedDashboardChooser chooser) { - chooser.addOption("RightL1", AutonomousClimbPosition.RIGHT_L1); - chooser.addOption("LeftL1", AutonomousClimbPosition.LEFT_L1); - chooser.addOption("CenterL1", AutonomousClimbPosition.CENTER_L1); - chooser.addDefaultOption("NoClimb", AutonomousClimbPosition.NO_CLIMB); + private static void configureClimbPositionChooser() { + AutonomousGenerator.CLIMB_POSITION_CHOOSER.addOption("RightL1", AutonomousClimbPosition.RIGHT_L1); + AutonomousGenerator.CLIMB_POSITION_CHOOSER.addOption("LeftL1", AutonomousClimbPosition.LEFT_L1); + AutonomousGenerator.CLIMB_POSITION_CHOOSER.addOption("CenterL1", AutonomousClimbPosition.CENTER_L1); + AutonomousGenerator.CLIMB_POSITION_CHOOSER.addDefaultOption("NoClimb", AutonomousClimbPosition.NO_CLIMB); } public enum AutonomousState { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java index 2ea9e54..83b51ab 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java @@ -21,7 +21,6 @@ import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.climber.ClimberConstants; import frc.trigon.robot.subsystems.hood.HoodCommands; -import frc.trigon.robot.subsystems.hood.HoodConstants; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index 12feccd..ed55527 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -35,7 +35,6 @@ public class AutonomousConstants { SHOOT_PRELOAD_BEFORE_NEUTRAL_ZONE_TIME_SECONDS = 1, SHOOT_PRELOAD_BEFORE_COLLECTING_FROM_DEPOT_TIME = 2; - public static final LoggedNetworkNumber DELIVERY_TIMEOUT_SECONDS = new LoggedNetworkNumber("DeliveryTimeoutSeconds", 6); public static final double TOTAL_MATCH_TIME_SECONDS = 160, AUTONOMOUS_TIME_SECONDS = 20,