Event cmp

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

@@ -335,7 +335,11 @@ public Command CMtoGH() { // algae path
     routine
         // run first path
         .active()
-        .whileTrue(Commands.sequence(steps.get("CMtoG").resetOdometry(), steps.get("CMtoG").cmd()));
+        .whileTrue(
+            Commands.sequence(
+                steps.get("CMtoG").resetOdometry(),
+                Commands.waitSeconds(1.5),
+                steps.get("CMtoG").cmd()));
 
     routine
         .observe(steps.get("CMtoG").done()) // TODO change to time based
@@ -360,6 +364,68 @@ public Command CMtoGH() { // algae path
             Commands.sequence(
                 intakeAlgaeInAuto(() -> steps.get("NItoIJ").getFinalPose()),
                 swerve.driveTeleop(() -> new ChassisSpeeds(-0.5, 0, 0)).withTimeout(0.2)));
+
+    // routine
+    //     .observe(
+    //         steps
+    //             .get("IJtoNI")
+    //             .atTime(steps.get("IJtoNI").getRawTrajectory().getTotalTime() - 0.2)) // TODO
+    // tune
+    //     .onTrue(Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoEF").cmd()));
+    // routine
+    //     .observe(steps.get("NItoEF").done())
+    //     .onTrue(Commands.sequence(intakeAlgaeInAuto(() -> steps.get("NItoEF").getFinalPose())));
+
+    // ---------------------------
+    // Commands.sequence(
+    //     // AutoAim.translateToXCoord(
+    //     //         swerve,
+    //     //         () ->
+    //     //             DriverStation.getAlliance().get() == Alliance.Blue
+    //     //                 ? AutoAim.BLUE_NET_X
+    //     //                 : AutoAim.RED_NET_X,
+    //     //         () -> 0,
+    //     //         () ->
+    //     //             DriverStation.getAlliance().get() == Alliance.Blue
+    //     //                 ? Rotation2d.k180deg
+    //     //                 : Rotation2d.kZero.plus(Rotation2d.fromDegrees(20.0)))
+    //         AutoAim.translateToPose(swerve,
+    //         () -> new Pose2d(DriverStation.getAlliance().get() == Alliance.Blue
+    //                         ? AutoAim.BLUE_NET_X
+    //                         : AutoAim.RED_NET_X,
+    //                         steps.get("GhtoNI").))
+    //         .until(
+    //             () ->
+    //                 (MathUtil.isNear(
+    //                         DriverStation.getAlliance().get() == Alliance.Blue
+    //                             ? AutoAim.BLUE_NET_X
+    //                             : AutoAim.RED_NET_X,
+    //                         swerve.getPose().getX(),
+    //                         Units.inchesToMeters(5))
+    //                     && MathUtil.isNear(
+    //                         DriverStation.getAlliance().get() == Alliance.Blue
+    //                             ? Rotation2d.k180deg
+    //                                 .plus(Rotation2d.fromDegrees(20.0))
+    //                                 .getDegrees()
+    //                             : Rotation2d.kZero
+    //                                 .plus(Rotation2d.fromDegrees(20.0))
+    //                                 .getDegrees(),
+    //                         swerve.getPose().getRotation().getDegrees(),
+    //                         5.0))),
+    // scoreAlgaeInAuto())
+    // )
+
+    // routine.observe(steps.get("NItoIJ").done()).; //TODO cancel into autoalign
+
+    // for (int i = 0; i < stops.length - 2; i++) {
+    //   String startPos = stops[i];
+    //   String endPos = stops[i + 1];
+    //   String nextPos = stops[i + 2];
+    //   runAlgaePath(routine, startPos, endPos, nextPos, steps);
+    // }
+    // routine
+    //     .observe(steps.get("NItoEF").done())
+    //     .onTrue(intakeAlgaeInAuto(() -> steps.get("NItoEF").getFinalPose()));
     return routine.cmd();
   }
 
@@ -370,6 +436,9 @@ public Command LOtoA() { // 2910
     // lo a b4 b2 dealgae
     steps.put("LOtoA", routine.trajectory("LOtoA"));
     steps.put("AtoB", routine.trajectory("AtoB"));
+    steps.put("BtoB", routine.trajectory("BtoB"));
+
+    if (Robot.isSimulation()) manipulator.setSecondBeambreak(true); // gah
     routine
         // run first path
         .active()
@@ -381,12 +450,41 @@ public Command LOtoA() { // 2910
         .onTrue(
             Commands.sequence(
                 scoreCoralInAuto(() -> steps.get("LOtoA").getFinalPose().get()),
-                Commands.runOnce(() -> autoGroundCoralIntake = true),
-                swerve
-                    .driveTeleop(() -> new ChassisSpeeds(-0.3, 0, 0))
+                AutoAim.translateToPose(swerve, () -> steps.get("AtoB").getInitialPose().get())
                     .until(
-                        () -> elevator.isNearExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS)),
+                        () ->
+                            elevator.isNearExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS)
+                                && AutoAim.isInTolerance(
+                                    swerve.getPose(), steps.get("AtoB").getInitialPose().get())),
+                Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2)),
                 steps.get("AtoB").cmd()));
+    routine
+        .observe(
+            steps
+                .get("AtoB")
+                .active()
+                .and(() -> manipulator.getSecondBeambreak() || manipulator.getFirstBeambreak()))
+        .onTrue(
+            Commands.runOnce(
+                () -> {
+                  autoGroundCoralIntake = false;
+                  Robot.setCurrentTarget(ReefTarget.L4);
+                }));
+
+    routine
+        .observe(
+            steps.get("AtoB").atTime(steps.get("AtoB").getRawTrajectory().getTotalTime() - 0.3))
+        .onTrue(
+            Commands.sequence(
+                scoreCoralInAuto(() -> steps.get("AtoB").getFinalPose().get()),
+                AutoAim.translateToPose(swerve, () -> steps.get("BtoB").getInitialPose().get())
+                    .until(
+                        () ->
+                            elevator.isNearExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS)
+                                && AutoAim.isInTolerance(
+                                    swerve.getPose(), steps.get("BtoB").getInitialPose().get())),
+                Commands.runOnce(() -> autoGroundCoralIntake = true),
+                steps.get("BtoB").cmd()));
 
     // routine
     //     .observe(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak())
@@ -443,7 +541,7 @@ public Command scoreCoralInAuto(Supplier<Pose2d> trajEndPose) {
                                     0.0,
                                     swerve.getVelocityRobotRelative().omegaRadiansPerSecond,
                                     3.0))
-                    .debounce(0.06)),
+                    .debounce(0.06 * 2)),
             Commands.print("Scoring!"),
             Commands.runOnce(
                 () -> {
@@ -517,6 +615,13 @@ public void bindCoralElevatorExtension(AutoRoutine routine, double toleranceMete
         .whileFalse(Commands.run(() -> autoPreScore = false));
   }
 
+  public void bindGroundCoralElevatorExtension(
+      AutoRoutine routine, double toleranceMeters, Trigger trigger) {
+    routine.observe(trigger).debounce(3).onTrue(Commands.run(() -> autoGroundCoralIntake = true));
+  }
+
+  // ([[[){ ([[[){ ([[[){ ([[[){ ([[[){ ([[[){ ([[[){ ([[[){ ([[[){ ([[[){
+
   public void bindAlgaeElevatorExtension(AutoRoutine routine, double toleranceMeters) {
     routine
         .observe(

src/main/java/frc/robot/Robot.java

@@ -188,9 +188,9 @@ public static enum AlgaeScoreTarget {
     PROCESSOR
   }
 
-  private static ReefTarget currentTarget = ReefTarget.L4;
-  private static AlgaeIntakeTarget algaeIntakeTarget = AlgaeIntakeTarget.STACK;
-  private static AlgaeScoreTarget algaeScoreTarget = AlgaeScoreTarget.NET;
+  @AutoLogOutput private static ReefTarget currentTarget = ReefTarget.L4;
+  @AutoLogOutput private static AlgaeIntakeTarget algaeIntakeTarget = AlgaeIntakeTarget.STACK;
+  @AutoLogOutput private static AlgaeScoreTarget algaeScoreTarget = AlgaeScoreTarget.NET;
   private boolean leftHandedTarget = false;
 
   @AutoLogOutput private boolean killVisionIK = true;
@@ -327,8 +327,8 @@ public static enum AlgaeScoreTarget {
                   new SimpleMotorFeedforward(0.0, 0.7),
                   new ProfiledPIDController(
                       0.5, 0.0, 0.0, new TrapezoidProfile.Constraints(15, 1))),
-          new BeambreakIOReal(1, true),
-          new BeambreakIOReal(0, true));
+          new BeambreakIOReal(0, true),
+          new BeambreakIOReal(1, true));
 
   private final ShoulderSubsystem shoulder =
       new ShoulderSubsystem(
@@ -492,6 +492,7 @@ public static enum AlgaeScoreTarget {
               .debounce(0.5)
               .or(operator.leftStick().and(operator.rightTrigger()).debounce(0.5)),
           driver.a(),
+          driver.b(),
           driver.start(),
           operator.rightBumper(),
           operator.leftBumper(),
@@ -1007,7 +1008,7 @@ public Robot() {
                                     > Math.abs(swerve.getPose().getX() - AutoAim.RED_NET_X)
                                 ? Rotation2d.kZero
                                 : Rotation2d.k180deg)
-                            .plus(Rotation2d.fromDegrees(30.0))),
+                            .plus(Rotation2d.fromDegrees(20.0))),
                 Commands.waitUntil(
                         () -> {
                           final var diff =

src/main/java/frc/robot/subsystems/ExtensionKinematics.java

@@ -50,7 +50,7 @@ public class ExtensionKinematics {
           Rotation2d.fromRadians(2.427));
   public static final Pose2d L3_POSE = solveFK(L3_EXTENSION);
   public static final Pose2d L4_POSE =
-      new Pose2d(new Translation2d(0.20, 2.03), Rotation2d.fromDegrees(110.0));
+      new Pose2d(new Translation2d(0.20, 2.03), Rotation2d.fromDegrees(115.0));
 
   public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE);
 

src/main/java/frc/robot/subsystems/ExtensionPathing.java

@@ -9,6 +9,7 @@
 import com.google.common.graph.MutableGraph;
 import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
 import frc.robot.subsystems.ExtensionKinematics.ExtensionState;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.shoulder.ShoulderSubsystem;
@@ -56,6 +57,9 @@ public class ExtensionPathing {
     graph.addNode(l3Tucked);
     graph.putEdge(tucked, l3Tucked);
     graph.putEdge(l3Tucked, l2Tucked);
+    final var l3 = ExtensionKinematics.L3_EXTENSION;
+    graph.addNode(l3);
+    graph.putEdge(l3, l3Tucked);
     final var l4Tucked =
         new ExtensionState(
             ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(),
@@ -124,21 +128,30 @@ public class ExtensionPathing {
         new ExtensionState(
             ElevatorSubsystem.INTAKE_ALGAE_LOW_EXTENSION,
             ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS,
-            ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS);
+            WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS);
     graph.addNode(algaeLow);
     graph.putEdge(betweenTucked, algaeLow);
 
     final var algaeHigh =
         new ExtensionState(
             ElevatorSubsystem.INTAKE_ALGAE_HIGH_EXTENSION,
             ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS,
-            ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS);
+            WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS);
     graph.addNode(algaeHigh);
     graph.putEdge(betweenTucked, algaeHigh);
     graph.putEdge(algaeLow, algaeHigh);
 
     graph.putEdge(l4Tucked, algaeHigh);
     graph.putEdge(l4Tucked, algaeLow);
+
+    final var algaeHalfTucked =
+        new ExtensionState(
+            Units.inchesToMeters(27.6),
+            ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS,
+            WristSubsystem.WRIST_TUCKED_CLEARANCE_POS);
+    graph.addNode(algaeHalfTucked);
+    graph.putEdge(algaeHalfTucked, algaeLow);
+    graph.putEdge(algaeHalfTucked, algaeHigh);
   }
 
   private ExtensionPathing() {}