From 0300a0ff86a30233a5173ecc49b4a4b66f2ce61c Mon Sep 17 00:00:00 2001
From: nharnwal <neelaksh.harnwal@gmail.com>
Date: Sat, 9 Mar 2024 15:02:26 -0500
Subject: [PATCH] Working on spike autos

---
 .../frc2024/commands/auto/AutoBuilder.java    |  4 +--
 .../drive/trajectory/DriveTrajectories.java   | 35 ++++++++++++-------
 2 files changed, 25 insertions(+), 14 deletions(-)

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 4eced6fb..2cf33a10 100644
--- a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java
+++ b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java
@@ -156,7 +156,7 @@ public Command sourceFRC6328Auto() {
                                 rollers.setGoalCommand(Rollers.Goal.FLOOR_INTAKE))
                             .withTimeout(grabCenterline2.getDuration() - 1.0),
 
-                        // Shoot centerline 2
+                        // Shoot centerline 2l
                         Commands.waitUntil(
                                 () ->
                                     autoTimer.hasElapsed(
@@ -498,7 +498,7 @@ public Command davisEthicalAuto() {
         .andThen(
             // Drive sequence
             Commands.sequence(
-                    resetPose(DriveTrajectories.startingLinePodium),
+                    resetPose(DriveTrajectories.startingLineSpike0),
                     Commands.startEnd(
                             () ->
                                 drive.setHeadingGoal(
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 eeaa062c..7e5b58c0 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
@@ -42,22 +42,21 @@ public class DriveTrajectories {
   public static final Pose2d startingCenterFace =
       FieldConstants.Subwoofer.centerFace.transformBy(
           new Transform2d(-DriveConstants.driveConfig.bumperWidthX() / 2, 0, new Rotation2d(0)));
-  public static final Pose2d startingLinePodium =
+
+  public static final Pose2d startingLineSpike0 =
       new Pose2d(
           FieldConstants.startingLineX - 0.5,
           FieldConstants.Stage.podiumLeg.getY(),
           Rotation2d.fromDegrees(180.0));
-  public static final Pose2d startingLineSpike2 =
+  public static final Pose2d startingLineSpike1 =
       new Pose2d(
           FieldConstants.startingLineX - 0.5,
-          FieldConstants.StagingLocations.spikeTranslations[2].getY(),
+          FieldConstants.StagingLocations.spikeTranslations[1].getY(),
           Rotation2d.fromDegrees(180.0));
-  public static final Pose2d startingLineSpike12 =
+  public static final Pose2d startingLineSpike2 =
       new Pose2d(
           FieldConstants.startingLineX - 0.5,
-          (FieldConstants.StagingLocations.spikeTranslations[1].getY()
-                  + FieldConstants.StagingLocations.spikeTranslations[2].getY())
-              / 2.0,
+          FieldConstants.StagingLocations.spikeTranslations[2].getY(),
           Rotation2d.fromDegrees(180.0));
 
   // Shooting positions
@@ -95,7 +94,7 @@ public class DriveTrajectories {
         "davisEthicalAuto_grabCenterline0",
         List.of(
             PathSegment.newBuilder()
-                .addPoseWaypoint(getShootingPose(startingLinePodium.getTranslation()))
+                .addPoseWaypoint(getShootingPose(startingLineSpike0.getTranslation()))
                 .addTranslationWaypoint(
                     FieldConstants.Stage.podiumLeg
                         .getTranslation()
@@ -218,9 +217,12 @@ public class DriveTrajectories {
         "sourceFRC6328_grabSpike0",
         List.of(
             PathSegment.newBuilder()
-                .addPoseWaypoint(startingSourceFace)
+                .addPoseWaypoint(startingLineSpike0)
                 .addPoseWaypoint(
-                    spike0ShootingPose.transformBy(new Translation2d(0.8, 0.0).toTransform2d()))
+                    new Pose2d(
+                        FieldConstants.StagingLocations.spikeTranslations[0].plus(
+                            new Translation2d(-0.6, 0)),
+                        spike0ShootingPose.getRotation()))
                 .addPoseWaypoint(
                     spike0ShootingPose.transformBy(new Translation2d(0.5, 0.0).toTransform2d()))
                 .build()));
@@ -280,7 +282,7 @@ public class DriveTrajectories {
         "centerSpike_grabSpike0",
         List.of(
             PathSegment.newBuilder()
-                .addPoseWaypoint(startingCenterFace)
+                .addPoseWaypoint(startingLineSpike1)
                 .addPoseWaypoint(
                     spike0ShootingPose.transformBy(new Translation2d(0.8, 0.0).toTransform2d()))
                 .addPoseWaypoint(
@@ -291,7 +293,7 @@ public class DriveTrajectories {
         "ampSpike_grabSpike2",
         List.of(
             PathSegment.newBuilder()
-                .addPoseWaypoint(startingAmpFace)
+                .addPoseWaypoint(startingLineSpike2)
                 .addPoseWaypoint(
                     spike2ShootingPose.transformBy(new Translation2d(0.8, 0.0).toTransform2d()))
                 .addPoseWaypoint(
@@ -341,6 +343,15 @@ public class DriveTrajectories {
                         FieldConstants.StagingLocations.centerlineTranslations[2].getY()))
                 .addPoseWaypoint(stageCenterShootingPose)
                 .build()));
+
+    // N5_S01_C2_S2
+    /*paths.put("N5_S01_C2_S2"),
+    List.of(
+            PathSegment.newBuilder()
+                    .addWaypoints(getLastWaypoint("sourceFRC6328_grabSpike1"))
+                    .addPoseWaypoint()
+
+    )*/
   }
 
   // calculate Pose2d of robot given a translation