Ground intake

.vscode/settings.json

@@ -58,5 +58,6 @@
     "edu.wpi.first.math.**.struct.*",
   ],
   "java.compile.nullAnalysis.mode": "automatic",
-  "java.debug.settings.onBuildFailureProceed": true
+  "java.debug.settings.onBuildFailureProceed": true,
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable"
 }

src/main/deploy/choreo/choreo.chor

@@ -3,7 +3,15 @@
  "version":1,
  "type":"Swerve",
  "variables":{
-  "expressions":{},
+  "expressions":{
+   "slow":{
+    "dimension":"LinVel",
+    "var":{
+     "exp":"1.25 m / s",
+     "val":1.25
+    }
+   }
+  },
   "poses":{
    "A":{
     "x":{

src/main/java/frc/robot/Autos.java

@@ -27,6 +27,7 @@
 import java.util.HashMap;
 import java.util.Optional;
 import java.util.function.Supplier;
+import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
 public class Autos {
@@ -35,8 +36,9 @@ public class Autos {
   private final FunnelSubsystem funnel;
   private final AutoFactory factory;
 
-  public static boolean autoPreScore = false;
-  public static boolean autoScore = false; // TODO perhaps this should not be static
+  @AutoLogOutput public static boolean autoPreScore = true;
+  @AutoLogOutput public static boolean autoScore = false; // TODO perhaps this should not be static
+  @AutoLogOutput public static boolean autoGroundIntake = false;
 
   public Autos(SwerveSubsystem swerve, ManipulatorSubsystem manipulator, FunnelSubsystem funnel) {
     this.swerve = swerve;
@@ -141,6 +143,7 @@ public void runPath(
   public Command LOtoJ() {
     final var routine = factory.newRoutine("LO to J");
     bindElevatorExtension(routine);
+    routine.active().onTrue(Commands.print("Auto!"));
     HashMap<String, AutoTrajectory> steps =
         new HashMap<String, AutoTrajectory>(); // key - name of path, value - traj
     String[] stops = {
@@ -164,11 +167,25 @@ public Command LOtoJ() {
       runPath(routine, startPos, endPos, nextPos, steps);
     }
 
+    // final var groundTraj = routine.trajectory("LtoAGround");
+
+    // routine
+    //     .observe(steps.get("PLOtoL").recentlyDone())
+    //     .onTrue(scoreInAuto(() -> steps.get("PLOtoL").getFinalPose().get()))
+    //     .and(() -> !manipulator.getSecondBeambreak() && !manipulator.getFirstBeambreak())
+    //     .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2)))
+    //     .onTrue(groundTraj.cmd().andThen(scoreInAuto(() -> groundTraj.getFinalPose().get())))
+    //     .onTrue(Commands.runOnce(() -> autoGroundIntake = true));
+
+    // routine
+    //     .observe(groundTraj.done())
+    //     .onTrue(Commands.runOnce(() -> autoGroundIntake = false).ignoringDisable(true));
+
     routine
         .observe(steps.get("LtoPLM").done())
         .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2)));
 
-    return routine.cmd();
+    return routine.cmd().alongWith(Commands.print("auto :("));
   }
 
   public Command ROtoE() {