added d side neutral stuff

Author: Wu-Fan-529

src/main/java/frc/robot/RobotContainer.java

@@ -26,6 +26,7 @@
 import frc.robot.commands.ResetPose;
 import frc.robot.constants.ChoreoTraj;
 import frc.robot.constants.ConstAuto;
+import frc.robot.constants.ConstField;
 import frc.robot.constants.ConstSystem;
 import frc.robot.constants.ConstSystem.constControllers;
 import frc.robot.subsystems.DriverStateMachine;
@@ -220,11 +221,29 @@ public void configAutonomous() {
         driverStateMachineInstance // The drive subsystem
     );
 
-    Command AutoPIDTuning = Commands.sequence(runPath(ChoreoTraj.AutoPIDTuning));
+    Command AutoPIDTuning = Commands.sequence(runPath(ChoreoTraj.AutoPIDTuning).asProxy());
+
     Command DoNothing = Commands.none();
 
+    Command DSideNeutral = Commands.sequence(
+        CollectAndScore(ChoreoTraj.DSideTrenchToNeutral,
+            ChoreoTraj.FirstDSideNeutralToAlliance,
+            ChoreoTraj.DSideAllianceToDepot,
+            ConstAuto.SHOOTING_TIMEOUT));
+
+    Command DSideDoubleNeutral = Commands.sequence(
+        DSideNeutral.asProxy(),
+        CollectAndScore(ChoreoTraj.DSideTrenchToNeutral,
+            ChoreoTraj.SecondDSideNeutralToAlliance,
+            ChoreoTraj.DSideAllianceToTrench,
+            ConstAuto.SHOOTING_TIMEOUT));
+
+    // Command DSideNeutralWithDepot = Commands.sequence(
+    // DSideNeutral.asProxy(),
+    // CollectAndScore(null, null, null, null));
+
     // Example: Add autonomous routines to the chooser
-    // Add more autonomous routines as needed, e.g.:
+    // Add more autonomous routines as needed, e.g.:\
     // autoChooser.addOption("Score and Leave", runPath("ScoreAndLeave"));
     autoChooser.setDefaultOption("Do Nothing", DoNothing);
     autoChooser.addOption("AutoPIDTuning", AutoPIDTuning);
@@ -274,6 +293,27 @@ Command ShootingOnMove(ChoreoTraj shootingPath, Time shootingTime) {
         TRY_NONE.asProxy());
   }
 
+  Command CollectAndScore(ChoreoTraj intakingPath, ChoreoTraj returnToAlliancePath, ChoreoTraj shootingPath,
+      Time shootingTime) {
+    return Commands.sequence(
+        Commands.runOnce(() -> stateMachineInstance.setRobotState(RobotState.NONE)).asProxy(),
+        runPath(intakingPath).asProxy().deadlineFor(TRY_INTAKING.asProxy()),
+        Commands.parallel(
+            runPath(returnToAlliancePath).asProxy(),
+            Commands.sequence(
+                // TRY_INTAKING is a deferred/instant command that finishes immediately after
+                // requesting the state. Using .until(...) on it will therefore end right away.
+                // Instead, explicitly request the INTAKING state once and then wait until the
+                // drivetrain is behind the horizontal line before continuing to prep shooting.
+                TRY_INTAKING.asProxy(),
+                Commands
+                    .waitUntil(() -> drivetrainInstance.isBehindHorizontalLine(ConstField.FieldElements.ALLIANCE_LINE,
+                        ConstField.isRedAlliance(), ConstField.FIELD_LENGTH)),
+                TRY_SHOOTING_ON_FLY.asProxy().withTimeout(ConstAuto.PREP_SHOOT_TIMEOUT))),
+        runPath(intakingPath).asProxy().alongWith(TRY_SHOOTING_ON_FLY.asProxy().withTimeout(shootingTime)),
+        TRY_NONE.asProxy());
+  }
+
   public Command runPath(ChoreoTraj path) {
     return autoFactory.trajectoryCmd(path.name()).asProxy()
         .alongWith(Commands.runOnce(() -> {

src/main/java/frc/robot/constants/ChoreoVars.java

@@ -4,7 +4,6 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.*;
-import edu.wpi.first.wpilibj2.command.Command;
 
 /**
  * Generated file containing variables defined in Choreo.

src/main/java/frc/robot/constants/ConstAuto.java

@@ -4,8 +4,12 @@
 
 package frc.robot.constants;
 
+import static edu.wpi.first.units.Units.Seconds;
+
+import edu.wpi.first.units.measure.Time;
+
 /** Add your docs here. */
 public class ConstAuto {
   public static final double PREP_SHOOT_TIMEOUT = 4;
-
+  public static final Time SHOOTING_TIMEOUT = Seconds.of(4.5);
 }

src/main/java/frc/robot/constants/ConstField.java

@@ -110,8 +110,8 @@ public List<Pose2d> getAlliancePoses() {
     }
   }
 
-  public static final Distance FIELD_LENGTH = Units.Inches.of(651.2); // TODO: Update from game manual
-  public static final Distance FIELD_WIDTH = Units.Inches.of(317.7); // TODO: Update from game manual
+  public static final Distance FIELD_LENGTH = Units.Inches.of(651.2);
+  public static final Distance FIELD_WIDTH = Units.Inches.of(317.7);
 
   /**
    * Boolean that controls when the path will be mirrored for the red
@@ -133,7 +133,7 @@ public static boolean isRedAlliance() {
   public static class FieldElements {
     // TODO: Replace with actual measurement
     private static final Pose2d RESET_POSE = new Pose2d(0, 0, new Rotation2d());
-
+    public static final Distance ALLIANCE_LINE = Units.Meters.of(4.008662700653076);
   }
 
   public static class FieldElementGroups {