From c7b8b530576cc03a20ebd5f7e71886795c2e34eb Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 28 Sep 2025 03:56:29 +0300 Subject: [PATCH 1/7] added logic so robot moved back from reef if arm is gonna smack inrto it --- .../CoralPlacingCommands.java | 50 +++++++++++++++++-- .../robot/constants/FieldConstants.java | 1 + .../robot/subsystems/arm/ArmElevator.java | 4 ++ 3 files changed, 50 insertions(+), 5 deletions(-) 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..a03203a9 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -57,9 +57,15 @@ 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() + return GeneralCommands.getContinuousConditionalCommand( + new ParallelCommandGroup( + ArmElevatorCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore), + getAutonomousDriveToReefThenManualDriveCommand(shouldScoreRight).unless(() -> REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1).asProxy() + ).onlyIf(CoralPlacingCommands::isTargetArmAngleAboveCurrentArmAngle), + SwerveCommands.getDriveToPoseCommand( + () -> calculateClosestNoHitReefPose(shouldScoreRight), + AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS), + CoralPlacingCommands::isTargetArmAngleAboveCurrentArmAngle ); } @@ -79,7 +85,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,6 +114,40 @@ 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 isTargetArmAngleAboveCurrentArmAngle() { + final Rotation2d currentAngle = REEF_CHOOSER.getArmElevatorState().targetAngle; + return RobotContainer.ARM_ELEVATOR.armAboveAngle(currentAngle) || RobotContainer.ARM_ELEVATOR.armAtAngle(currentAngle); + } + private static boolean isReadyToScore(boolean shouldScoreRight) { return RobotContainer.ARM_ELEVATOR.atState(REEF_CHOOSER.getArmElevatorState().prepareState, shouldReverseScore()) && RobotContainer.SWERVE.atPose(calculateClosestScoringPose(shouldScoreRight)); @@ -155,7 +195,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..26846d1c 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -45,6 +45,7 @@ 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/subsystems/arm/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevator.java index 6e1541b1..40326dba 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; From f10f3a99f18c86cdacc0104d7eb10a1237dea88f Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 28 Sep 2025 04:17:32 +0300 Subject: [PATCH 2/7] made it work --- .../robot/commands/commandfactories/CoralPlacingCommands.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 a03203a9..e316ec6e 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -65,8 +65,8 @@ private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRig SwerveCommands.getDriveToPoseCommand( () -> calculateClosestNoHitReefPose(shouldScoreRight), AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS), - CoralPlacingCommands::isTargetArmAngleAboveCurrentArmAngle - ); + () -> (isTargetArmAngleAboveCurrentArmAngle() || RobotContainer.SWERVE.atPose(calculateClosestNoHitReefPose(shouldScoreRight))) && !shouldReverseScore() + ).unless(() -> RobotContainer.ARM_ELEVATOR.atState(REEF_CHOOSER.getArmElevatorState()) && RobotContainer.SWERVE.atPose(calculateClosestNoHitReefPose(shouldScoreRight))); } private static Command getAutonomousDriveToReefThenManualDriveCommand(boolean shouldScoreRight) { From 0ccea11d6f90d8a56ea7ab8f0ec598b6ba71b7c0 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 28 Sep 2025 04:24:39 +0300 Subject: [PATCH 3/7] fix --- .../commands/commandfactories/CoralPlacingCommands.java | 8 ++++---- .../java/frc/trigon/robot/subsystems/arm/ArmElevator.java | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) 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 e316ec6e..3d89c47e 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -64,8 +64,8 @@ private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRig ).onlyIf(CoralPlacingCommands::isTargetArmAngleAboveCurrentArmAngle), SwerveCommands.getDriveToPoseCommand( () -> calculateClosestNoHitReefPose(shouldScoreRight), - AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS), - () -> (isTargetArmAngleAboveCurrentArmAngle() || RobotContainer.SWERVE.atPose(calculateClosestNoHitReefPose(shouldScoreRight))) && !shouldReverseScore() + AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).unless(CoralPlacingCommands::shouldReverseScore).until(() -> RobotContainer.SWERVE.atPose(calculateClosestNoHitReefPose(shouldScoreRight))), + () -> (isTargetArmAngleAboveCurrentArmAngle() || RobotContainer.SWERVE.atPose(calculateClosestNoHitReefPose(shouldScoreRight))) ).unless(() -> RobotContainer.ARM_ELEVATOR.atState(REEF_CHOOSER.getArmElevatorState()) && RobotContainer.SWERVE.atPose(calculateClosestNoHitReefPose(shouldScoreRight))); } @@ -144,8 +144,8 @@ private static FlippablePose2d calculateClosestNoHitReefPose(boolean shouldScore } private static boolean isTargetArmAngleAboveCurrentArmAngle() { - final Rotation2d currentAngle = REEF_CHOOSER.getArmElevatorState().targetAngle; - return RobotContainer.ARM_ELEVATOR.armAboveAngle(currentAngle) || RobotContainer.ARM_ELEVATOR.armAtAngle(currentAngle); + final Rotation2d targetAngle = REEF_CHOOSER.getArmElevatorState().targetAngle; + return RobotContainer.ARM_ELEVATOR.armAboveAngle(targetAngle) || RobotContainer.ARM_ELEVATOR.armAtAngle(targetAngle); } private static boolean isReadyToScore(boolean shouldScoreRight) { 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 40326dba..73b5fd78 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevator.java @@ -132,7 +132,7 @@ public boolean armAtAngle(Rotation2d targetAngle) { } public boolean armAboveAngle(Rotation2d targetAngle) { - return targetAngle.getDegrees() > getCurrentArmAngle().getDegrees(); + return targetAngle.getDegrees() < getCurrentArmAngle().getDegrees(); } public boolean elevatorAtPosition(double positionMeters) { From c87fadf4f23d87f95d76413aec56c27288d2554e Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 28 Sep 2025 04:37:25 +0300 Subject: [PATCH 4/7] more fix --- .../commandfactories/CoralPlacingCommands.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) 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 3d89c47e..bae77abb 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -61,12 +61,12 @@ private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRig new ParallelCommandGroup( ArmElevatorCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore), getAutonomousDriveToReefThenManualDriveCommand(shouldScoreRight).unless(() -> REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1).asProxy() - ).onlyIf(CoralPlacingCommands::isTargetArmAngleAboveCurrentArmAngle), + ).onlyIf(CoralPlacingCommands::isPrepareArmAngleAboveCurrentArmAngle), SwerveCommands.getDriveToPoseCommand( () -> calculateClosestNoHitReefPose(shouldScoreRight), AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).unless(CoralPlacingCommands::shouldReverseScore).until(() -> RobotContainer.SWERVE.atPose(calculateClosestNoHitReefPose(shouldScoreRight))), - () -> (isTargetArmAngleAboveCurrentArmAngle() || RobotContainer.SWERVE.atPose(calculateClosestNoHitReefPose(shouldScoreRight))) - ).unless(() -> RobotContainer.ARM_ELEVATOR.atState(REEF_CHOOSER.getArmElevatorState()) && RobotContainer.SWERVE.atPose(calculateClosestNoHitReefPose(shouldScoreRight))); + () -> (isPrepareArmAngleAboveCurrentArmAngle() || RobotContainer.SWERVE.atPose(calculateClosestNoHitReefPose(shouldScoreRight)) || shouldReverseScore()) + ).until(() -> RobotContainer.ARM_ELEVATOR.atState(REEF_CHOOSER.getArmElevatorState().prepareState) && RobotContainer.SWERVE.atPose(calculateClosestNoHitReefPose(shouldScoreRight))); } private static Command getAutonomousDriveToReefThenManualDriveCommand(boolean shouldScoreRight) { @@ -143,8 +143,8 @@ private static FlippablePose2d calculateClosestNoHitReefPose(boolean shouldScore return new FlippablePose2d(closestScoringPose.transformBy(scoringPoseToBranch), false); } - private static boolean isTargetArmAngleAboveCurrentArmAngle() { - final Rotation2d targetAngle = REEF_CHOOSER.getArmElevatorState().targetAngle; + private static boolean isPrepareArmAngleAboveCurrentArmAngle() { + final Rotation2d targetAngle = REEF_CHOOSER.getArmElevatorState().prepareState.targetAngle; return RobotContainer.ARM_ELEVATOR.armAboveAngle(targetAngle) || RobotContainer.ARM_ELEVATOR.armAtAngle(targetAngle); } From 49a2dd1ec3e5cb35042e8fef6503728b721297a3 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 28 Sep 2025 04:50:31 +0300 Subject: [PATCH 5/7] Update CoralPlacingCommands.java --- .../robot/commands/commandfactories/CoralPlacingCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 bae77abb..81653fd3 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -61,7 +61,7 @@ private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRig new ParallelCommandGroup( ArmElevatorCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore), getAutonomousDriveToReefThenManualDriveCommand(shouldScoreRight).unless(() -> REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1).asProxy() - ).onlyIf(CoralPlacingCommands::isPrepareArmAngleAboveCurrentArmAngle), + ), SwerveCommands.getDriveToPoseCommand( () -> calculateClosestNoHitReefPose(shouldScoreRight), AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).unless(CoralPlacingCommands::shouldReverseScore).until(() -> RobotContainer.SWERVE.atPose(calculateClosestNoHitReefPose(shouldScoreRight))), From 4b2927dc54b61cfce38a345560837d89f66e69b6 Mon Sep 17 00:00:00 2001 From: Nummun14 Date: Sat, 27 Sep 2025 22:56:09 -0400 Subject: [PATCH 6/7] made it work --- .../CoralPlacingCommands.java | 48 ++++++++++++------- .../robot/constants/FieldConstants.java | 1 + .../frc/trigon/robot/misc/ReefChooser.java | 19 -------- .../subsystems/arm/ArmElevatorConstants.java | 1 + 4 files changed, 32 insertions(+), 37 deletions(-) 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 81653fd3..9deee0e1 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( @@ -57,25 +58,33 @@ private static Command getScoreCommand(boolean shouldScoreRight) { } private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRight) { - return GeneralCommands.getContinuousConditionalCommand( - new ParallelCommandGroup( - ArmElevatorCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore), - getAutonomousDriveToReefThenManualDriveCommand(shouldScoreRight).unless(() -> REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1).asProxy() - ), - SwerveCommands.getDriveToPoseCommand( - () -> calculateClosestNoHitReefPose(shouldScoreRight), - AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).unless(CoralPlacingCommands::shouldReverseScore).until(() -> RobotContainer.SWERVE.atPose(calculateClosestNoHitReefPose(shouldScoreRight))), - () -> (isPrepareArmAngleAboveCurrentArmAngle() || RobotContainer.SWERVE.atPose(calculateClosestNoHitReefPose(shouldScoreRight)) || shouldReverseScore()) - ).until(() -> RobotContainer.ARM_ELEVATOR.atState(REEF_CHOOSER.getArmElevatorState().prepareState) && RobotContainer.SWERVE.atPose(calculateClosestNoHitReefPose(shouldScoreRight))); + return new ParallelCommandGroup( + getOpenArmElevatorIfWontHitReef(shouldScoreRight), + new SequentialCommandGroup( + getAutonomousDriveToNoHitReefPose(shouldScoreRight).asProxy().onlyWhile(() -> !isPrepareArmAngleAboveCurrentArmAngle()), + getAutonomousDriveToReef(shouldScoreRight).asProxy() + ) + ); } - 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 ); } @@ -144,7 +153,10 @@ private static FlippablePose2d calculateClosestNoHitReefPose(boolean shouldScore } private static boolean isPrepareArmAngleAboveCurrentArmAngle() { - final Rotation2d targetAngle = REEF_CHOOSER.getArmElevatorState().prepareState.targetAngle; + 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); } diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 26846d1c..1918a305 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -46,6 +46,7 @@ public class FieldConstants { 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/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), From 28a67a4f6ea35c7de03851a95f6463308e7aab84 Mon Sep 17 00:00:00 2001 From: Shm Date: Sat, 27 Sep 2025 23:08:41 -0400 Subject: [PATCH 7/7] made L1 not reverse --- .../robot/commands/commandfactories/CoralPlacingCommands.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 9deee0e1..29ff5e76 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -63,7 +63,7 @@ private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRig new SequentialCommandGroup( getAutonomousDriveToNoHitReefPose(shouldScoreRight).asProxy().onlyWhile(() -> !isPrepareArmAngleAboveCurrentArmAngle()), getAutonomousDriveToReef(shouldScoreRight).asProxy() - ) + ).unless(() -> REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1) ); } @@ -166,6 +166,8 @@ private static boolean isReadyToScore(boolean 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();