From 1db89aa14d335c11063dc3fc0886d83d7aca62c7 Mon Sep 17 00:00:00 2001 From: suryatho Date: Fri, 12 Apr 2024 20:26:36 -0400 Subject: [PATCH 1/6] Add inspirational auto --- .../frc2024/AutoSelector.java | 5 +++ .../frc2024/RobotContainer.java | 23 ++++++++++ .../frc2024/commands/auto/AutoBuilder.java | 43 +++++++++++++++++++ .../drive/trajectory/DriveTrajectories.java | 35 +++++++++++++++ 4 files changed, 106 insertions(+) diff --git a/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java b/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java index 83b84802..d1e71656 100644 --- a/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java +++ b/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java @@ -146,5 +146,10 @@ public static enum AutoQuestionResponse { AMP_WALL, SCORE_POOPED, FOURTH_CENTER, + IMMEDIATELY, + SIX_SECONDS, + FOURTEEN_SECONDS, + YES, + NO } } diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 5edb6e04..df5f4f7e 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -344,6 +344,11 @@ public RobotContainer() { .withTimeout(0.9) // Rumble three times .beforeStarting(() -> Leds.getInstance().endgameAlert = true) .finallyDo(() -> Leds.getInstance().endgameAlert = false)); + + Logger.recordOutput( + "SubwooferStart", + FieldConstants.Subwoofer.sourceFaceCorner.transformBy( + GeomUtil.toTransform2d(Units.inchesToMeters(-17.0), Units.inchesToMeters(15.0)))); } private void configureAutos() { @@ -385,6 +390,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.FOURTEEN_SECONDS))), + 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 3188d4e0..67586694 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java @@ -691,4 +691,47 @@ private Command unethical_poopThenScoreCenterlines( superstructure.aimWithCompensation( secondShotCompensation))))))); } + + public Command davisInspirationalAuto() { + return 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.FOURTEEN_SECONDS, + Commands.waitSeconds(13.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 bff054e8..3c236814 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.HashMap; import java.util.List; import java.util.Map; @@ -51,6 +52,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 = @@ -645,6 +653,33 @@ public class DriveTrajectories { .build())); } + // Davis Inspirational Auto (named "inspirational_XXX") + static { + paths.put( + "inspirational_leaveFromSource", + List.of( + PathSegment.newBuilder() + .addPoseWaypoint(startingSourceSubwoofer) + .addTranslationWaypoint( + startingSourceSubwoofer.getTranslation().plus(new Translation2d(2.0, -1.5))) + .build())); + paths.put( + "inspirational_leaveFromCenter", + List.of( + PathSegment.newBuilder() + .addPoseWaypoint(startingCenter) + .addTranslationWaypoint( + startingCenter.getTranslation().plus(new Translation2d(1.0, 0.0))) + .build())); + paths.put( + "inspirational_leaveFromAmp", + List.of( + PathSegment.newBuilder() + .addPoseWaypoint(startingAmpSubwoofer) + .addPoseWaypoint(startingAmpWall.transformBy(GeomUtil.toTransform2d(-1.0, 0.0))) + .build())); + } + /** Calculates aimed pose from translation. */ private static Pose2d getShootingPose(Translation2d translation) { return new Pose2d( From 55dca550841fc6faec2bbd3065f678c3d6b588ec Mon Sep 17 00:00:00 2001 From: suryatho Date: Sat, 13 Apr 2024 01:17:46 -0400 Subject: [PATCH 2/6] Critical no comma error --- src/main/java/org/littletonrobotics/frc2024/AutoSelector.java | 2 +- .../frc2024/subsystems/drive/trajectory/DriveTrajectories.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java b/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java index 973cc3b6..d5c4cb39 100644 --- a/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java +++ b/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java @@ -146,7 +146,7 @@ public static enum AutoQuestionResponse { AMP_WALL, SCORE_POOPED, FOURTH_CENTER, - THINKING_ON_YOUR_FEET + THINKING_ON_YOUR_FEET, IMMEDIATELY, SIX_SECONDS, FOURTEEN_SECONDS, 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 cbf59756..5b8027f3 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,8 +14,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; -import java.util.ArrayList; import edu.wpi.first.math.util.Units; +import java.util.ArrayList; import java.util.HashMap; import java.util.List; import java.util.Map; From 8145b0e50d578aea6f04e1619cdeae7895782ccc Mon Sep 17 00:00:00 2001 From: nharnwal Date: Sat, 13 Apr 2024 13:03:56 -0400 Subject: [PATCH 3/6] Better source trajectory --- .../subsystems/drive/trajectory/DriveTrajectories.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 5b8027f3..9927616e 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 @@ -734,7 +734,9 @@ public class DriveTrajectories { PathSegment.newBuilder() .addPoseWaypoint(startingSourceSubwoofer) .addTranslationWaypoint( - startingSourceSubwoofer.getTranslation().plus(new Translation2d(2.0, -1.5))) + startingSourceSubwoofer.getTranslation().plus(new Translation2d(0.75, -3))) + .addTranslationWaypoint( + startingSourceSubwoofer.getTranslation().plus(new Translation2d(2.0, -3.75))) .build())); paths.put( "inspirational_leaveFromCenter", From e48504e61c311b4833d040e22191b1b9f7a1a667 Mon Sep 17 00:00:00 2001 From: nharnwal Date: Sat, 13 Apr 2024 13:05:23 -0400 Subject: [PATCH 4/6] Replace 14 second option with last second initiation --- .../frc2024/AutoSelector.java | 2 +- .../frc2024/RobotContainer.java | 2 +- .../frc2024/commands/auto/AutoBuilder.java | 98 ++++++++++++------- 3 files changed, 65 insertions(+), 37 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java b/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java index d5c4cb39..23adc493 100644 --- a/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java +++ b/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java @@ -149,7 +149,7 @@ public static enum AutoQuestionResponse { THINKING_ON_YOUR_FEET, IMMEDIATELY, SIX_SECONDS, - FOURTEEN_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 633b36af..b4654131 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -396,7 +396,7 @@ private void configureAutos() { List.of( AutoQuestionResponse.IMMEDIATELY, AutoQuestionResponse.SIX_SECONDS, - AutoQuestionResponse.FOURTEEN_SECONDS))), + AutoQuestionResponse.LAST_SECOND))), autoBuilder.davisInspirationalAuto()); // Set up feedforward characterization 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 8080a4bc..4ed7a121 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java @@ -697,45 +697,73 @@ private Command unethical_poopThenScoreCenterlines( } public Command davisInspirationalAuto() { - return 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 - ) + 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( - // 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.FOURTEEN_SECONDS, - Commands.waitSeconds(13.0)), - () -> responses.get().get(2)) - .andThen( - Commands.select( + 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, - followTrajectory( - drive, new HolonomicTrajectory("inspirational_leaveFromSource")), + resetPose(DriveTrajectories.startingSourceSubwoofer), AutoQuestionResponse.CENTER, - followTrajectory( - drive, new HolonomicTrajectory("inspirational_leaveFromCenter")), + resetPose(DriveTrajectories.startingCenter), AutoQuestionResponse.AMP, - followTrajectory( - drive, new HolonomicTrajectory("inspirational_leaveFromAmp"))), - () -> responses.get().get(0))) - .onlyIf(() -> responses.get().get(1) == AutoQuestionResponse.YES)); + 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( + 14.6 - leaveFromSource.getDuration()), + AutoQuestionResponse.CENTER, + Commands.waitSeconds( + 14.6 - leaveFromCenter.getDuration()), + AutoQuestionResponse.AMP, + Commands.waitSeconds( + 14.6 - 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)))); } } From 11504c202bf97b03a1c101c8ab561cec3a7ed2e4 Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Sat, 13 Apr 2024 16:30:19 -0400 Subject: [PATCH 5/6] Remove debug log --- .../java/org/littletonrobotics/frc2024/RobotContainer.java | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index b4654131..295b97b4 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -342,11 +342,6 @@ public RobotContainer() { .withTimeout(0.9) // Rumble three times .beforeStarting(() -> Leds.getInstance().endgameAlert = true) .finallyDo(() -> Leds.getInstance().endgameAlert = false)); - - Logger.recordOutput( - "SubwooferStart", - FieldConstants.Subwoofer.sourceFaceCorner.transformBy( - GeomUtil.toTransform2d(Units.inchesToMeters(-17.0), Units.inchesToMeters(15.0)))); } private void configureAutos() { From 7e1f3ec9e3d22cedcc8831cc932a1bd36c89a481 Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Sat, 13 Apr 2024 17:03:18 -0400 Subject: [PATCH 6/6] Adjustments --- .../frc2024/commands/auto/AutoBuilder.java | 6 +++--- .../drive/trajectory/DriveTrajectories.java | 13 ++++++++----- 2 files changed, 11 insertions(+), 8 deletions(-) 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 4ed7a121..2aa80592 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java @@ -737,13 +737,13 @@ public Command davisInspirationalAuto() { Map.of( AutoQuestionResponse.SOURCE, Commands.waitSeconds( - 14.6 - leaveFromSource.getDuration()), + 13.5 - leaveFromSource.getDuration()), AutoQuestionResponse.CENTER, Commands.waitSeconds( - 14.6 - leaveFromCenter.getDuration()), + 13.5 - leaveFromCenter.getDuration()), AutoQuestionResponse.AMP, Commands.waitSeconds( - 14.6 - leaveFromAmp.getDuration())), + 13.5 - leaveFromAmp.getDuration())), () -> responses.get().get(0))), () -> responses.get().get(2)) .andThen( 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 9927616e..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 @@ -734,9 +734,9 @@ public class DriveTrajectories { PathSegment.newBuilder() .addPoseWaypoint(startingSourceSubwoofer) .addTranslationWaypoint( - startingSourceSubwoofer.getTranslation().plus(new Translation2d(0.75, -3))) - .addTranslationWaypoint( - startingSourceSubwoofer.getTranslation().plus(new Translation2d(2.0, -3.75))) + FieldConstants.Stage.podiumLeg + .getTranslation() + .plus(new Translation2d(0.0, -2.0))) .build())); paths.put( "inspirational_leaveFromCenter", @@ -744,14 +744,17 @@ public class DriveTrajectories { PathSegment.newBuilder() .addPoseWaypoint(startingCenter) .addTranslationWaypoint( - startingCenter.getTranslation().plus(new Translation2d(1.0, 0.0))) + FieldConstants.StagingLocations.spikeTranslations[1].plus( + new Translation2d(1.0, 0.0))) .build())); paths.put( "inspirational_leaveFromAmp", List.of( PathSegment.newBuilder() .addPoseWaypoint(startingAmpSubwoofer) - .addPoseWaypoint(startingAmpWall.transformBy(GeomUtil.toTransform2d(-1.0, 0.0))) + .addTranslationWaypoint( + FieldConstants.StagingLocations.spikeTranslations[2].plus( + new Translation2d(1.0, 0.0))) .build())); }