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

Author: ShmayaR

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

@@ -10,13 +10,13 @@
 import frc.trigon.robot.constants.FieldConstants;
 import frc.trigon.robot.constants.OperatorConstants;
 import frc.trigon.robot.misc.ReefChooser;
+import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands;
 import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants;
 import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands;
 import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants;
 import frc.trigon.robot.subsystems.swerve.SwerveCommands;
 import lib.utilities.flippable.FlippablePose2d;
 import lib.utilities.flippable.FlippableTranslation2d;
-import org.littletonrobotics.junction.AutoLogOutput;
 
 public class CoralPlacingCommands {
     public static boolean SHOULD_SCORE_AUTONOMOUSLY = true;
@@ -79,8 +79,9 @@ private static Command getAutonomousDriveToNoHitReefPose(boolean shouldScoreRigh
     }
 
     private static Command getPrepareArmElevatorIfWontHitReef(boolean shouldScoreRight) {
-        return GeneralCommands.runWhen(
+        return GeneralCommands.getContinuousConditionalCommand(
                 GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, true, CoralPlacingCommands::shouldReverseScore),
+                ArmElevatorCommands.getStayInPlaceCommand(),
                 () -> CoralPlacingCommands.isPrepareArmAngleAboveCurrentArmAngle() || calculateDistanceToTargetScoringPose(shouldScoreRight) > FieldConstants.SAFE_DISTANCE_FROM_SCORING_POSE_METERS
         );
     }

src/main/java/frc/trigon/robot/constants/FieldConstants.java

@@ -32,7 +32,7 @@ public class FieldConstants {
             REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS = 0.17,
             REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6,
             REEF_CENTER_TO_TARGET_NO_HIT_REEF_POSITION_X_TRANSFORM_METERS = REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS + 0.2;
-    public static final double SAFE_DISTANCE_FROM_SCORING_POSE_METERS = 0.17;
+    public static final double SAFE_DISTANCE_FROM_SCORING_POSE_METERS = 0.15;
 
     private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false;
     public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = createAprilTagFieldLayout();

src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java

@@ -44,16 +44,21 @@ public static Command resetElevatorPositionCommand() {
         return new ParallelCommandGroup(
                 new ExecuteEndCommand(
                         () -> RobotContainer.ARM_ELEVATOR.setTargetArmState(ArmElevatorConstants.ArmElevatorState.ZERO_ELEVATOR, false),
-                        () -> {}
+                        () -> {
+                        }
                 ).until(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.ZERO_ELEVATOR)),
                 new ExecuteEndCommand(
                         () -> RobotContainer.ARM_ELEVATOR.setElevatorVoltage(OperatorConstants.DRIVER_CONTROLLER.getRightY() * 2),
-                        () -> {},
+                        () -> {
+                        },
                         RobotContainer.ARM_ELEVATOR
                 ),
                 SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0),
                 EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.EJECT)
-        ).finallyDo(() -> {RobotContainer.ARM_ELEVATOR.resetElevatorPosition(); RobotContainer.END_EFFECTOR.setTargetState(EndEffectorConstants.EndEffectorState.REST);});
+        ).finallyDo(() -> {
+            RobotContainer.ARM_ELEVATOR.resetElevatorPosition();
+            RobotContainer.END_EFFECTOR.setTargetState(EndEffectorConstants.EndEffectorState.REST);
+        });
     }
 
     public static Command getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState targetState) {
@@ -77,6 +82,17 @@ public static Command getSetTargetStateCommand(Supplier<ArmElevatorConstants.Arm
         );
     }
 
+    public static Command getStayInPlaceCommand() {
+        return new ExecuteEndCommand(
+                () -> {
+                    RobotContainer.ARM_ELEVATOR.setTargetArmAngle(RobotContainer.ARM_ELEVATOR.getCurrentArmAngle(), true);
+                    RobotContainer.ARM_ELEVATOR.setTargetElevatorPositionMeters(RobotContainer.ARM_ELEVATOR.getCurrentElevatorPositionMeters(), true);
+                },
+                RobotContainer.ARM_ELEVATOR::stop,
+                RobotContainer.ARM_ELEVATOR
+        );
+    }
+
     public static Command getPrepareForStateCommand(Supplier<ArmElevatorConstants.ArmElevatorState> targetState) {
         return getPrepareForStateCommand(targetState, () -> false);
     }