Add N6-S12-C0123 and N5_S1_C234

.gitignore

@@ -178,4 +178,4 @@ networktables.json
 src/main/java/org/littletonrobotics/frc2024/BuildConstants.java
 
 # Generated paths
-src/main/deploy/trajectories/*.pathblob
+#src/main/deploy/trajectories/*.pathblob

src/main/java/org/littletonrobotics/frc2024/Constants.java

@@ -21,7 +21,7 @@
  */
 public final class Constants {
   public static final double loopPeriodSecs = 0.02;
-  private static RobotType robotType = RobotType.DEVBOT;
+  private static RobotType robotType = RobotType.SIMBOT;
   public static final boolean tuningMode = true;
 
   public static RobotType getRobot() {

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("N6_S12-C0123", autoBuilder.N6_S12_C0123());
   }
 
   /**

src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java

@@ -103,17 +103,73 @@ public Command davisEthicalAuto() {
   }
 
   public Command N5_S1_C234() {
+    Timer autoTimer = new Timer();
     HolonomicTrajectory driveToS1 = new HolonomicTrajectory("N5-S1-C234_driveToS1");
+
     HolonomicTrajectory driveToC2 = new HolonomicTrajectory("N5-S1-C234_driveToC2");
     HolonomicTrajectory driveToC3 = new HolonomicTrajectory("N5-S1-C234_driveToC3");
     HolonomicTrajectory driveToC4 = new HolonomicTrajectory("N5-S1-C234_driveToC4");
 
+    return sequence(
+        runOnce(autoTimer::restart),
+        runOnce(() -> flywheels.setIdleMode(Flywheels.IdleMode.AUTO)),
+        // Shoot preloaded note
+        resetPose(driveToS1),
+        shoot(drive, superstructure, flywheels, rollers),
+        runOnce(() -> System.out.printf("First shot at %.2f seconds.", autoTimer.get())),
+        followTrajectory(drive, driveToS1)
+            .deadlineWith(intakeIntoShot(drive, superstructure, flywheels, rollers)),
+        runOnce(() -> System.out.printf("Second shot at %.2f seconds.", autoTimer.get())),
+        followTrajectory(drive, driveToC2)
+            .deadlineWith(
+                sequence(
+                    // Check if full length of robot + some has passed wing for arm safety
+                    waitUntilXCrossed(
+                        FieldConstants.wingX + DriveConstants.driveConfig.bumperWidthX() * 0.7,
+                        true),
+                    intake(superstructure, rollers).withTimeout(0.8),
+                    // Wait until we are close enough to shot to start arm aiming
+                    waitUntilXCrossed(FieldConstants.Stage.podiumLeg.getX() + 0.5, false),
+                    superstructure.aim())),
+        shoot(drive, superstructure, flywheels, rollers),
+        runOnce(() -> System.out.printf("Third shot at %.2f seconds.", autoTimer.get())),
+        followTrajectory(drive, driveToC3)
+            .deadlineWith(
+                sequence(
+                    waitUntilXCrossed(
+                        FieldConstants.wingX + DriveConstants.driveConfig.bumperWidthX() * 0.7,
+                        true),
+                    intake(superstructure, rollers).withTimeout(1.0),
+                    superstructure.aim())),
+        shoot(drive, superstructure, flywheels, rollers),
+        runOnce(() -> System.out.printf("Fourth shot at %.2f seconds.", autoTimer.get())),
+        followTrajectory(drive, driveToC4)
+            .deadlineWith(
+                sequence(
+                    waitUntilXCrossed(
+                        FieldConstants.wingX + DriveConstants.driveConfig.bumperWidthX() * 0.7,
+                        true),
+                    intake(superstructure, rollers).withTimeout(1.0),
+                    superstructure.aim())),
+        shoot(drive, superstructure, flywheels, rollers),
+        runOnce(() -> System.out.printf("Fifth shot at %.2f seconds.", autoTimer.get())),
+        // Revert to teleop idle mode
+        runOnce(() -> flywheels.setIdleMode(Flywheels.IdleMode.TELEOP)));
+  }
+
+  public Command N6_S12_C0123() {
+    HolonomicTrajectory driveToS1 = new HolonomicTrajectory("N6-S12-C0123_driveToS1");
+    HolonomicTrajectory driveToS2 = new HolonomicTrajectory("N6-S12-C0123_driveToS2");
+    HolonomicTrajectory driveToC0 = new HolonomicTrajectory("N6-S12-C0123_driveToC0");
+    HolonomicTrajectory driveToC1 = new HolonomicTrajectory("N6-S12-C0123_driveToC1");
+    HolonomicTrajectory driveToC2 = new HolonomicTrajectory("N6-S12-C0123_driveToC2");
     return sequence(
         resetPose(driveToS1),
         followTrajectory(drive, driveToS1),
-        followTrajectory(drive, driveToC2),
-        followTrajectory(drive, driveToC3),
-        followTrajectory(drive, driveToC4));
+        followTrajectory(drive, driveToS2),
+        followTrajectory(drive, driveToC0),
+        followTrajectory(drive, driveToC1),
+        followTrajectory(drive, driveToC2));
   }
 
   public Command N5_S0_C012() {
@@ -182,6 +238,6 @@ public Command N5_S0_C012() {
   //        );
 }
 
-  //  public Command driveStraight() {
-  //    return reset("driveToPodium")
-  //        .andThen(path("driveToPodium"));
+//  public Command driveStraight() {
+//    return reset("driveToPodium")
+//        .andThen(path("driveToPodium"));

src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java

@@ -26,7 +26,7 @@
 
 public class AutoCommands {
   public static final LoggedTunableNumber shootTimeoutSecs =
-      new LoggedTunableNumber("Auto/ShotTimeoutSecs", 0.5);
+      new LoggedTunableNumber("Auto/ShotTimeoutSecs", 0.1);
 
   /**
    * Resets pose to beginning of trajectory accounting for alliance color {@link

src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java

@@ -39,7 +39,7 @@ public class DriveTrajectories {
               new Rotation2d()));
   private static final Pose2d startingCenterFace =
       FieldConstants.Subwoofer.centerFace.transformBy(
-          new Transform2d(DriveConstants.driveConfig.bumperWidthX() / 2, 0, new Rotation2d(0)));
+          new Transform2d(-DriveConstants.driveConfig.bumperWidthX() / 2, 0, new Rotation2d(0)));
 
   // Center intake locations
   private static final double intakeOffset = 0.5;
@@ -258,6 +258,22 @@ public class DriveTrajectories {
         List.of(
             PathSegment.newBuilder()
                 .addPoseWaypoint(startingAmpFace)
+                .addPoseWaypoint(
+                    getShootingPose(FieldConstants.StagingLocations.spikeTranslations[2]))
+                .build()));
+
+    paths.put(
+        "N6-S12-C0123_driveToS2",
+        List.of(
+            PathSegment.newBuilder()
+                .addPoseWaypoint(
+                    getShootingPose(FieldConstants.StagingLocations.spikeTranslations[2]))
+                .addTranslationWaypoint(
+                    new Translation2d(
+                        FieldConstants.startingLineX,
+                        (FieldConstants.StagingLocations.spikeTranslations[2].getY()
+                                + FieldConstants.StagingLocations.spikeTranslations[1].getY())
+                            / 2))
                 .addPoseWaypoint(
                     getShootingPose(FieldConstants.StagingLocations.spikeTranslations[1]))
                 .build()));
@@ -274,10 +290,47 @@ public class DriveTrajectories {
                 .addPoseWaypoint(
                     getShootingPose(
                         FieldConstants.Stage.center
-                            .transformBy(new Transform2d(0, 1.3, new Rotation2d()))
+                            .transformBy(new Transform2d(0, 2, new Rotation2d()))
                             .getTranslation()),
                     30)
                 .build()));
+
+    paths.put(
+        "N6-S12-C0123_driveToC1",
+        List.of(
+            PathSegment.newBuilder()
+                .addPoseWaypoint(
+                    getShootingPose(
+                        FieldConstants.Stage.center
+                            .transformBy(new Transform2d(0, 2, new Rotation2d()))
+                            .getTranslation()))
+                .addPoseWaypoint(intakingCenterlinePoses[3])
+                .addPoseWaypoint(
+                    getShootingPose(
+                        FieldConstants.Stage.center
+                            .transformBy(new Transform2d(-1, 2, new Rotation2d()))
+                            .getTranslation()))
+                .build()));
+
+    paths.put(
+        "N6-S12-C0123_driveToC2",
+        List.of(
+            PathSegment.newBuilder()
+                .addPoseWaypoint(
+                    getShootingPose(
+                        FieldConstants.Stage.center
+                            .transformBy(new Transform2d(-1, 2, new Rotation2d()))
+                            .getTranslation()))
+                .addPoseWaypoint(
+                    new Pose2d(
+                        FieldConstants.Stage.center.getTranslation(), new Rotation2d(Math.PI)))
+                .addPoseWaypoint(intakingCenterlinePoses[2])
+                .addPoseWaypoint(
+                    getShootingPose(
+                        FieldConstants.Stage.center
+                            .transformBy(new Transform2d(-.5, .6, new Rotation2d(Math.PI)))
+                            .getTranslation()))
+                .build()));
   }
 
   // calculate Pose2d of robot given a translation