Girls gen updates

src/main/java/frc/robot/Controls.java

@@ -170,12 +170,16 @@ private void configureDrivebaseBindings() {
         // applying the request to drive with the inputs
         s.drivebaseSubsystem
             .applyRequest(
-                () ->
-                    drive
-                        .withVelocityX(soloController.isConnected() ? getSoloDriveX() : getDriveX())
-                        .withVelocityY(soloController.isConnected() ? getSoloDriveY() : getDriveY())
-                        .withRotationalRate(
-                            soloController.isConnected() ? getSoloDriveRotate() : getDriveRotate()))
+                () -> {
+                  boolean soloConnected = soloController.isConnected();
+                  SwerveRequest.FieldCentric request =
+                      drive
+                          .withVelocityX(soloConnected ? getSoloDriveX() : getDriveX())
+                          .withVelocityY(soloConnected ? getSoloDriveY() : getDriveY())
+                          .withRotationalRate(
+                              soloConnected ? getSoloDriveRotate() : getDriveRotate());
+                  return request;
+                })
             .withName("Drive"));
 
     // various former controls that were previously used and could be referenced in the future
@@ -495,9 +499,12 @@ private Command getCoralBranchHeightCommand(ScoringType version) {
     } else {
       return switch (branchHeight) {
         case CORAL_LEVEL_FOUR -> superStructure.coralLevelFour(driverController.rightBumper());
-        case CORAL_LEVEL_THREE -> superStructure.coralLevelThree(driverController.rightBumper());
-        case CORAL_LEVEL_TWO -> superStructure.coralLevelTwo(driverController.rightBumper());
-        case CORAL_LEVEL_ONE -> superStructure.coralLevelOne(driverController.rightBumper());
+        case CORAL_LEVEL_THREE -> superStructure.coralLevelThree(
+            driverController.rightBumper(), () -> false);
+        case CORAL_LEVEL_TWO -> superStructure.coralLevelTwo(
+            driverController.rightBumper(), () -> false);
+        case CORAL_LEVEL_ONE -> superStructure.coralLevelOne(
+            driverController.rightBumper(), () -> false);
       };
     }
   }

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

@@ -14,6 +14,32 @@
 import java.util.function.Supplier;
 
 public class SuperStructure {
+  private static class CoralLevelConfig {
+    final String name;
+    final double elevatorPrePos;
+    final double armScore;
+    final double preTimeout;
+
+    CoralLevelConfig(String name, double elevatorPrePos, double armScore, double preTimeout) {
+      this.name = name;
+      this.elevatorPrePos = elevatorPrePos;
+      this.armScore = armScore;
+      this.preTimeout = preTimeout;
+    }
+  }
+
+  private static final CoralLevelConfig L4 =
+      new CoralLevelConfig(
+          "L4", ElevatorSubsystem.CORAL_LEVEL_FOUR_PRE_POS, ArmPivot.CORAL_PRESET_PRE_L4, 0.7);
+
+  private static final CoralLevelConfig L3 =
+      new CoralLevelConfig(
+          "L3", ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS, ArmPivot.CORAL_PRESET_L3, 0.5);
+
+  private static final CoralLevelConfig L2 =
+      new CoralLevelConfig(
+          "L2", ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS, ArmPivot.CORAL_PRESET_L2, 0.5);
+
   private final ElevatorSubsystem elevator;
   private final ArmPivot armPivot;
   private final SpinnyClaw spinnyClaw;
@@ -57,16 +83,6 @@ private Command colorSet(int r, int g, int b, String name) {
     return elevatorLight.colorSet(r, g, b, name);
   }
 
-  private Command repeatPrescoreScoreSwing(Command command, BooleanSupplier score) {
-    // repeats scoring sequence if the coral is still in the claw
-    if (armSensor == null) {
-      return Commands.sequence(
-          command, Commands.waitUntil(() -> !score.getAsBoolean()), Commands.waitUntil(score));
-    } else {
-      return command.repeatedly().onlyWhile(armSensor.inClaw());
-    }
-  }
-
   private Command repeatPrescoreScoreSwing(
       Command command, BooleanSupplier score, BooleanSupplier ipScore) {
     // repeats scoring sequence if the coral is still in the claw
@@ -75,172 +91,58 @@ private Command repeatPrescoreScoreSwing(
           command,
           Commands.waitUntil(() -> !score.getAsBoolean()),
           Commands.waitUntil(ipScore).until(score));
-    } else {
-      return command.repeatedly().onlyWhile(armSensor.inClaw());
     }
+    return command.repeatedly().onlyWhile(armSensor.inClaw());
   }
 
-  public Command coralLevelFour(BooleanSupplier score) {
-    if (branchSensors != null) { // checks if sensor enabled then use for faster scoring
+  private Command coralLevel(
+      CoralLevelConfig cfg,
+      BooleanSupplier score,
+      BooleanSupplier ipScore,
+      boolean skipPreIntakeInAuto) {
+
+    if (branchSensors != null) {
       score = branchSensors.withinScoreRange().or(score);
     }
-    return Commands.sequence(
-            Commands.parallel(
-                    Commands.print("Pre position"),
-                    elevator
-                        .setLevel(ElevatorSubsystem.CORAL_LEVEL_FOUR_PRE_POS)
-                        .deadlineFor( // keeps spinny claw engaged until coral has been scored
-                            armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP).until(score)),
-                    spinnyClaw.stop())
-                .withTimeout(0.7),
-            repeatPrescoreScoreSwing(
-                Commands.sequence(
-                    Commands.parallel(
-                            elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_FOUR_PRE_POS),
-                            armPivot.moveToPosition(ArmPivot.CORAL_PRESET_PRE_L4))
-                        .withDeadline(
-                            Commands.waitUntil(
-                                score)), // waits until driver presses the score button or until
-                    // auto scoring happens
-                    armPivot
-                        .moveToPosition(ArmPivot.CORAL_PRESET_DOWN)
-                        .withTimeout(1.5)
-                        .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))),
-                score),
-            Commands.print("Pre preIntake()"),
-            coralPreIntake()
-                .unless(
-                    RobotModeTriggers
-                        .autonomous()), // doesn't go to preintake if auto, should add for otehr
-            // commands
-            Commands.print("Post preIntake()"))
-        .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L4").asProxy())
-        .withName("Coral Level 4");
-  }
 
-  public Command coralLevelThree(BooleanSupplier score) { // same as L4
     return Commands.sequence(
             Commands.parallel(
                     elevator
-                        .setLevel(ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS)
-                        .deadlineFor(
-                            armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP).until(score)),
-                    spinnyClaw.stop())
-                .withTimeout(0.5),
-            repeatPrescoreScoreSwing(
-                Commands.repeatingSequence(
-                    armPivot
-                        .moveToPosition(ArmPivot.CORAL_PRESET_L3)
-                        .withDeadline(Commands.waitUntil(score)),
-                    armPivot
-                        .moveToPosition(ArmPivot.CORAL_PRESET_DOWN)
-                        .withTimeout(1.5)
-                        .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))),
-                score),
-            coralPreIntake())
-        .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L3").asProxy())
-        .withName("Coral Level 3");
-  }
-
-  public Command coralLevelTwo(BooleanSupplier score) { // same as L4 and L3
-    return Commands.sequence(
-            Commands.parallel(
-                    elevator
-                        .setLevel(ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS)
-                        .deadlineFor(
-                            armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP).until(score)),
-                    spinnyClaw.stop())
-                .withTimeout(0.5),
-            repeatPrescoreScoreSwing(
-                Commands.sequence(
-                    armPivot
-                        .moveToPosition(ArmPivot.CORAL_PRESET_L2)
-                        .withDeadline(Commands.waitUntil(score)),
-                    armPivot
-                        .moveToPosition(ArmPivot.CORAL_PRESET_DOWN)
-                        .withTimeout(1.5)
-                        .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))),
-                score),
-            coralPreIntake())
-        .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L2").asProxy())
-        .withName("Coral Level 2");
-  }
-
-  public Command coralLevelOne(BooleanSupplier score) {
-    return Commands.sequence(
-            Commands.parallel(
-                    elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_ONE_POS),
-                    armPivot.moveToPosition(ArmPivot.CORAL_PRESET_L1),
-                    spinnyClaw.stop()) // holds coral without wearing flywheels
-                .withTimeout(0.5)
-                .withDeadline(Commands.waitUntil(score)),
-            spinnyClaw.coralLevelOneHoldExtakePower().withTimeout(0.25), // spits out coral
-            Commands.waitSeconds(1), // Wait to clear the reef
-            coralPreIntake())
-        .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L1").asProxy())
-        .withName("Coral Level 1");
-  }
-
-  // New versions with ipScore
-  // if robot is in position for intermediate scoring, it can score early but if vision is dead
-  // manual control still works
-  // only used on solo controls
-
-  public Command coralLevelThree(BooleanSupplier score, BooleanSupplier ipScore) { // same as L4
-    return Commands.sequence(
-            Commands.parallel(
-                    elevator
-                        .setLevel(ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS)
+                        .setLevel(cfg.elevatorPrePos)
                         .deadlineFor(
                             armPivot
                                 .moveToPosition(ArmPivot.CORAL_PRESET_UP)
                                 .until(ipScore)
                                 .until(score)),
                     spinnyClaw.stop())
-                .withTimeout(0.5),
+                .withTimeout(cfg.preTimeout),
             repeatPrescoreScoreSwing(
-                Commands.repeatingSequence(
+                Commands.sequence(
                     armPivot
-                        .moveToPosition(ArmPivot.CORAL_PRESET_L3)
+                        .moveToPosition(cfg.armScore)
                         .withDeadline(Commands.waitUntil(ipScore).until(score)),
                     armPivot
                         .moveToPosition(ArmPivot.CORAL_PRESET_DOWN)
                         .withTimeout(1.5)
                         .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))),
                 score,
                 ipScore),
-            coralPreIntake())
-        .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L3").asProxy())
-        .withName("Coral Level 3");
+            coralPreIntake()
+                .unless(() -> skipPreIntakeInAuto && RobotModeTriggers.autonomous().getAsBoolean()))
+        .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L" + cfg.name).asProxy())
+        .withName("Coral Level " + cfg.name);
   }
 
-  public Command coralLevelTwo(
-      BooleanSupplier score, BooleanSupplier ipScore) { // same as L4 and L3
-    return Commands.sequence(
-            Commands.parallel(
-                    elevator
-                        .setLevel(ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS)
-                        .deadlineFor(
-                            armPivot
-                                .moveToPosition(ArmPivot.CORAL_PRESET_UP)
-                                .until(ipScore)
-                                .until(score)),
-                    spinnyClaw.stop())
-                .withTimeout(0.5),
-            repeatPrescoreScoreSwing(
-                Commands.sequence(
-                    armPivot
-                        .moveToPosition(ArmPivot.CORAL_PRESET_L2)
-                        .withDeadline(Commands.waitUntil(ipScore).until(score)),
-                    armPivot
-                        .moveToPosition(ArmPivot.CORAL_PRESET_DOWN)
-                        .withTimeout(1.5)
-                        .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))),
-                score,
-                ipScore),
-            coralPreIntake())
-        .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L2").asProxy())
-        .withName("Coral Level 2");
+  public Command coralLevelFour(BooleanSupplier score) {
+    return coralLevel(L4, score, () -> false, true);
+  }
+
+  public Command coralLevelThree(BooleanSupplier score, BooleanSupplier ipScore) {
+    return coralLevel(L3, score, ipScore, false);
+  }
+
+  public Command coralLevelTwo(BooleanSupplier score, BooleanSupplier ipScore) {
+    return coralLevel(L2, score, ipScore, false);
   }
 
   public Command coralLevelOne(BooleanSupplier score, BooleanSupplier ipScore) {