All the offseason changes put together.

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

@@ -151,7 +151,10 @@ private void configureDrivebaseBindings() {
       // Stop running this method
       return;
     }
-
+    // if the solo controller is connected, use it for driving
+    // gets set when robot code starts running for the first time so usally is false and isnt
+    // currently in use
+    boolean soloCC = soloController.isConnected();
     // Note that X is defined as forward according to WPILib convention,
     // and Y is defined as to the left according to WPILib convention.
 
@@ -379,9 +382,6 @@ private void configureSuperStructureBindings() {
                               .asProxy()
                           : Commands.none())
                   .withName("Automatic Intake"));
-    }
-
-    if (sensors.armSensor != null) {
       sensors
           .armSensor
           .inClaw()
@@ -394,12 +394,7 @@ private void configureSuperStructureBindings() {
                           case ALGAE -> soloScoringMode = SoloScoringMode.ALGAE_IN_CLAW;
                         }
                       })
-                  .withName("Set solo scoring mode"));
-
-      sensors
-          .armSensor
-          .inClaw()
-          .and(RobotModeTriggers.teleop())
+                  .withName("Set solo scoring mode"))
           .onFalse(
               Commands.runOnce(
                       () -> {
@@ -718,7 +713,7 @@ private void configureSpinnyClawBindings() {
                   } else {
                     return switch (scoringMode) {
                       case CORAL -> Commands.none().withName("No manual spit - Coral mode");
-                      case ALGAE -> s.spinnyClawSubsytem.algaeExtakePower();
+                      case ALGAE -> s.spinnyClawSubsytem.algaeExtakeProcessorPower();
                     };
                   }
                 }));
@@ -884,7 +879,7 @@ public void vibrateCoDriveController(double vibration) {
     }
   }
 
-  private double getJoystickInput(double input) {
+  private double getSoloJoystickInput(double input) {
     if (soloController.leftStick().getAsBoolean() || soloController.rightStick().getAsBoolean()) {
       return 0; // stop driving if either stick is pressed
     }
@@ -898,65 +893,101 @@ private double getJoystickInput(double input) {
   private double getSoloDriveX() {
     // Joystick +Y is back
     // Robot +X is forward
-    return getJoystickInput(-soloController.getLeftY()) * MaxSpeed;
+    return getSoloJoystickInput(-soloController.getLeftY()) * MaxSpeed;
   }
 
   // takes the Y value from the joystick, and applies a deadband and input scaling
   private double getSoloDriveY() {
     // Joystick +X is right
     // Robot +Y is left
-    return getJoystickInput(-soloController.getLeftX()) * MaxSpeed;
+    return getSoloJoystickInput(-soloController.getLeftX()) * MaxSpeed;
   }
 
   // takes the rotation value from the joystick, and applies a deadband and input scaling
   private double getSoloDriveRotate() {
     // Joystick +X is right
     // Robot +angle is CCW (left)
-    return getJoystickInput(-soloController.getRightX()) * MaxSpeed;
+    return getSoloJoystickInput(-soloController.getRightX()) * MaxSpeed;
   }
 
   private void configureSoloControllerBindings() {
     // Barge + Auto align left + Select scoring mode Algae
     soloController
         .leftTrigger()
         .onTrue(
-            Commands.runOnce(
-                () -> {
-                  Command scoreCommand;
-                  switch (soloScoringMode) {
-                    case CORAL_IN_CLAW -> {
-                      scoreCommand = getSoloCoralBranchHeightCommand();
-                    }
-                    case ALGAE_IN_CLAW -> {
-                      Command bargeScoreCommand =
-                          BargeAlign.bargeScore(
+            Commands.deferredProxy(
+                    () -> {
+                      Command scoreCommand;
+                      switch (soloScoringMode) {
+                        case CORAL_IN_CLAW -> {
+                          scoreCommand =
+                              getSoloCoralBranchHeightCommand()
+                                  .until(
+                                      () ->
+                                          soloController.a().getAsBoolean()
+                                              || soloController.b().getAsBoolean()
+                                              || soloController.x().getAsBoolean()
+                                              || soloController.y().getAsBoolean());
+                        }
+
+                        case ALGAE_IN_CLAW -> {
+                          Command bargeScoreCommand =
+                              BargeAlign.bargeScore(
                                   s.drivebaseSubsystem,
                                   superStructure,
                                   () -> getSoloDriveX(),
                                   () -> getSoloDriveY(),
                                   () -> getSoloDriveRotate(),
-                                  soloController.rightBumper())
-                              .withName("Algae score then intake");
-                      scoreCommand =
-                          Commands.sequence(
-                              bargeScoreCommand,
-                              Commands.runOnce(
-                                  () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE));
-                    }
-                    case NO_GAME_PIECE -> {
-                      scoreCommand =
-                          Commands.parallel(
-                              Commands.runOnce(() -> intakeMode = ScoringMode.ALGAE)
-                                  .alongWith(scoringModeSelectRumble())
-                                  .withName("Algae Scoring Mode"),
-                              AutoAlgaeHeights.autoAlgaeIntakeCommand(
-                                      s.drivebaseSubsystem, superStructure, this)
-                                  .until(() -> sensors.armSensor.booleanInClaw()));
-                    }
-                    default -> scoreCommand = Commands.none();
-                  }
-                  CommandScheduler.getInstance().schedule(scoreCommand);
-                }));
+                                  soloController.rightBumper());
+
+                          if (sensors.intakeSensor.booleanInIntake()) {
+                            Command holdGroundIntakeOut =
+                                superStructure.holdGroundIntakeOut(soloController.povUp());
+
+                            Command quickHandoff =
+                                Commands.sequence(
+                                        superStructure
+                                            .quickGroundIntake(() -> true)
+                                            .withName("Quick Gound intake"),
+                                        Commands.runOnce(
+                                            () -> soloScoringMode = soloScoringMode.CORAL_IN_CLAW))
+                                    .withName("Quick Handoff After Algae Score");
+
+                            scoreCommand =
+                                holdGroundIntakeOut
+                                    .deadlineFor(bargeScoreCommand)
+                                    .andThen(quickHandoff)
+                                    .withName("Algae Score + Ground Handoff");
+                          } else {
+                            scoreCommand =
+                                Commands.sequence(
+                                        bargeScoreCommand,
+                                        Commands.runOnce(
+                                            () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE))
+                                    .withName("Barge Algae Score");
+                          }
+                        }
+
+                        case NO_GAME_PIECE -> {
+                          scoreCommand =
+                              Commands.parallel(
+                                  Commands.runOnce(() -> intakeMode = ScoringMode.ALGAE)
+                                      .alongWith(scoringModeSelectRumble())
+                                      .withName("Algae Scoring Mode"),
+                                  AutoAlgaeHeights.autoAlgaeIntakeCommand(
+                                          s.drivebaseSubsystem, superStructure, this)
+                                      .until(() -> sensors.armSensor.booleanInClaw()));
+                        }
+
+                        default -> {
+                          scoreCommand = Commands.none();
+                        }
+                      }
+
+                      return scoreCommand;
+                    })
+                .withName("Solo Left Trigger Action"));
+
     soloController
         .leftTrigger()
         .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW)
@@ -966,15 +997,14 @@ private void configureSoloControllerBindings() {
     soloController
         .rightTrigger()
         .onTrue(
-            Commands.runOnce(
+            Commands.deferredProxy(
                     () -> {
                       Command scoreCommand =
                           switch (soloScoringMode) {
                             case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommand();
                             case ALGAE_IN_CLAW -> Commands.sequence(
                                     superStructure.algaeProcessorScore(
                                         soloController.rightBumper()),
-                                    Commands.waitSeconds(0.7),
                                     Commands.runOnce(
                                         () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE))
                                 .withName("Processor score");
@@ -985,22 +1015,34 @@ private void configureSoloControllerBindings() {
                                 superStructure.coralPreIntake(),
                                 s.climbPivotSubsystem.toStow());
                           };
-                      CommandScheduler.getInstance().schedule(scoreCommand);
+                      return scoreCommand;
                     })
                 .withName("score"));
     soloController
         .rightTrigger()
         .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW)
         .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE)
         .whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this));
-    // Ground Intake
     soloController
         .leftBumper()
         .onTrue(
-            Commands.parallel(
-                    Commands.runOnce(() -> intakeMode = ScoringMode.CORAL),
-                    superStructure.quickGroundIntake(soloController.povUp()))
-                .withName("Quick Gound intake"));
+            Commands.runOnce(
+                () -> {
+                  Command intakeCommand;
+                  switch (soloScoringMode) {
+                    case CORAL_IN_CLAW, NO_GAME_PIECE -> intakeCommand =
+                        superStructure
+                            .quickGroundIntake(soloController.povUp())
+                            .withName("Quick Ground Intake")
+                            .alongWith(Commands.runOnce(() -> intakeMode = ScoringMode.CORAL));
+                    case ALGAE_IN_CLAW -> intakeCommand =
+                        superStructure
+                            .holdGroundIntakeOut(soloController.povUp())
+                            .withName("Hold Ground Intake Out");
+                    default -> intakeCommand = Commands.none();
+                  }
+                  CommandScheduler.getInstance().schedule(intakeCommand);
+                }));
     // Scoring levels coral and algae intake heights
     soloController
         .y()

src/main/java/frc/robot/sensors/IntakeSensor.java

@@ -59,4 +59,9 @@ public Trigger inIntake() {
             })
         .debounce(0.1);
   }
+
+  public boolean booleanInIntake() {
+    double distance = getSensorDistance().in(Meters);
+    return distance > INTAKE_LOWER_LIMIT && distance < INTAKE_UPPER_LIMIT;
+  }
 }

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

@@ -160,8 +160,7 @@ public Command algaeIntakePower() {
     return setPower(ALGAE_INTAKE_SPEED).withName("Algae intake power");
   }
 
-  public Command
-      algaeExtakePower() { // can change to net extake maybe? also bound as general extake though
+  public Command algaeExtakeNetPower() {
     return setPower(ALGAE_EXTAKE_SPEED).withName("Algae extake power");
   }
 

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

@@ -203,6 +203,21 @@ public Command groundIntake(BooleanSupplier retract) {
     }
   }
 
+  public Command holdGroundIntakeOut(BooleanSupplier retract) {
+    if (groundSpinny == null || groundArm == null) {
+      return Commands.none().withName("hold ground intake disabled");
+    }
+
+    return Commands.sequence(
+            Commands.parallel(
+                groundArm
+                    .moveToPosition(GroundArm.GROUND_POSITION)
+                    .andThen(groundArm.setVoltage(GroundArm.GROUND_HOLD_VOLTAGE))
+                    .withDeadline(Commands.waitUntil(retract)),
+                groundSpinny.setGroundIntakePower()))
+        .withName("Hold Ground Intake Out");
+  }
+
   // This is the actual version in use. It moves the coral directly into the claw.
   public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph
     if (groundSpinny == null || groundArm == null || intakeSensor == null) {
@@ -421,7 +436,7 @@ public Command algaeNetScore(BooleanSupplier score) {
                 armPivot.moveToPosition(ArmPivot.ALGAE_NET_SCORE),
                 spinnyClaw.algaeIntakePower()),
             Commands.waitUntil(score),
-            spinnyClaw.algaeHoldExtakePower().withTimeout(0.7),
+            spinnyClaw.algaeExtakeNetPower(),
             Commands.waitSeconds(0.7),
             armPivot.moveToPosition(
                 ArmPivot.CORAL_PRESET_UP), // added to prevent hitting the barge after scoring net