Skip to content

Commit

Permalink
Trajectories don't generate
Browse files Browse the repository at this point in the history
  • Loading branch information
@suryatho
suryatho committed Apr 12, 2024
1 parent 84efde6 commit a66e59d
Show file tree
Hide file tree
Showing 3 changed files with 307 additions and 16 deletions.
1 change: 1 addition & 0 deletions src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -361,6 +361,7 @@ private void configureAutos() {
"How many spike notes?",
List.of(AutoQuestionResponse.TWO, AutoQuestionResponse.THREE))),
autoBuilder.davisSpikyAuto());
autoSelector.addRoutine("Davis CA Auto", autoBuilder.davisCAAuto());
autoSelector.addRoutine("Davis Speedy Auto", List.of(), autoBuilder.davisSpeedyAuto());
autoSelector.addRoutine("Davis Ethical Auto", autoBuilder.davisEthicalAuto());
autoSelector.addRoutine(
Expand Down
170 changes: 170 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 @@ -52,6 +52,10 @@ public class AutoBuilder {
loadTrajectorySet("spiky_secondIntakeReturn");
private static final Map<Translation2d, HolonomicTrajectory> spiky_farIntakeReturn =
loadTrajectorySet("spiky_farIntakeReturn");
private static final Map<Translation2d, HolonomicTrajectory> CA_secondIntakeReturnToSpikes =
loadTrajectorySet("spiky_secondIntakeReturnToSpikes");
private static final Map<Translation2d, HolonomicTrajectory> CA_farIntakeReturnToSpikes =
loadTrajectorySet("spiky_farIntakeReturnToSpikes");

private static Map<Translation2d, HolonomicTrajectory> loadTrajectorySet(String name) {
Map<Translation2d, HolonomicTrajectory> result = new HashMap<>();
Expand Down Expand Up @@ -389,6 +393,172 @@ public Command davisSpikyAuto() {
Commands.runOnce(() -> System.out.println("Centerlines finished at: " + autoTimer.get())));
}

public Command davisCAAuto() {
final HolonomicTrajectory preloadWithSpike2 = new HolonomicTrajectory("CA_startToFirstIntake");
final HolonomicTrajectory firstIntake = new HolonomicTrajectory("spiky_firstIntake");
final HolonomicTrajectory secondIntake = new HolonomicTrajectory("spiky_secondIntake");
final HolonomicTrajectory farIntake = new HolonomicTrajectory("spiky_farIntake");
final double spike1IntakeTime = 2.5;

Timer autoTimer = new Timer();
Timer returnTimer = new Timer();
Timer remainingSpikesTimer = new Timer();
Container<HolonomicTrajectory> firstReturnTrajectory = new Container<>();
Container<HolonomicTrajectory> secondIntakeTrajectory = new Container<>();
Container<Map<Translation2d, HolonomicTrajectory>> secondReturnTrajectorySet =
new Container<>();
Container<HolonomicTrajectory> secondReturnAndSpikesTrajectory = new Container<>();
Container<Boolean> farTrajectory = new Container<>();
return Commands.runOnce(
() -> {
// Reset timers
autoTimer.restart();
remainingSpikesTimer.stop();
remainingSpikesTimer.reset();
returnTimer.stop();
returnTimer.reset();
// Reset selected
firstReturnTrajectory.value = null;
secondIntakeTrajectory.value = null;
secondReturnTrajectorySet.value = null;
secondReturnAndSpikesTrajectory.value = null;
farTrajectory.value = false;
})
.andThen(
// Trajectory sequencing
Commands.sequence(
resetPose(DriveTrajectories.startingAmp),
followTrajectory(drive, preloadWithSpike2), // Do preload + spike 2
followTrajectory(drive, firstIntake)
.until(rollers::isTouchingNote), // Intake from centerline
// Return to stageLeftShot
Commands.runOnce(
() -> {
firstReturnTrajectory.value =
spiky_firstIntakeReturn.get(
RobotState.getInstance()
.getTrajectorySetpoint()
.getTranslation()
.nearest(new ArrayList<>(spiky_firstIntakeReturn.keySet())));
returnTimer.restart();
}),
followTrajectory(drive, () -> firstReturnTrajectory.value),
// Second intake and return
Commands.runOnce(
() -> {
final boolean isFarIntake =
RobotState.getInstance().getTrajectorySetpoint().getY()
<= FieldConstants.Stage.ampLeg.getY();
farTrajectory.value = true;
secondIntakeTrajectory.value = isFarIntake ? farIntake : secondIntake;
secondReturnTrajectorySet.value =
isFarIntake
? CA_farIntakeReturnToSpikes
: CA_secondIntakeReturnToSpikes;
}),
followTrajectory(drive, () -> secondIntakeTrajectory.value)
.until(rollers::isTouchingNote),
followTrajectory(
drive,
() -> {
secondReturnAndSpikesTrajectory.value =
secondReturnTrajectorySet.value.get(
RobotState.getInstance()
.getEstimatedPose()
.getTranslation()
.nearest(
secondReturnTrajectorySet.value.keySet().stream()
.toList()));
return secondReturnAndSpikesTrajectory.value;
}))
.alongWith(
// Superstructure and rollers seqeuence
Commands.sequence(
// Score preload and spike 2
Commands.waitUntil(() -> autoTimer.hasElapsed(1.0))
.andThen(rollers.setGoalCommand(Rollers.Goal.QUICK_INTAKE_TO_FEED))
.deadlineWith(
Commands.parallel(
aim(drive),
superstructure.setGoalCommand(Superstructure.Goal.AIM)))
.until(() -> autoTimer.hasElapsed(2.6)),
// Intake and shoot first centerline
waitUntilXCrossed(FieldConstants.wingX, true)
.andThen(rollers.setGoalCommand(Rollers.Goal.FLOOR_INTAKE))
.until(
() ->
firstReturnTrajectory.value != null
&& returnTimer.hasElapsed(
firstReturnTrajectory.value.getDuration()
- shootTimeoutSecs.get() / 2.0))
.andThen(feed(rollers))
.deadlineWith(
Commands.waitUntil(
() ->
firstReturnTrajectory.value != null
&& returnTimer.hasElapsed(
firstReturnTrajectory.value.getDuration()
- 1.5))
.andThen(
Commands.parallel(
aim(drive),
superstructure.aimWithCompensation(0.0)))),

// Intake and shoot second centerline then score spikes
// Rollers sequence
Commands.sequence(
// Shoot second centerline
rollers
.setGoalCommand(Rollers.Goal.FLOOR_INTAKE)
.raceWith(
waitUntilXCrossed(
DriveTrajectories.CA_lastCenterlineShot.getX(), false)),
feed(rollers),
// Intake and shoot spike 1
rollers
.setGoalCommand(Rollers.Goal.FLOOR_INTAKE)
.until(
() ->
remainingSpikesTimer.hasElapsed(
spike1IntakeTime + spikeIntakeDelay)),
// Run intake and feeder for spike 0
rollers
.setGoalCommand(Rollers.Goal.QUICK_INTAKE_TO_FEED)
.until(
() ->
autoTimer.hasElapsed(
preloadWithSpike2.getDuration()
+ firstIntake.getDuration()
+ firstReturnTrajectory.value.getDuration()
+ secondIntakeTrajectory.value.getDuration()
+ secondReturnAndSpikesTrajectory.value
.getDuration()
+ spikeFeedThroughDelay * 0.5))))
.deadlineWith(
// Reset timer
waitUntilXCrossed(DriveTrajectories.CA_lastCenterlineShot.getX(), false)
.andThen(remainingSpikesTimer::restart),
// Aim arm
waitUntilXCrossed(stageAimX, false)
.andThen(superstructure.setGoalCommand(Superstructure.Goal.AIM)),
Commands.sequence(
// Aim last centerline shot
waitUntilXCrossed(
DriveTrajectories.CA_lastCenterlineShot.getX(), false)
.andThen(aim(drive))
.until(
() ->
remainingSpikesTimer.hasElapsed(
shootTimeoutSecs.get() * 1.5)),

// Aim spike 1 and 0
Commands.waitUntil(
() -> remainingSpikesTimer.hasElapsed(spike1IntakeTime))
.andThen(aim(drive)))))
// Run flywheels
.deadlineWith(flywheels.shootCommand()));
}

public Command davisSpeedyAuto() {
var grabCenterline4 = new HolonomicTrajectory("speedy_ampToCenterline4");
var grabCenterline3 = new HolonomicTrajectory("speedy_centerline4ToCenterline3");
Expand Down
152 changes: 136 additions & 16 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,12 +14,9 @@
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 java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.Set;
import java.util.*;
import java.util.function.Function;
import java.util.stream.Stream;
import lombok.experimental.ExtensionMethod;
import org.littletonrobotics.frc2024.FieldConstants;
import org.littletonrobotics.frc2024.util.GeomUtil;
Expand Down Expand Up @@ -102,6 +99,18 @@ public class DriveTrajectories {
.build()));
}

// First intake
private static final Waypoint startFirstIntakeWaypoint =
Waypoint.newBuilder()
.fromPose(
new Pose2d(
FieldConstants.wingX + 1.0,
FieldConstants.StagingLocations.centerlineTranslations[4].getY(),
Rotation2d.fromDegrees(180.0)))
.setVehicleVelocity(
VehicleVelocityConstraint.newBuilder().setVx(3.0).setVy(0.0).setOmega(0.0).build())
.build();

// Davis Spiky Auto (named "spiky_XXX")
static {
final double shootingVelocity = 0.7;
Expand Down Expand Up @@ -282,17 +291,6 @@ public class DriveTrajectories {
.setStraightLine(true)
.build()));

// First intake
var startFirstIntakeWaypoint =
Waypoint.newBuilder()
.fromPose(
new Pose2d(
FieldConstants.wingX + 1.0,
FieldConstants.StagingLocations.centerlineTranslations[4].getY(),
Rotation2d.fromDegrees(180.0)))
.setVehicleVelocity(
VehicleVelocityConstraint.newBuilder().setVx(3.0).setVy(0.0).setOmega(0.0).build())
.build();
for (int spikeIndex = 0; spikeIndex < 3; spikeIndex++) {
Pose2d spikePose = spikeShootingPoses[spikeIndex];

Expand Down Expand Up @@ -443,6 +441,128 @@ public class DriveTrajectories {
}
}

public static final Pose2d CA_lastCenterlineShot =
getShootingPose(
StagingLocations.spikeTranslations[2]
.interpolate(StagingLocations.spikeTranslations[1], 0.3)
.plus(new Translation2d(0.25, 0.0)));

// Davis CA Auto (named "CA_XXX")
static {
final double shootingVelocity = 0.9;

// First trajectory
paths.put(
"CA_startToFirstIntake",
List.of(
PathSegment.newBuilder()
.addPoseWaypoint(startingAmp)
.addPoseWaypoint(
getShootingPose(StagingLocations.spikeTranslations[2])
.transformBy(GeomUtil.toTransform2d(0.7, 0.0)))
.build(),
PathSegment.newBuilder()
.addPoseWaypoint(
getShootingPose(StagingLocations.spikeTranslations[2])
.transformBy(GeomUtil.toTransform2d(-0.35, 0.0)))
.setMaxVelocity(shootingVelocity)
.setStraightLine(true)
.setMaxOmega(0)
.build(),
PathSegment.newBuilder().addWaypoints(startFirstIntakeWaypoint).build()));

// Segments for scoring the last centerline and remaining spikes
final List<PathSegment> lastCenterlineShotToSpike0Segment =
List.of(
// Shoot last centerline while moving in front of spike 1
PathSegment.newBuilder()
.addPoseWaypoint(CA_lastCenterlineShot)
.addPoseWaypoint(
getShootingPose(
StagingLocations.spikeTranslations[2]
.interpolate(StagingLocations.spikeTranslations[1], 0.4)
.plus(new Translation2d(-0.65, 0.0))))
.setMaxVelocity(shootingVelocity)
.build(),
// Intake spike 1
PathSegment.newBuilder()
.addPoseWaypoint(
new Pose2d(StagingLocations.spikeTranslations[1], Rotation2d.fromDegrees(160.0))
.transformBy(GeomUtil.toTransform2d(0.8, 0.0)))
.addPoseWaypoint(
new Pose2d(StagingLocations.spikeTranslations[1], Rotation2d.fromDegrees(160.0))
.transformBy(GeomUtil.toTransform2d(0.5, 0.0)))
.build(),
// Shoot spike 1
PathSegment.newBuilder()
.addPoseWaypoint(
getShootingPose(
StagingLocations.spikeTranslations[1]
.interpolate(StagingLocations.spikeTranslations[0], 0.4)
.plus(new Translation2d(-0.65, 0.0))))
.setMaxVelocity(1.2)
.build(),
// Intake and shoot spike 0
PathSegment.newBuilder()
.addPoseWaypoint(
getShootingPose(StagingLocations.spikeTranslations[0])
.transformBy(GeomUtil.toTransform2d(0.45, 0.0)))
.build());

// Return + spikes trajectories
for (String intakeName : List.of("spiky_secondIntake", "spiky_farIntake")) {
suppliedPaths.add(
completedPaths -> {
if (!completedPaths.contains(intakeName)) return null;
final double minDistance = 0.22;

Map<String, List<PathSegment>> returnPaths = new HashMap<>();
var intakeTrajectory = new HolonomicTrajectory(intakeName);
int index = -1;
int validIndex = -1;
Translation2d lastTranslation = new Translation2d();

for (var state : intakeTrajectory.getStates()) {
index++;
Translation2d translation = state.getPose().getTranslation();
if ((translation.getDistance(lastTranslation) > minDistance
&& translation.getX() > FieldConstants.wingX + 1.0)
|| index == intakeTrajectory.getStates().length - 1) {
validIndex++;
lastTranslation = translation;
boolean throughStage = translation.getY() < FieldConstants.Stage.ampLeg.getY();

PathSegment firstSegment = PathSegment.newBuilder().build();
if (throughStage) {
firstSegment =
firstSegment.toBuilder()
.addContinuationWaypoint(state)
.addTranslationWaypoint(
stageCenterAvoidance.plus(new Translation2d(0.0, -0.3)))
.addTranslationWaypoint(
Stage.ampLeg
.getTranslation()
.interpolate(Stage.podiumLeg.getTranslation(), 0.4))
.build();
} else {
firstSegment =
firstSegment.toBuilder()
.addContinuationWaypoint(state)
.addTranslationWaypoint(stageLeftAvoidance)
.build();
}
paths.put(
intakeName + "ReturnToSpikes" + String.format("%03d", validIndex),
Stream.of(List.of(firstSegment), lastCenterlineShotToSpike0Segment)
.flatMap(Collection::stream)
.toList());
}
}
return returnPaths;
});
}
}

// Davis Speedy Auto (named "speedy_XXX")
static {
final Pose2d stageLeftShootingPose =
Expand Down

0 comments on commit a66e59d

Please sign in to comment.