diff --git a/build.gradle b/build.gradle index 1e81e373..5bd35cfe 100644 --- a/build.gradle +++ b/build.gradle @@ -164,7 +164,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' - testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher:1.10.2' implementation 'gov.nist.math:jama:1.0.3' diff --git a/src/main/deploy/trajectories/N5-S0-C0123_driveToC0.pathblob b/src/main/deploy/trajectories/N5-S0-C0123_driveToC0.pathblob index bfe65381..88b146ac 100644 Binary files a/src/main/deploy/trajectories/N5-S0-C0123_driveToC0.pathblob and b/src/main/deploy/trajectories/N5-S0-C0123_driveToC0.pathblob differ diff --git a/src/main/deploy/trajectories/N5-S0-C0123_driveToC1.pathblob b/src/main/deploy/trajectories/N5-S0-C0123_driveToC1.pathblob index 4c980997..8ad5b347 100644 Binary files a/src/main/deploy/trajectories/N5-S0-C0123_driveToC1.pathblob and b/src/main/deploy/trajectories/N5-S0-C0123_driveToC1.pathblob differ diff --git a/src/main/deploy/trajectories/N5-S0-C0123_driveToC2.pathblob b/src/main/deploy/trajectories/N5-S0-C0123_driveToC2.pathblob index 696e358b..8a31ca9b 100644 Binary files a/src/main/deploy/trajectories/N5-S0-C0123_driveToC2.pathblob and b/src/main/deploy/trajectories/N5-S0-C0123_driveToC2.pathblob differ diff --git a/src/main/deploy/trajectories/davisEthicalAuto_driveToPodium.pathblob b/src/main/deploy/trajectories/davisEthicalAuto_driveToPodium.pathblob index edcf9d9e..700e8b89 100644 Binary files a/src/main/deploy/trajectories/davisEthicalAuto_driveToPodium.pathblob and b/src/main/deploy/trajectories/davisEthicalAuto_driveToPodium.pathblob differ diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 01155c06..812a5fce 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -203,6 +203,7 @@ private void configureAutos() { autoChooser.addOption("Davis Ethical Auto", autoBuilder.davisEthicalAuto()); autoChooser.addOption("N5_S1_C234", autoBuilder.N5_S1_C234()); autoChooser.addOption("N5_S0_C012", autoBuilder.N5_S0_C012()); + autoChooser.addOption("N5_C432_S2", autoBuilder.N5_C432_S2()); autoChooser.addOption("N6_S12-C0123", autoBuilder.N6_S12_C0123()); } 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 a893cf49..54b24a37 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java @@ -186,6 +186,20 @@ public Command N5_S0_C012() { followTrajectory(drive, driveToC2)); } + public Command N5_C432_S2() { + HolonomicTrajectory driveToC4 = new HolonomicTrajectory("N5-C432-S2_driveToC4"); + HolonomicTrajectory driveToC3 = new HolonomicTrajectory("N5-C432-S2_driveToC3"); + HolonomicTrajectory driveToC2 = new HolonomicTrajectory("N5-C432-S2_driveToC2"); + HolonomicTrajectory driveToPodium = new HolonomicTrajectory("N5-C432-S2_driveToPodium"); + + return sequence( + resetPose(driveToC4), + followTrajectory(drive, driveToC4), + followTrajectory(drive, driveToC3), + followTrajectory(drive, driveToC2), + followTrajectory(drive, driveToPodium)); + } + // public Command N5_S0_C012() { // return sequence( // reset("N5-S0-C0123_driveToS0"), 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 e078e22f..0ccc1bde 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 @@ -63,8 +63,12 @@ public class DriveTrajectories { PathSegment.newBuilder() .addPoseWaypoint(startingSourceFace) .addPoseWaypoint( - getShootingPose(FieldConstants.StagingLocations.spikeTranslations[0]) - .transformBy(new Transform2d(intakeOffset, 0, new Rotation2d()))) + getShootingPose( + new Pose2d( + FieldConstants.StagingLocations.spikeTranslations[0], + new Rotation2d(0)) + .transformBy(new Transform2d(-intakeOffset, 0, new Rotation2d(0))) + .getTranslation())) .build())); paths.put( @@ -197,13 +201,11 @@ public class DriveTrajectories { PathSegment.newBuilder() .addPoseWaypoint( getShootingPose(FieldConstants.StagingLocations.spikeTranslations[2])) - .addPoseWaypoint( - new Pose2d( - intakingCenterlinePoses[4].getTranslation(), new Rotation2d(Math.PI))) + .addPoseWaypoint(intakingCenterlinePoses[4]) .addPoseWaypoint( getShootingPose( FieldConstants.Stage.center - .transformBy(new Transform2d(0, 1.3, new Rotation2d())) + .transformBy(new Transform2d(0, 2.2, new Rotation2d(Math.PI))) .getTranslation()), 30) .build())); @@ -215,15 +217,14 @@ public class DriveTrajectories { .addPoseWaypoint( getShootingPose( FieldConstants.Stage.center - .transformBy(new Transform2d(0, 1.3, new Rotation2d())) - .getTranslation())) - .addPoseWaypoint( - new Pose2d( - intakingCenterlinePoses[3].getTranslation(), Rotation2d.fromDegrees(-180))) + .transformBy(new Transform2d(0, 2.2, new Rotation2d(Math.PI))) + .getTranslation()), + 30) + .addPoseWaypoint(intakingCenterlinePoses[3]) .addPoseWaypoint( getShootingPose( FieldConstants.Stage.center - .transformBy(new Transform2d(2, 0, new Rotation2d(Math.PI / 2))) + .transformBy(new Transform2d(0, 2.2, new Rotation2d(Math.PI))) .getTranslation())) .build())); @@ -234,21 +235,78 @@ public class DriveTrajectories { .addPoseWaypoint( getShootingPose( FieldConstants.Stage.center - .transformBy(new Transform2d(2, 0, new Rotation2d(Math.PI / 2))) + .transformBy(new Transform2d(0, 2.2, new Rotation2d(Math.PI))) .getTranslation())) .addTranslationWaypoint( - FieldConstants.Stage.center - .transformBy(new Transform2d(1.25, 0, Rotation2d.fromDegrees(-60))) + FieldConstants.Stage.ampLeg + .transformBy(new Transform2d(0, -1, Rotation2d.fromDegrees(60))) + .getTranslation()) + .addPoseWaypoint(intakingCenterlinePoses[2]) + .addPoseWaypoint( + getShootingPose( + FieldConstants.Stage.ampLeg + .transformBy(new Transform2d(-0.5, -1.25, Rotation2d.fromDegrees(60))) + .getTranslation())) + .build())); + + // N5-C012-S0 + paths.put( + "N5-C432-S2_driveToC4", + List.of( + PathSegment.newBuilder() + .addPoseWaypoint(startingSourceFace) + .addPoseWaypoint(intakingCenterlinePoses[0]) + .addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.6))) + .build())); + + paths.put( + "N5-C432-S2_driveToC3", + List.of( + PathSegment.newBuilder() + .addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.6))) + .addTranslationWaypoint( + FieldConstants.Stage.sourceLeg + .transformBy(new Transform2d(0, -1, new Rotation2d(0))) + .getTranslation()) + .addPoseWaypoint(intakingCenterlinePoses[1]) + .addTranslationWaypoint( + FieldConstants.Stage.sourceLeg + .transformBy(new Transform2d(0, -1, new Rotation2d())) + .getTranslation()) + .addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.6))) + .build())); + paths.put( + "N5-C432-S2_driveToC2", + List.of( + PathSegment.newBuilder() + .addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.6))) + .addTranslationWaypoint( + FieldConstants.Stage.sourceLeg + .transformBy(new Transform2d(0, 1, Rotation2d.fromDegrees(0))) .getTranslation()) .addPoseWaypoint(intakingCenterlinePoses[2]) .addTranslationWaypoint( - FieldConstants.Stage.center - .transformBy(new Transform2d(1.25, 0, Rotation2d.fromDegrees(-60))) + FieldConstants.Stage.sourceLeg + .transformBy(new Transform2d(0, 1, Rotation2d.fromDegrees(0))) + .getTranslation()) + .addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.6))) + .build())); + + paths.put( + "N5-C432-S2_driveToPodium", + List.of( + PathSegment.newBuilder() + .addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.6))) + .addTranslationWaypoint( + FieldConstants.Stage.podiumLeg + .transformBy(new Transform2d(-1, 0, new Rotation2d())) .getTranslation()) .addPoseWaypoint( getShootingPose( - FieldConstants.Stage.center - .transformBy(new Transform2d(2, 0, new Rotation2d(Math.PI / 2))) + new Pose2d( + FieldConstants.StagingLocations.spikeTranslations[0], + new Rotation2d(0)) + .transformBy(new Transform2d(-intakeOffset, 0, new Rotation2d())) .getTranslation())) .build()));