From a55356ac5fbb9d6c2ad703432aed9bc110f70658 Mon Sep 17 00:00:00 2001 From: Surya Thoppae <63197592+suryatho@users.noreply.github.com> Date: Sat, 13 Apr 2024 17:13:22 -0400 Subject: [PATCH] Davis inspirational auto (#258) * Add inspirational auto * Critical no comma error * Better source trajectory * Replace 14 second option with last second initiation * Remove debug log * Adjustments --------- Co-authored-by: nharnwal Co-authored-by: Jonah <47046556+jwbonner@users.noreply.github.com> --- .../frc2024/AutoSelector.java | 7 +- .../frc2024/RobotContainer.java | 18 +++++ .../frc2024/commands/auto/AutoBuilder.java | 71 +++++++++++++++++++ .../drive/trajectory/DriveTrajectories.java | 40 +++++++++++ 4 files changed, 135 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java b/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java index 0fc9e084..23adc493 100644 --- a/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java +++ b/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java @@ -146,6 +146,11 @@ public static enum AutoQuestionResponse { AMP_WALL, SCORE_POOPED, FOURTH_CENTER, - THINKING_ON_YOUR_FEET + THINKING_ON_YOUR_FEET, + IMMEDIATELY, + SIX_SECONDS, + LAST_SECOND, + YES, + NO } } diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index fb6f84e0..bc307928 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -375,6 +375,24 @@ private void configureAutos() { "First center note?", List.of(AutoQuestionResponse.SOURCE_WALL, AutoQuestionResponse.SOURCE_MIDDLE))), autoBuilder.davisUnethicalAuto()); + autoSelector.addRoutine( + "Davis Inspirational Auto", + List.of( + new AutoQuestion( + "Starting subwoofer location?", + List.of( + AutoQuestionResponse.SOURCE, + AutoQuestionResponse.CENTER, + AutoQuestionResponse.AMP)), + new AutoQuestion( + "Earn mobility bonus?", List.of(AutoQuestionResponse.YES, AutoQuestionResponse.NO)), + new AutoQuestion( + "Mobility delay time?", + List.of( + AutoQuestionResponse.IMMEDIATELY, + AutoQuestionResponse.SIX_SECONDS, + AutoQuestionResponse.LAST_SECOND))), + autoBuilder.davisInspirationalAuto()); // Set up feedforward characterization autoSelector.addRoutine( diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java index a36182e5..2aa80592 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java @@ -695,4 +695,75 @@ private Command unethical_poopThenScoreCenterlines( superstructure.aimWithCompensation( secondShotCompensation))))))); } + + public Command davisInspirationalAuto() { + HolonomicTrajectory leaveFromSource = new HolonomicTrajectory("inspirational_leaveFromSource"); + HolonomicTrajectory leaveFromCenter = new HolonomicTrajectory("inspirational_leaveFromCenter"); + HolonomicTrajectory leaveFromAmp = new HolonomicTrajectory("inspirational_leaveFromAmp"); + Timer autoTimer = new Timer(); + return Commands.runOnce(autoTimer::restart) + .andThen( + Commands.parallel( + Commands.sequence( + waitUntilXCrossed(FieldConstants.startingLineX, true), + Commands.runOnce( + () -> System.out.println("Crossed starting line at " + autoTimer.get()))), + Commands.select( + Map.of( + AutoQuestionResponse.SOURCE, + resetPose(DriveTrajectories.startingSourceSubwoofer), + AutoQuestionResponse.CENTER, + resetPose(DriveTrajectories.startingCenter), + AutoQuestionResponse.AMP, + resetPose(DriveTrajectories.startingAmpSubwoofer)), + () -> responses.get().get(0) // Starting location + ) + .andThen( + // Shoot preload in one second + Commands.waitSeconds(1.0 - shootTimeoutSecs.get()) + .andThen(feed(rollers)) + .deadlineWith( + flywheels.shootCommand(), superstructure.aimWithCompensation(0)), + + // Wait time + Commands.select( + Map.of( + AutoQuestionResponse.IMMEDIATELY, + Commands.none(), + AutoQuestionResponse.SIX_SECONDS, + Commands.waitSeconds(5.0), + AutoQuestionResponse.LAST_SECOND, + Commands.select( + Map.of( + AutoQuestionResponse.SOURCE, + Commands.waitSeconds( + 13.5 - leaveFromSource.getDuration()), + AutoQuestionResponse.CENTER, + Commands.waitSeconds( + 13.5 - leaveFromCenter.getDuration()), + AutoQuestionResponse.AMP, + Commands.waitSeconds( + 13.5 - leaveFromAmp.getDuration())), + () -> responses.get().get(0))), + () -> responses.get().get(2)) + .andThen( + Commands.select( + Map.of( + AutoQuestionResponse.SOURCE, + followTrajectory( + drive, + new HolonomicTrajectory( + "inspirational_leaveFromSource")), + AutoQuestionResponse.CENTER, + followTrajectory( + drive, + new HolonomicTrajectory( + "inspirational_leaveFromCenter")), + AutoQuestionResponse.AMP, + followTrajectory( + drive, + new HolonomicTrajectory("inspirational_leaveFromAmp"))), + () -> responses.get().get(0))) + .onlyIf(() -> responses.get().get(1) == AutoQuestionResponse.YES)))); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java index 62dc68b3..ffd8235b 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; import java.util.ArrayList; import java.util.HashMap; import java.util.List; @@ -58,6 +59,13 @@ public class DriveTrajectories { Rotation2d.fromDegrees(180.0)); public static final Pose2d startingFarSource = new Pose2d(FieldConstants.startingLineX - 0.5, 1.57, Rotation2d.fromDegrees(180)); + // Subwoofer starting locations + public static final Pose2d startingSourceSubwoofer = + FieldConstants.Subwoofer.sourceFaceCorner.transformBy( + GeomUtil.toTransform2d(-Units.inchesToMeters(17.0), Units.inchesToMeters(17.0))); + public static final Pose2d startingAmpSubwoofer = + FieldConstants.Subwoofer.ampFaceCorner.transformBy( + GeomUtil.toTransform2d(-Units.inchesToMeters(17.0), -Units.inchesToMeters(17.0))); // Shooting poses public static final Pose2d stageLeftShootingPose = @@ -718,6 +726,38 @@ public class DriveTrajectories { .build())); } + // Davis Inspirational Auto (named "inspirational_XXX") + static { + paths.put( + "inspirational_leaveFromSource", + List.of( + PathSegment.newBuilder() + .addPoseWaypoint(startingSourceSubwoofer) + .addTranslationWaypoint( + FieldConstants.Stage.podiumLeg + .getTranslation() + .plus(new Translation2d(0.0, -2.0))) + .build())); + paths.put( + "inspirational_leaveFromCenter", + List.of( + PathSegment.newBuilder() + .addPoseWaypoint(startingCenter) + .addTranslationWaypoint( + FieldConstants.StagingLocations.spikeTranslations[1].plus( + new Translation2d(1.0, 0.0))) + .build())); + paths.put( + "inspirational_leaveFromAmp", + List.of( + PathSegment.newBuilder() + .addPoseWaypoint(startingAmpSubwoofer) + .addTranslationWaypoint( + FieldConstants.StagingLocations.spikeTranslations[2].plus( + new Translation2d(1.0, 0.0))) + .build())); + } + /** Calculates aimed pose from translation. */ private static Pose2d getShootingPose(Translation2d translation) { return new Pose2d(