Skip to content

Commit

Permalink
Davis inspirational auto (#258)
Browse files Browse the repository at this point in the history 
* 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 <[email protected]>
Co-authored-by: Jonah <[email protected]>
  • Loading branch information
@suryatho @harnwalN @jwbonner
3 people authored Apr 13, 2024
1 parent b510c63 commit a55356a
Show file tree
Hide file tree
Showing 4 changed files with 135 additions and 1 deletion.
7 changes: 6 additions & 1 deletion src/main/java/org/littletonrobotics/frc2024/AutoSelector.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
}
}
18 changes: 18 additions & 0 deletions src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
71 changes: 71 additions & 0 deletions src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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))));
}
}
40 changes: 40 additions & 0 deletions ...ain/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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 =
Expand Down Expand Up @@ -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(
Expand Down

0 comments on commit a55356a

Please sign in to comment.