fix Davis podium pose and add N5-C432-S2

Mechanical-Advantage · Feb 17, 2024 · 9a139dd · 9a139dd
1 parent 1185edb
commit 9a139dd
Show file tree

Hide file tree

Showing 8 changed files with 93 additions and 20 deletions.
diff --git 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
diff --git a/src/main/deploy/trajectories/N5-S0-C0123_driveToC1.pathblob b/src/main/deploy/trajectories/N5-S0-C0123_driveToC1.pathblob
diff --git a/src/main/deploy/trajectories/N5-S0-C0123_driveToC2.pathblob b/src/main/deploy/trajectories/N5-S0-C0123_driveToC2.pathblob
diff --git a/src/main/deploy/trajectories/davisEthicalAuto_driveToPodium.pathblob b/src/main/deploy/trajectories/davisEthicalAuto_driveToPodium.pathblob
diff --git 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
@@ -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/...ain/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java b/...ain/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()));