diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index f5bc1e1a..29ff5e76 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/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) { diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 9f7b8daa..1918a305 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/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() { diff --git a/src/main/java/frc/trigon/robot/misc/ReefChooser.java b/src/main/java/frc/trigon/robot/misc/ReefChooser.java index a7a5b885..3d45a603 100644 --- a/src/main/java/frc/trigon/robot/misc/ReefChooser.java +++ b/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 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)); diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevator.java index 6e1541b1..73b5fd78 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevator.java +++ b/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; diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java index a547ab75..ee976ede 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java +++ b/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),