No Hit Reef Logic #24
Merged
Conversation
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
WalkthroughAdds gated autonomous scoring preparation and no‑hit reef navigation: CoralPlacingCommands now gates prepare-score by an arm-angle check, splits driving into no‑hit and reef targets, adds helper gates and calculations; FieldConstants adds a no‑hit X offset; ArmElevator gains armAboveAngle; ReefChooser HID bindings removed; new PREPARE_SCORE_L1 state added. Changes
Sequence Diagram(s)sequenceDiagram
auton->>CoralPlacingCommands: getAutonomouslyPrepareScoreCommand(level, side)
activate CoralPlacingCommands
CoralPlacingCommands->>ArmElevator: isPrepareArmAngleAboveCurrentArmAngle()
ArmElevator-->>CoralPlacingCommands: boolean
alt armAboveAngle == true
CoralPlacingCommands->>GeneralCommands: getContinuousConditionalCommand(cond, innerParallel)
activate GeneralCommands
note right of GeneralCommands #DDEBF7: cond = isPrepareArmAngleAboveCurrentArmAngle()
par prepare-and-drive
opt level != L1
GeneralCommands->>Drive: getAutonomousDriveToNoHitReefPose(shouldScoreRight)
Drive-->>GeneralCommands: reached no-hit pose
GeneralCommands->>Drive: getAutonomousDriveToReef(shouldScoreRight)
Drive-->>GeneralCommands: reached reef pose
end
GeneralCommands->>ArmElevator: getOpenArmElevatorIfWontHitReef(shouldScoreRight)
GeneralCommands->>ArmElevator: moveArmToTarget(level, side)
GeneralCommands->>Mechanisms: prepareScoring(level)
end
deactivate GeneralCommands
else
note over CoralPlacingCommands: wait until armAboveAngle becomes true
end
deactivate CoralPlacingCommands
Estimated code review effort🎯 3 (Moderate) | ⏱️ ~20 minutes Possibly related PRs
Suggested reviewers
Poem
Pre-merge checks and finishing touches❌ Failed checks (1 warning)
✅ Passed checks (2 passed)
✨ Finishing touches
🧪 Generate unit tests
📜 Recent review detailsConfiguration used: CodeRabbit UI Review profile: CHILL Plan: Pro 📒 Files selected for processing (1)
🚧 Files skipped from review as they are similar to previous changes (1)
Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out. Comment |
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 1
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (3)
src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java(4 hunks)src/main/java/frc/trigon/robot/constants/FieldConstants.java(1 hunks)src/main/java/frc/trigon/robot/subsystems/arm/ArmElevator.java(1 hunks)
🧰 Additional context used
🧠 Learnings (1)
📚 Learning: 2025-09-18T22:46:49.269Z
Learnt from: ShmayaR
PR: Programming-TRIGON/RobotCodeOffSeason2025#16
File: src/main/java/frc/trigon/robot/subsystems/arm/Arm.java:86-88
Timestamp: 2025-09-18T22:46:49.269Z
Learning: In the arm subsystem for this robot, angles above 90 degrees bypass the isArmAboveSafeAngle() logic, making the Rotation2d wrap-around issue less critical in practice according to the team's current system design.
Applied to files:
src/main/java/frc/trigon/robot/subsystems/arm/ArmElevator.java
🧬 Code graph analysis (1)
src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java (5)
src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java (1)
GeneralCommands(15-65)src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorCommands.java (1)
ArmElevatorCommands(15-78)src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java (1)
SwerveCommands(15-163)src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(27-105)src/main/java/frc/trigon/robot/constants/FieldConstants.java (1)
FieldConstants(17-116)
Comment on lines
146
to
149
| private static boolean isTargetArmAngleAboveCurrentArmAngle() { | ||
| final Rotation2d currentAngle = REEF_CHOOSER.getArmElevatorState().targetAngle; | ||
| return RobotContainer.ARM_ELEVATOR.armAboveAngle(currentAngle) || RobotContainer.ARM_ELEVATOR.armAtAngle(currentAngle); | ||
| } |
There was a problem hiding this comment.
Fix reversed-target handling in arm angle predicate.
When shouldReverseScore() is true we command the arm with the mirrored (360°−θ) angle, but this helper still compares against the unmirrored targetAngle. Past 180°, the predicate stays true, so we never fall back to the no-hit reef path and the arm keeps driving into the reef—exactly what this PR is trying to avoid. Pull the angle from whichever state prepareToState will actually command (prepareState when it exists), mirror it when we are reversed, and then run the comparison.
Apply this diff to align the predicate with the commanded angle:
private static boolean isTargetArmAngleAboveCurrentArmAngle() {
- final Rotation2d currentAngle = REEF_CHOOSER.getArmElevatorState().targetAngle;
- return RobotContainer.ARM_ELEVATOR.armAboveAngle(currentAngle) || RobotContainer.ARM_ELEVATOR.armAtAngle(currentAngle);
+ ArmElevatorConstants.ArmElevatorState targetState = REEF_CHOOSER.getArmElevatorState();
+ if (targetState == null)
+ return false;
+
+ ArmElevatorConstants.ArmElevatorState angleSourceState =
+ targetState.prepareState != null ? targetState.prepareState : targetState;
+ Rotation2d baseAngle = angleSourceState.targetAngle;
+ Rotation2d angleToCompare = shouldReverseScore()
+ ? Rotation2d.fromDegrees(Rotation2d.k180deg.getDegrees() * 2 - baseAngle.getDegrees())
+ : baseAngle;
+
+ return RobotContainer.ARM_ELEVATOR.armAboveAngle(angleToCompare)
+ || RobotContainer.ARM_ELEVATOR.armAtAngle(angleToCompare);
}📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
Suggested change
| 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 isTargetArmAngleAboveCurrentArmAngle() { | |
| ArmElevatorConstants.ArmElevatorState targetState = REEF_CHOOSER.getArmElevatorState(); | |
| if (targetState == null) { | |
| return false; | |
| } | |
| ArmElevatorConstants.ArmElevatorState angleSourceState = | |
| targetState.prepareState != null ? targetState.prepareState : targetState; | |
| Rotation2d baseAngle = angleSourceState.targetAngle; | |
| Rotation2d angleToCompare = shouldReverseScore() | |
| ? Rotation2d.fromDegrees(Rotation2d.k180deg.getDegrees() * 2 - baseAngle.getDegrees()) | |
| : baseAngle; | |
| return RobotContainer.ARM_ELEVATOR.armAboveAngle(angleToCompare) | |
| || RobotContainer.ARM_ELEVATOR.armAtAngle(angleToCompare); | |
| } |
🤖 Prompt for AI Agents
In
src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java
around lines 146 to 149, the predicate uses
REEF_CHOOSER.getArmElevatorState().targetAngle which is the unmirrored angle
causing incorrect behavior when shouldReverseScore() is true; change it to
obtain the actual angle that will be commanded by prepareToState (use the
prepareState if present, otherwise the target state), and if
shouldReverseScore() returns true mirror that angle (compute 360° − θ or
Rotation2d equivalent) before calling
RobotContainer.ARM_ELEVATOR.armAboveAngle(...) / armAtAngle(...), so the
comparison matches the commanded (possibly reversed) arm angle.
There was a problem hiding this comment.
Actionable comments posted: 1
🧹 Nitpick comments (2)
src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java (2)
66-67: Avoid recomputing “closest no‑hit” with different snapshots.Re-evaluating calculateClosestNoHitReefPose(...) for the drive target and for the atPose(...) check can diverge as the “closest” branch changes while driving. Using a shared Supplier (as in the refactor above) keeps both consistent for the lifetime of that command instance.
Also applies to: 68-68
88-115: Deduplicate scoring/no‑hit pose calculators.Both methods differ only by the X transform constant. A single helper reduces drift and ensures consistent branch selection logic.
Proposed extraction (sketch):
+ private static FlippablePose2d calculateClosestReefPose(boolean shouldScoreRight, double xTransformMeters) { + final Translation2d robotPos = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); + final Translation2d reefCenter = new FlippableTranslation2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, true).get(); + final Transform2d centerToPose = new Transform2d(xTransformMeters, 0, new Rotation2d()); + + double best = Double.POSITIVE_INFINITY; + Pose2d closest = new Pose2d(); + for (final Rotation2d rot : FieldConstants.REEF_CLOCK_ANGLES) { + final Pose2d pose = new Pose2d(reefCenter, rot).transformBy(centerToPose); + final double d = pose.getTranslation().getDistance(robotPos); + if (d < best) { + best = d; + closest = pose; + } + } + final Transform2d toBranch = 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(closest.transformBy(toBranch), false); + }Then:
- calculateClosestScoringPose(...): pass FieldConstants.REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS
- calculateClosestNoHitReefPose(...): pass FieldConstants.REEF_CENTER_TO_TARGET_NO_HIT_REEF_POSITION_X_TRANSFORM_METERS
Also applies to: 117-144
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (1)
src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java(4 hunks)
🧰 Additional context used
🧬 Code graph analysis (1)
src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java (6)
src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java (1)
GeneralCommands(15-65)src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorCommands.java (1)
ArmElevatorCommands(15-78)src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java (1)
SwerveCommands(15-163)src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(27-105)src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(46-125)src/main/java/frc/trigon/robot/constants/FieldConstants.java (1)
FieldConstants(17-116)
🔇 Additional comments (1)
src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java (1)
146-149: Fix reversed-angle handling and add null-safety in prepare-angle predicate.When reversed, you must mirror the angle you compare against; otherwise gating stays true/false incorrectly and we can still drive into the reef. Also guard against null target/prepare states.
Apply this diff:
- private static boolean isPrepareArmAngleAboveCurrentArmAngle() { - final Rotation2d targetAngle = REEF_CHOOSER.getArmElevatorState().prepareState.targetAngle; - return RobotContainer.ARM_ELEVATOR.armAboveAngle(targetAngle) || RobotContainer.ARM_ELEVATOR.armAtAngle(targetAngle); - } + private static boolean isPrepareArmAngleAboveCurrentArmAngle() { + ArmElevatorConstants.ArmElevatorState targetState = REEF_CHOOSER.getArmElevatorState(); + if (targetState == null) + return false; + + ArmElevatorConstants.ArmElevatorState angleSourceState = + targetState.prepareState != null ? targetState.prepareState : targetState; + if (angleSourceState == null || angleSourceState.targetAngle == null) + return false; + + Rotation2d baseAngle = angleSourceState.targetAngle; + Rotation2d angleToCompare = shouldReverseScore() + ? Rotation2d.fromDegrees(Rotation2d.k180deg.getDegrees() * 2 - baseAngle.getDegrees()) + : baseAngle; + + return RobotContainer.ARM_ELEVATOR.armAboveAngle(angleToCompare) + || RobotContainer.ARM_ELEVATOR.armAtAngle(angleToCompare); + }If needed, I can also adjust prepareToState to return the effective commanded angle to avoid duplicating this logic.
Comment on lines
60
to
69
| return GeneralCommands.getContinuousConditionalCommand( | ||
| 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))), | ||
| () -> (isPrepareArmAngleAboveCurrentArmAngle() || RobotContainer.SWERVE.atPose(calculateClosestNoHitReefPose(shouldScoreRight)) || shouldReverseScore()) | ||
| ).until(() -> RobotContainer.ARM_ELEVATOR.atState(REEF_CHOOSER.getArmElevatorState().prepareState) && RobotContainer.SWERVE.atPose(calculateClosestNoHitReefPose(shouldScoreRight))); |
There was a problem hiding this comment.
Remove premature termination and redundant gating in autonomous-prep logic.
The inner .until(...) ends the prepare phase as soon as we’re at the no‑hit pose (even if not ready to score), causing the sequence to advance and set scoring states while still off the branch. Also, the extra .onlyIf(...) on onTrue is redundant with GeneralCommands.getContinuousConditionalCommand’s onlyWhile(condition). Recompute of the no‑hit pose in multiple places risks target mismatch.
Apply this refactor:
- 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()
- ).onlyIf(CoralPlacingCommands::isPrepareArmAngleAboveCurrentArmAngle),
- 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)));
- }
+ private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRight) {
+ final java.util.function.Supplier<FlippablePose2d> noHitPose = () -> calculateClosestNoHitReefPose(shouldScoreRight);
+ final java.util.function.BooleanSupplier gate =
+ () -> isPrepareArmAngleAboveCurrentArmAngle() || RobotContainer.SWERVE.atPose(noHitPose.get()) || shouldReverseScore();
+
+ return GeneralCommands.getContinuousConditionalCommand(
+ new ParallelCommandGroup(
+ ArmElevatorCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore),
+ getAutonomousDriveToReefThenManualDriveCommand(shouldScoreRight)
+ .unless(() -> REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1)
+ .asProxy()
+ ),
+ SwerveCommands.getDriveToPoseCommand(noHitPose, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS)
+ .unless(CoralPlacingCommands::shouldReverseScore),
+ gate
+ );
+ }This leaves the termination to the caller: in getAutonomouslyScoreCommand it remains gated by .until(isReadyToScore(...)), ensuring we only advance once we’re at the scoring pose and prepared. If you prefer, I can push the .until(isReadyToScore) inside and simplify the callers.
📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
Suggested change
| return GeneralCommands.getContinuousConditionalCommand( | |
| 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))), | |
| () -> (isPrepareArmAngleAboveCurrentArmAngle() || RobotContainer.SWERVE.atPose(calculateClosestNoHitReefPose(shouldScoreRight)) || shouldReverseScore()) | |
| ).until(() -> RobotContainer.ARM_ELEVATOR.atState(REEF_CHOOSER.getArmElevatorState().prepareState) && RobotContainer.SWERVE.atPose(calculateClosestNoHitReefPose(shouldScoreRight))); | |
| private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRight) { | |
| final java.util.function.Supplier<FlippablePose2d> noHitPose = () -> calculateClosestNoHitReefPose(shouldScoreRight); | |
| final java.util.function.BooleanSupplier gate = | |
| () -> isPrepareArmAngleAboveCurrentArmAngle() | |
| || RobotContainer.SWERVE.atPose(noHitPose.get()) | |
| || shouldReverseScore(); | |
| return GeneralCommands.getContinuousConditionalCommand( | |
| new ParallelCommandGroup( | |
| ArmElevatorCommands.getPrepareForStateCommand( | |
| REEF_CHOOSER::getArmElevatorState, | |
| CoralPlacingCommands::shouldReverseScore | |
| ), | |
| getAutonomousDriveToReefThenManualDriveCommand(shouldScoreRight) | |
| .unless(() -> REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1) | |
| .asProxy() | |
| ), | |
| SwerveCommands.getDriveToPoseCommand( | |
| noHitPose, | |
| AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS | |
| ) | |
| .unless(CoralPlacingCommands::shouldReverseScore), | |
| gate | |
| ); | |
| } |
🤖 Prompt for AI Agents
In
src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java
around lines 60 to 69, remove the premature .until(...) on the inner
ParallelCommandGroup and drop the redundant .onlyIf(...) on its onTrue branch so
the prepare phase isn't terminated as soon as the no‑hit pose is reached and the
gating is handled by the caller; also compute
calculateClosestNoHitReefPose(shouldScoreRight) once into a local variable or
supplier and reuse that value in the ParallelCommandGroup, the
SwerveCommands.getDriveToPoseCommand call, and the outer until condition to
avoid inconsistent recomputations, leaving the overall termination to the
caller's .until(isReadyToScore(...)) logic.
This was referenced Sep 28, 2025
Merged
Merged
Merged
Merged
Open
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Summary by CodeRabbit
New Features
Refactor
Chores