Merge branch 'dev' of https://github.com/Programming-TRIGON/RobotCodeOffSeason2025 into dev

Author: ShmayaR

src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java

@@ -195,7 +195,7 @@ private static Command getCollectAlgaeFromLollipopSequenceCommand() {
                         .onlyIf(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE)
                         .until(() -> !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE)
                         .repeatedly()
-        );
+        ).finallyDo(() -> SHOULD_COLLECT_FROM_LOLLIPOP = false);
     }
 
     private static Command getCollectAlgaeFromFloorSequenceCommand() {

src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java

@@ -22,13 +22,10 @@ public class CoralPlacingCommands {
     static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER;
 
     public static Command getScoreInReefCommand(boolean shouldScoreRight) {
-        return new SequentialCommandGroup(
-                CoralCollectionCommands.getLoadCoralCommand(),
-                new ConditionalCommand(
-                        getAutonomouslyScoreCommand(shouldScoreRight),
-                        getScoreCommand(shouldScoreRight),
-                        () -> SHOULD_SCORE_AUTONOMOUSLY && REEF_CHOOSER.getScoringLevel() != ScoringLevel.L1
-                )
+        return new ConditionalCommand(
+                getAutonomouslyScoreCommand(shouldScoreRight),
+                getScoreCommand(shouldScoreRight),
+                () -> SHOULD_SCORE_AUTONOMOUSLY && REEF_CHOOSER.getScoringLevel() != ScoringLevel.L1
         ).onlyIf(CoralCollectionCommands::hasCoral);
     }
 
@@ -44,6 +41,7 @@ private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) {
 
     private static Command getScoreCommand(boolean shouldScoreRight) {
         return new SequentialCommandGroup(
+                CoralCollectionCommands.getLoadCoralCommand(),
                 getPrepareArmElevatorIfWontHitReef(shouldScoreRight).until(OperatorConstants.CONTINUE_TRIGGER),
                 new ParallelCommandGroup(
                         GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, false, CoralPlacingCommands::shouldReverseScore),
@@ -54,7 +52,9 @@ private static Command getScoreCommand(boolean shouldScoreRight) {
 
     private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRight) {
         return new ParallelCommandGroup(
-                getPrepareArmElevatorIfWontHitReef(shouldScoreRight),
+                new SequentialCommandGroup(
+                        CoralCollectionCommands.getLoadCoralCommand(),
+                        getPrepareArmElevatorIfWontHitReef(shouldScoreRight)),
                 new SequentialCommandGroup(
                         getAutonomousDriveToNoHitReefPose(shouldScoreRight).asProxy().until(CoralPlacingCommands::isPrepareArmAngleAboveCurrentArmAngle),
                         new WaitUntilCommand(CoralPlacingCommands::isPrepareArmAngleAboveCurrentArmAngle),