No Hit Reef Logic

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

@@ -24,6 +24,7 @@
 public class CoralPlacingCommands {
     public static boolean SHOULD_SCORE_AUTONOMOUSLY = true;
     private static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER;
+    private static final double SAFE_DISTANCE_FROM_SCORING_POSE_METERS = 0.2;
 
     public static Command getScoreInReefCommand(boolean shouldScoreRight) {
         return new SequentialCommandGroup(
@@ -58,18 +59,32 @@ private static Command getScoreCommand(boolean shouldScoreRight) {
 
     private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRight) {
         return new ParallelCommandGroup(
-                ArmElevatorCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore),
-                getAutonomousDriveToReefThenManualDriveCommand(shouldScoreRight).asProxy()
+                getOpenArmElevatorIfWontHitReef(shouldScoreRight),
+                new SequentialCommandGroup(
+                        getAutonomousDriveToNoHitReefPose(shouldScoreRight).asProxy().onlyWhile(() -> !isPrepareArmAngleAboveCurrentArmAngle()),
+                        getAutonomousDriveToReef(shouldScoreRight).asProxy()
+                ).unless(() -> REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1)
         );
     }
 
-    private static Command getAutonomousDriveToReefThenManualDriveCommand(boolean shouldScoreRight) {
-        return new SequentialCommandGroup(
-                SwerveCommands.getDriveToPoseCommand(
-                        () -> CoralPlacingCommands.calculateClosestScoringPose(shouldScoreRight),
-                        AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS
-                ),
-                GeneralCommands.getFieldRelativeDriveCommand()
+    private static Command getAutonomousDriveToReef(boolean shouldScoreRight) {
+        return SwerveCommands.getDriveToPoseCommand(
+                () -> CoralPlacingCommands.calculateClosestScoringPose(shouldScoreRight),
+                AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS
+        );
+    }
+
+    private static Command getAutonomousDriveToNoHitReefPose(boolean shouldScoreRight) {
+        return SwerveCommands.getDriveToPoseCommand(
+                () -> CoralPlacingCommands.calculateClosestNoHitReefPose(shouldScoreRight),
+                AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS
+        );
+    }
+
+    private static Command getOpenArmElevatorIfWontHitReef(boolean shouldScoreRight) {
+        return new GeneralCommands().runWhen(
+                ArmElevatorCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore),
+                () -> CoralPlacingCommands.isPrepareArmAngleAboveCurrentArmAngle() || calculateDistanceToTargetScoringPose(shouldScoreRight) > SAFE_DISTANCE_FROM_SCORING_POSE_METERS
         );
     }
 
@@ -79,7 +94,7 @@ private static double calculateDistanceToTargetScoringPose(boolean shouldScoreRi
         return currentTranslation.getDistance(targetTranslation);
     }
 
-    public static FlippablePose2d calculateClosestScoringPose(boolean shouldScoreRight) {
+    private static FlippablePose2d calculateClosestScoringPose(boolean shouldScoreRight) {
         final Translation2d robotPositionOnField = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation();
         final Translation2d reefCenterPosition = new FlippableTranslation2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, true).get();
         final Rotation2d[] reefClockAngles = FieldConstants.REEF_CLOCK_ANGLES;
@@ -108,12 +123,51 @@ public static FlippablePose2d calculateClosestScoringPose(boolean shouldScoreRig
         return new FlippablePose2d(closestScoringPose.transformBy(scoringPoseToBranch), false);
     }
 
+    private static FlippablePose2d calculateClosestNoHitReefPose(boolean shouldScoreRight) {
+        final Translation2d robotPositionOnField = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation();
+        final Translation2d reefCenterPosition = new FlippableTranslation2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, true).get();
+        final Rotation2d[] reefClockAngles = FieldConstants.REEF_CLOCK_ANGLES;
+        final Transform2d reefCenterToScoringPose = new Transform2d(FieldConstants.REEF_CENTER_TO_TARGET_NO_HIT_REEF_POSITION_X_TRANSFORM_METERS, 0, new Rotation2d());
+
+        double distanceFromClosestScoringPoseMeters = Double.POSITIVE_INFINITY;
+        Pose2d closestScoringPose = new Pose2d();
+        for (final Rotation2d targetRotation : reefClockAngles) {
+            final Pose2d reefCenterAtTargetRotation = new Pose2d(reefCenterPosition, targetRotation);
+
+
+            final Pose2d currentScoringPose = reefCenterAtTargetRotation.transformBy(reefCenterToScoringPose);
+            final double distanceFromCurrentScoringPoseMeters = currentScoringPose.getTranslation().getDistance(robotPositionOnField);
+            if (distanceFromCurrentScoringPoseMeters < distanceFromClosestScoringPoseMeters) {
+                distanceFromClosestScoringPoseMeters = distanceFromCurrentScoringPoseMeters;
+                closestScoringPose = currentScoringPose;
+            }
+        }
+
+        final Transform2d scoringPoseToBranch = new Transform2d(
+                0,
+                shouldScoreRight ? FieldConstants.REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS : -FieldConstants.REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS,
+                shouldReverseScore() ? Rotation2d.k180deg : new Rotation2d()
+        );
+
+        return new FlippablePose2d(closestScoringPose.transformBy(scoringPoseToBranch), false);
+    }
+
+    private static boolean isPrepareArmAngleAboveCurrentArmAngle() {
+        ArmElevatorConstants.ArmElevatorState targetState = REEF_CHOOSER.getArmElevatorState();
+        final Rotation2d targetAngle = targetState.prepareState == null
+                ? targetState.targetAngle
+                : targetState.prepareState.targetAngle;
+        return RobotContainer.ARM_ELEVATOR.armAboveAngle(targetAngle) || RobotContainer.ARM_ELEVATOR.armAtAngle(targetAngle);
+    }
+
     private static boolean isReadyToScore(boolean shouldScoreRight) {
         return RobotContainer.ARM_ELEVATOR.atState(REEF_CHOOSER.getArmElevatorState().prepareState, shouldReverseScore())
                 && RobotContainer.SWERVE.atPose(calculateClosestScoringPose(shouldScoreRight));
     }
 
     private static boolean shouldReverseScore() {
+        if (REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1)
+            return false;
         final Rotation2d robotRotation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation();
         final Translation2d robotTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation();
         final Translation2d reefCenterTranslation = FieldConstants.FLIPPABLE_REEF_CENTER_TRANSLATION.get();
@@ -155,7 +209,7 @@ public enum ScoringLevel {
             this.rotationTransform = rotationTransform;
             this.armElevatorState = determineArmElevatorState();
         }
-        
+
 
         private ArmElevatorConstants.ArmElevatorState determineArmElevatorState() {
             return switch (level) {

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

@@ -45,6 +45,8 @@ public class FieldConstants {
     public static final double
             REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS = 1.3,
             REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS = 0.17,
+            REEF_CENTER_TO_TARGET_NO_HIT_REEF_POSITION_X_TRANSFORM_METERS = REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS + 0.3,
+
             REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6;
 
     private static AprilTagFieldLayout createAprilTagFieldLayout() {

src/main/java/frc/trigon/robot/misc/ReefChooser.java

@@ -12,13 +12,10 @@
 import java.util.function.Supplier;
 
 public class ReefChooser {
-    private final CommandGenericHID hid;
     private CoralPlacingCommands.ScoringLevel scoringLevel = CoralPlacingCommands.ScoringLevel.L4;
     private FieldConstants.ReefSide reefSide = FieldConstants.ReefSide.LEFT;
 
     public ReefChooser(int port) {
-        hid = new CommandGenericHID(port);
-
         new WaitCommand(3).andThen(this::configureBindings).ignoringDisable(true).schedule();
     }
 
@@ -33,7 +30,6 @@ public ArmElevatorConstants.ArmElevatorState getArmElevatorState() {
 
     private void configureBindings() {
         configureScoringLevelBindings();
-        configureReefHIDBindings();
         configureFallbackBindings();
     }
 
@@ -50,21 +46,6 @@ private Command getSetTargetLevelCommand(Supplier<CoralPlacingCommands.ScoringLe
         ).ignoringDisable(true);
     }
 
-    private void configureReefHIDBindings() {
-        for (int i = 0; i < 12; i++)
-            hid.button(i + 1).onTrue(getSetFaceFromIndexCommand(i));
-    }
-
-    private Command getSetFaceFromIndexCommand(int index) {
-        return new InstantCommand(
-                () -> setFaceSideFromIndex(index)
-        ).ignoringDisable(true);
-    }
-
-    private void setFaceSideFromIndex(int index) {
-        reefSide = FieldConstants.ReefSide.values()[index % 2];
-    }
-
     private void configureFallbackBindings() {
         OperatorConstants.SET_TARGET_REEF_SCORING_SIDE_LEFT_TRIGGER.onTrue(getSetTargetReefSideCommand(FieldConstants.ReefSide.LEFT));
         OperatorConstants.SET_TARGET_REEF_SCORING_SIDE_RIGHT_TRIGGER.onTrue(getSetTargetReefSideCommand(FieldConstants.ReefSide.RIGHT));

src/main/java/frc/trigon/robot/subsystems/arm/ArmElevator.java

@@ -131,6 +131,10 @@ public boolean armAtAngle(Rotation2d targetAngle) {
         return currentToTargetAngleDifferenceDegrees < ArmElevatorConstants.ANGLE_TOLERANCE.getDegrees();
     }
 
+    public boolean armAboveAngle(Rotation2d targetAngle) {
+        return targetAngle.getDegrees() < getCurrentArmAngle().getDegrees();
+    }
+
     public boolean elevatorAtPosition(double positionMeters) {
         final double currentToTargetStateDifferenceMeters = Math.abs(positionMeters - getCurrentElevatorPositionMeters());
         return currentToTargetStateDifferenceMeters < ArmElevatorConstants.ELEVATOR_POSITION_TOLERANCE_METERS;

src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java

@@ -327,6 +327,7 @@ private static void configureReverseLimitSensor() {
     }
 
     public enum ArmElevatorState {
+        PREPARE_SCORE_L1(Rotation2d.fromDegrees(110), 0.3, null, false, 1),
         PREPARE_SCORE_L2(Rotation2d.fromDegrees(100), 0.3, null, false, 1),
         PREPARE_SCORE_L3(Rotation2d.fromDegrees(100), 0.7, null, false, 1),
         PREPARE_SCORE_L4(Rotation2d.fromDegrees(120), 1.2, null, false, 1),