From a66e59d401ef449da1ee8dfaed8b90bad273ca5a Mon Sep 17 00:00:00 2001 From: suryatho Date: Thu, 11 Apr 2024 21:23:44 -0400 Subject: [PATCH] Trajectories don't generate --- .../frc2024/RobotContainer.java | 1 + .../frc2024/commands/auto/AutoBuilder.java | 170 ++++++++++++++++++ .../drive/trajectory/DriveTrajectories.java | 152 ++++++++++++++-- 3 files changed, 307 insertions(+), 16 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 7e56edbd..468059a6 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -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( 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 0014469d..080a6ee4 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java @@ -52,6 +52,10 @@ public class AutoBuilder { loadTrajectorySet("spiky_secondIntakeReturn"); private static final Map spiky_farIntakeReturn = loadTrajectorySet("spiky_farIntakeReturn"); + private static final Map CA_secondIntakeReturnToSpikes = + loadTrajectorySet("spiky_secondIntakeReturnToSpikes"); + private static final Map CA_farIntakeReturnToSpikes = + loadTrajectorySet("spiky_farIntakeReturnToSpikes"); private static Map loadTrajectorySet(String name) { Map result = new HashMap<>(); @@ -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 firstReturnTrajectory = new Container<>(); + Container secondIntakeTrajectory = new Container<>(); + Container> secondReturnTrajectorySet = + new Container<>(); + Container secondReturnAndSpikesTrajectory = new Container<>(); + Container 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"); 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 4755f6f4..0f36b011 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,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; @@ -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; @@ -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]; @@ -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 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> 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 =