Skip to content
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

Jump to bottom

Davis inspirational auto #258

Merged
merged 8 commits into from
Apr 13, 2024
Merged

Davis inspirational auto #258

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
1db89aa
Add inspirational auto
suryatho Apr 13, 2024
eb30f60
Merge branch 'main' into davis-inspirational-auto
suryatho Apr 13, 2024
55dca55
Critical no comma error
suryatho Apr 13, 2024
8145b0e
Better source trajectory
harnwalN Apr 13, 2024
e48504e
Replace 14 second option with last second initiation
harnwalN Apr 13, 2024
11504c2
Remove debug log
jwbonner Apr 13, 2024
eba772b
Merge branch 'main' into davis-inspirational-auto
jwbonner Apr 13, 2024
7e1f3ec
Adjustments
jwbonner Apr 13, 2024
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
7 changes: 6 additions & 1 deletion src/main/java/org/littletonrobotics/frc2024/AutoSelector.java
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading