From b309dca3a2a026d166cd4d296ec6baf4584b926b Mon Sep 17 00:00:00 2001
From: Jonah <47046556+jwbonner@users.noreply.github.com>
Date: Sun, 31 Mar 2024 21:45:56 -0400
Subject: [PATCH] Spiky auto tuning

---
 .../org/littletonrobotics/frc2024/RobotState.java    |  7 ++-----
 .../frc2024/commands/auto/AutoBuilder.java           |  7 ++-----
 .../drive/trajectory/DriveTrajectories.java          | 12 ++++++++++++
 3 files changed, 16 insertions(+), 10 deletions(-)

diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java
index 8199f1e4..56b8c245 100644
--- a/src/main/java/org/littletonrobotics/frc2024/RobotState.java
+++ b/src/main/java/org/littletonrobotics/frc2024/RobotState.java
@@ -49,7 +49,6 @@ public record AimingParameters(
       new LoggedTunableNumber("RobotState/AutoLookahead", 0.5);
   private static final LoggedTunableNumber lookahead =
       new LoggedTunableNumber("RobotState/lookaheadS", 0.35);
-  @Setter private boolean useAutoLookahead = false;
   private static final double poseBufferSizeSeconds = 2.0;
 
   private static final double armAngleCoefficient = 57.254371165197;
@@ -210,10 +209,8 @@ public AimingParameters getAimingParameters() {
             .plus(FudgeFactors.speaker.getTransform());
     Pose2d fieldToPredictedVehicle;
     if (DriverStation.isAutonomousEnabled()) {
-      fieldToPredictedVehicle =
-          useAutoLookahead
-              ? getPredictedPose(autoLookahead.get(), autoLookahead.get())
-              : getEstimatedPose();
+      fieldToPredictedVehicle = getPredictedPose(autoLookahead.get(), autoLookahead.get());
+
     } else {
       fieldToPredictedVehicle =
           lookaheadDisable.getAsBoolean()
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 3e0b22a5..5dae9088 100644
--- a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java
+++ b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java
@@ -19,7 +19,6 @@
 import lombok.RequiredArgsConstructor;
 import org.littletonrobotics.frc2024.AutoSelector.AutoQuestionResponse;
 import org.littletonrobotics.frc2024.FieldConstants;
-import org.littletonrobotics.frc2024.RobotState;
 import org.littletonrobotics.frc2024.subsystems.drive.Drive;
 import org.littletonrobotics.frc2024.subsystems.drive.trajectory.DriveTrajectories;
 import org.littletonrobotics.frc2024.subsystems.drive.trajectory.HolonomicTrajectory;
@@ -37,7 +36,7 @@ public class AutoBuilder {
 
   private static final double preloadDelay = 1.0;
   private static final double spikeIntakeDelay = 0.35;
-  private static final double spikeFeedThroughDelay = 0.35;
+  private static final double spikeFeedThroughDelay = 0.45;
   private static final double stageAimX = FieldConstants.Stage.center.getX() - 0.3;
 
   /** All shot compensation values used when shooting corresponding centerline note. */
@@ -164,9 +163,7 @@ private Command scoreSpikes(AutoQuestionResponse startingLocation, boolean score
                         () -> scoresThree)))
 
         // Always aim and run flywheels
-        .deadlineWith(superstructure.aimWithCompensation(0.0), flywheels.shootCommand())
-        .beforeStarting(() -> RobotState.getInstance().setUseAutoLookahead(true))
-        .finallyDo(() -> RobotState.getInstance().setUseAutoLookahead(false));
+        .deadlineWith(superstructure.aimWithCompensation(0.0), flywheels.shootCommand());
   }
 
   /** Scores two centerline notes with the given trajectories. */
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 535b79b3..77703205 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
@@ -292,6 +292,12 @@ public class DriveTrajectories {
               .addPoseWaypoint(
                   new Pose2d(centerlineNote, shotToCenterlineIntakeOrientation)
                       .transformBy(new Translation2d(centerlineIntakeOffset, 0.0).toTransform2d()))
+              .addPoseWaypoint(
+                  new Pose2d(centerlineNote, shotToCenterlineIntakeOrientation)
+                      .transformBy(
+                          new Translation2d(
+                                  centerlineIntakeOffset + centerlinePrepareIntakeOffset, 0.0)
+                              .toTransform2d()))
               .build();
       shotToCenterlineIntakeSegments[i] = shotToCenterlineIntake;
 
@@ -327,6 +333,12 @@ public class DriveTrajectories {
                     new Pose2d(centerlineNote, spikeTocenterlineIntakeOrientation)
                         .transformBy(
                             new Translation2d(centerlineIntakeOffset, 0.0).toTransform2d()))
+                .addPoseWaypoint(
+                    new Pose2d(centerlineNote, spikeTocenterlineIntakeOrientation)
+                        .transformBy(
+                            new Translation2d(
+                                    centerlineIntakeOffset + centerlinePrepareIntakeOffset, 0.0)
+                                .toTransform2d()))
                 .build();
         spikeToCenterlineIntakeSegments[spikeIndex][i] = spikeToCenterline;
       }