Skip to content

Better Autonomous Delivery Logic #51

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Trigon5990 wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Better Autonomous Delivery Logic #51

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
84c3a8d
started implementing
Trigon5990 Feb 22, 2026
560caac
Merge branch 'main' into better-autonomous-delivery-logic
Trigon5990 Feb 22, 2026
5b93144
did stuff, haven't tested and prob doesn't work
Trigon5990 Feb 22, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/main/java/frc/trigon/lib
Open in desktop
Submodule lib updated 1 files
+9 −0 utilities/flippable/FlippablePose2d.java
96 changes: 71 additions & 25 deletions src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/AutonomousGenerator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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;
Expand All @@ -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<AutonomousState>
FIRST_AUTONOMOUS_CHOOSER = new LoggedDashboardChooser<>("FirstAutonomousChooser", new SendableChooser<>()),
Expand All @@ -32,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() {
Expand All @@ -45,30 +49,67 @@ 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(FOURTH_AUTONOMOUS_CHOOSER.get(), THIRD_AUTONOMOUS_CHOOSER.get(), FIFTH_AUTONOMOUS_CHOOSER.get()),
getCommandFromState(FIFTH_AUTONOMOUS_CHOOSER.get(), FOURTH_AUTONOMOUS_CHOOSER.get(), null)
getCommandFromState(1),
getCommandFromState(2),
getCommandFromState(3),
getCommandFromState(4),
getCommandFromState(5)
);
}

private static Command getCommandFromState(AutonomousState state, AutonomousState previousState, AutonomousState nextState) {
private static Command getCommandFromState(int index) {
AutonomousState state = getStateFromIndex(index);

if (state == null)
return Commands.none();

return switch (state) {
case DELIVERY ->
GeneralAutonomousCommands.getDeliveryCommand(previousState, AutonomousConstants.DELIVERY_TIMEOUT_SECONDS::get);
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(nextState, 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) {
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(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();

return AutonomousState.DELIVERY.expectedRobotPose.get().get();
}

private static Command getLogCommand() {
return new RunCommand(AutonomousGenerator::log);
}
Expand All @@ -82,15 +123,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() {
Expand All @@ -105,23 +149,25 @@ private static void configureAutonomousChooser(LoggedDashboardChooser<Autonomous
chooser.addDefaultOption("Nothing", null);
}

private static void configureClimbPositionChooser(LoggedDashboardChooser<AutonomousClimbPosition> 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 {
DELIVERY(false),
SCORE(true),
COLLECT_FROM_DEPOT(true),
COLLECT_FROM_NEUTRAL_ZONE(false);
DELIVERY(false, () -> 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<FlippableTranslation2d> expectedRobotPose;

AutonomousState(boolean isInAllianceZone) {
AutonomousState(boolean isInAllianceZone, Supplier<FlippableTranslation2d> expectedRobotPose) {
this.isInAllianceZone = isInAllianceZone;
this.expectedRobotPose = expectedRobotPose;
}
}

Expand Down
1 change: 0 additions & 1 deletion ...java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
1 change: 0 additions & 1 deletion src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
Loading