Autonomous #19
Autonomous #19
Conversation
WalkthroughUpdated robot pose estimators to use three reef-tag cameras and TRANSLATION distance; rewrote auto chooser to populate mirrored PathPlannerAuto entries and a "None" option; added extensive autonomous command factories and scoring state; integrated IntakeAssist, added intake prepareForState + command; various constants, camera, object-detection, and logging changes. Changes
Sequence Diagram(s)sequenceDiagram
participant RC as RobotContainer
participant AB as AutoBuilder
participant PP as PathPlannerAuto
participant Chooser as SendableChooser
RC->>AB: getAllAutoNames()
AB-->>RC: autoNames
loop each autoName
RC->>PP: new PathPlannerAuto(autoName, mirrored=false)
RC->>Chooser: addOption(autoName, nonMirrored)
RC->>PP: new PathPlannerAuto(autoName, mirrored=true)
RC->>Chooser: addOption(autoName + " (mirrored)", mirrored)
end
alt DEFAULT_AUTO_NAME found
RC->>Chooser: setDefaultOption(matchingVariant)
else
RC->>Chooser: setDefaultOption("None")
end
Note right of RC: addCommandsToChooser(...) registers floor autos
sequenceDiagram
participant Driver as DriverStation
participant Auto as AutonomousCommands
participant Pose as CORAL_POSE_ESTIMATOR
participant Swerve as SwerveSubsystem
participant Intake as IntakeSubsystem
participant Arm as ArmElevator
Driver->>Auto: request auto (e.g., find/collect/score)
Auto->>Pose: query coral pose (TRANSLATION)
Pose-->>Auto: coral pose
Auto->>Swerve: driveTo(targetPose)
Auto->>Intake: run collect/unload sequences (includes IntakeAssist)
Auto->>Arm: prepare/score sequence (calculateClosestOpenScoringPose)
Auto->>Auto: update SCORED_L4S
Estimated code review effort🎯 4 (Complex) | ⏱️ ~60 minutes Possibly related PRs
Suggested reviewers
Poem
Pre-merge checks and finishing touches❌ Failed checks (2 warnings)
✅ Passed checks (1 passed)
✨ Finishing touches
🧪 Generate unit tests
📜 Recent review detailsConfiguration used: CodeRabbit UI Review profile: CHILL Plan: Pro 📒 Files selected for processing (1)
💤 Files with no reviewable 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: 6
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (3)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (1)
179-182: Fix: atAngle can never be true with 0-degree toleranceUsing strict
<with a 0° tolerance prevents convergence; use<=(and consider a small non‑zero tolerance).- return currentToTargetAngleDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); + return currentToTargetAngleDifferenceDegrees <= ArmConstants.ANGLE_TOLERANCE.getDegrees();src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
36-41: PID constants are all zero - needs tuning.Both
AUTO_TRANSLATION_PID_CONSTANTSandAUTO_ROTATION_PID_CONSTANTShave all gains set to zero for both simulation and real robot. This will result in no corrective action during path following.The PID controllers won't provide any correction with zero gains. These need to be tuned for proper autonomous operation:
private static final PIDConstants AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? - new PIDConstants(0, 0, 0) : - new PIDConstants(0, 0, 0), + new PIDConstants(0, 0, 0) : // TODO: Tune simulation PID values + new PIDConstants(0, 0, 0), // TODO: Tune real robot PID values AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? - new PIDConstants(0, 0, 0) : - new PIDConstants(0, 0, 0); + new PIDConstants(0, 0, 0) : // TODO: Tune simulation PID values + new PIDConstants(0, 0, 0); // TODO: Tune real robot PID valuessrc/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (1)
52-55: Fix SysId logging units — use meters (m) / meters per second (m/s).getPositionRotations() returns rotations; use getPositionMeters(). Convert VELOCITY (rot/s) to m/s with rotationsToMeters(...).
- .linearPosition(Units.Meters.of(getPositionRotations())) - .linearVelocity(Units.MetersPerSecond.of(masterMotor.getSignal(TalonFXSignal.VELOCITY))) + .linearPosition(Units.Meters.of(getPositionMeters())) + .linearVelocity(Units.MetersPerSecond.of(rotationsToMeters(masterMotor.getSignal(TalonFXSignal.VELOCITY))))Location: src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java:52-55
🧹 Nitpick comments (18)
src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java (2)
27-34: Public API typo: Calulation → Calculation (add alias, deprecate old).Fix the misspelling without breaking callers by deprecating the old method and adding a correctly named wrapper.
public static Command getGearRatioCalulationCommand() { + // existing implementation unchanged return new GearRatioCalculationCommand( ArmConstants.ARM_MASTER_MOTOR, ArmConstants.ANGLE_ENCODER, 0.5, RobotContainer.ARM ); } + + /** Prefer this correctly spelled API. */ + public static Command getGearRatioCalculationCommand() { + return getGearRatioCalulationCommand(); + }Optionally also deprecate the old name:
- public static Command getGearRatioCalulationCommand() { + /** @deprecated Use {@link #getGearRatioCalculationCommand()} */ + @Deprecated + public static Command getGearRatioCalulationCommand() {
41-48: FunctionalCommand never finishes; verify intent or refactor.
isFinishedalways returns false, so this runs until interrupted. If intentional, ignore. If not, add a finish predicate or useRunEndCommand.Example options:
- Add finish condition:
- () -> false, + () -> RobotContainer.ARM.atTargetState(targetState),
- Or replace with:
// new RunEndCommand(() -> RobotContainer.ARM.setArmState(targetState, isStateReversed), RobotContainer.ARM::stop, RobotContainer.ARM) // and an InstantCommand beforehand to setEndEffectorState(targetState) if neededsrc/main/java/frc/trigon/robot/subsystems/arm/Arm.java (1)
86-88: Add Javadoc + telemetry for operator feedbackDocument that the value is debounced (200 ms) and expose it to logs.
public class Arm extends MotorSubsystem { @@ - public boolean hasGamePiece() { + /** + * Returns true if the debounced collection sensor indicates a game piece is present. + * Debounced by ArmConstants.COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS. + */ + public boolean hasGamePiece() { return ArmConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); }Optional: log it each loop:
public void updatePeriodically() { armMasterMotor.update(); endEffectorMotor.update(); angleEncoder.update(); ArmConstants.DISTANCE_SENSOR.updateSensor(); Logger.recordOutput("Arm/CurrentPositionDegrees", getAngle().getDegrees()); + Logger.recordOutput("Arm/HasGamePiece", hasGamePiece()); }src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
120-121: Set a small non‑zero ANGLE_TOLERANCE to avoid oscillationA tiny deadband prevents chatter and numeric jitter.
- static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0); + static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0.5);src/main/java/frc/trigon/robot/RobotContainer.java (1)
117-151: Consider extracting magic string literals and improving error handling.While the auto chooser builder logic looks correct, consider:
- Extracting "Mirrored" suffix to a constant to avoid duplication
- Adding null checks for
autoNameto prevent potential NPE@SuppressWarnings("All") private void buildAutoChooser() { + final String MIRRORED_SUFFIX = "Mirrored"; autoChooser = new LoggedDashboardChooser<>("AutoChooser"); final List<String> autoNames = AutoBuilder.getAllAutoNames(); boolean hasDefault = false; for (String autoName : autoNames) { + if (autoName == null) continue; final PathPlannerAuto autoNonMirrored = new PathPlannerAuto(autoName); final PathPlannerAuto autoMirrored = new PathPlannerAuto(autoName, true); if (!AutonomousConstants.DEFAULT_AUTO_NAME.isEmpty() && AutonomousConstants.DEFAULT_AUTO_NAME.equals(autoName)) { hasDefault = true; autoChooser.addDefaultOption(autoNonMirrored.getName(), autoNonMirrored); - autoChooser.addOption(autoMirrored.getName() + "Mirrored", autoMirrored); - } else if (!AutonomousConstants.DEFAULT_AUTO_NAME.isEmpty() && AutonomousConstants.DEFAULT_AUTO_NAME.equals(autoName + "Mirrored")) { + autoChooser.addOption(autoMirrored.getName() + MIRRORED_SUFFIX, autoMirrored); + } else if (!AutonomousConstants.DEFAULT_AUTO_NAME.isEmpty() && AutonomousConstants.DEFAULT_AUTO_NAME.equals(autoName + MIRRORED_SUFFIX)) { hasDefault = true; - autoChooser.addDefaultOption(autoMirrored.getName() + "Mirrored", autoMirrored); + autoChooser.addDefaultOption(autoMirrored.getName() + MIRRORED_SUFFIX, autoMirrored); autoChooser.addOption(autoNonMirrored.getName(), autoNonMirrored); } else { autoChooser.addOption(autoNonMirrored.getName(), autoNonMirrored); - autoChooser.addOption(autoMirrored.getName() + "Mirrored", autoMirrored); + autoChooser.addOption(autoMirrored.getName() + MIRRORED_SUFFIX, autoMirrored); } }src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (2)
30-30: Address the TODO comment for feedforward calibration.The
FEEDFORWARD_SCALARis marked with a TODO for calibration. This is a critical parameter for autonomous path following accuracy.Would you like me to create an issue to track the feedforward scalar calibration task?
88-90: Implement the registerCommands TODO.The
registerCommands()method is empty with a TODO comment. This should register named commands for PathPlanner to use.Would you like me to help implement the command registration based on the autonomous commands defined in
AutonomousCommandsclass?src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java (1)
64-64: Address the TODO comment about intake state checking.The condition currently only checks distance but needs to also verify if the intake is open before attempting to collect game pieces.
Would you like me to implement the intake open check or create an issue to track this task?
src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (1)
78-78: Verify the typo in method name.The method name contains what appears to be a typo:
calculateDistanceFromTrackedCGamePiecehas an extra 'C' before 'GamePiece'.- public static Translation2d calculateDistanceFromTrackedCGamePiece() { + public static Translation2d calculateDistanceFromTrackedGamePiece() {Also update the call site on line 74:
- distanceFromTrackedGamePiece = calculateDistanceFromTrackedCGamePiece(); + distanceFromTrackedGamePiece = calculateDistanceFromTrackedGamePiece();src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (7)
95-96: Typo in method name: should be calculateDistanceFromTrackedGamePieceThe method name has a typo:
calculateDistanceFromTrackedCGamePieceshould becalculateDistanceFromTrackedGamePiece.- IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedCGamePiece).withTimeout(1.5) + IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece).withTimeout(1.5)
50-56: Magic number for end velocity should be a constantThe hard-coded value
2.3for end velocity on line 50 should be extracted to a constant for better maintainability.+private static final double FIND_CORAL_END_VELOCITY = 2.3; + public static Command getFindCoralCommand(boolean isRight) { return new SequentialCommandGroup( - SwerveCommands.getDriveToPoseCommand(() -> isRight ? FieldConstants.AUTO_FIND_CORAL_POSE_RIGHT : FieldConstants.AUTO_FIND_CORAL_POSE_LEFT, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS, 2.3), + SwerveCommands.getDriveToPoseCommand(() -> isRight ? FieldConstants.AUTO_FIND_CORAL_POSE_RIGHT : FieldConstants.AUTO_FIND_CORAL_POSE_LEFT, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS, FIND_CORAL_END_VELOCITY),
51-55: Magic number for rotation power should be a constantThe hard-coded value
0.2for rotation power on line 54 should be extracted to a constant.+private static final double CORAL_SEARCH_ROTATION_POWER = 0.2; + SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> 0, () -> 0, - () -> 0.2 + () -> CORAL_SEARCH_ROTATION_POWER )
96-96: Magic number for timeout should be a constantThe hard-coded timeout value
1.5on line 96 should be extracted to a constant for better maintainability.+private static final double ASSIST_INTAKE_TIMEOUT_SECONDS = 1.5; + - IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedCGamePiece).withTimeout(1.5) + IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece).withTimeout(ASSIST_INTAKE_TIMEOUT_SECONDS)
127-127: Magic number for timeout should be a constantThe hard-coded timeout value
0.25on line 127 should be extracted to a constant.+private static final double PLACE_CORAL_TIMEOUT_SECONDS = 0.25; + - ).withTimeout(0.25); + ).withTimeout(PLACE_CORAL_TIMEOUT_SECONDS);
34-34: Consider adding documentation for SCORED_L4S arrayThe public field
SCORED_L4Slacks documentation explaining its purpose and the meaning of array indices.+/** + * Array tracking which L4 branches have been scored on. + * Index represents branch position (clockPosition * 2 + side). + */ public static final LoggedNetworkBoolean[] SCORED_L4S = getEmptyL4LoggedNetworkBooleanArray();
87-87: TODO comment indicates missing implementationLine 87 has a TODO comment for loading coral command that needs to be implemented.
Would you like me to help implement the
CoralPlacementCommands.getLoadCoralCommand()or create an issue to track this missing functionality?src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java (1)
105-105: Make tolerance a true constant (or document mutability).
POSITION_TOLERANCE_METERSis package‑private and mutable. If runtime tuning isn’t intended, mark itfinal(and optionallypublicif other packages need it).- static double POSITION_TOLERANCE_METERS = 0.02; + static final double POSITION_TOLERANCE_METERS = 0.02;src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (1)
92-94: Include boundary in tolerance; consider call‑site control/debouncing.Use
<=to avoid edge misses at exactly the tolerance. Optional: add an overload that accepts a custom tolerance or debounce the result to prevent chatter.- return Math.abs(getPositionMeters() - targetState.targetPositionMeters) < ElevatorConstants.POSITION_TOLERANCE_METERS; + return Math.abs(getPositionMeters() - targetState.targetPositionMeters) <= ElevatorConstants.POSITION_TOLERANCE_METERS;
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (17)
src/main/java/frc/trigon/robot/RobotContainer.java(3 hunks)src/main/java/frc/trigon/robot/commands/CommandConstants.java(2 hunks)src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java(2 hunks)src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java(4 hunks)src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java(1 hunks)src/main/java/frc/trigon/robot/constants/AutonomousConstants.java(1 hunks)src/main/java/frc/trigon/robot/constants/FieldConstants.java(2 hunks)src/main/java/frc/trigon/robot/subsystems/arm/Arm.java(1 hunks)src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java(1 hunks)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java(1 hunks)src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java(1 hunks)src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java(2 hunks)src/main/java/frc/trigon/robot/subsystems/intake/Intake.java(1 hunks)src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java(1 hunks)src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java(3 hunks)src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java(3 hunks)src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java(2 hunks)
🧰 Additional context used
🧬 Code graph analysis (13)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (2)
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-262)src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java (2)
isHoldingGamePiece(24-26)SimulationFieldHandler(16-161)
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (1)
src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java (1)
ElevatorConstants(28-199)
src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java (1)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(46-157)
src/main/java/frc/trigon/robot/constants/FieldConstants.java (1)
src/main/java/lib/utilities/flippable/FlippablePose2d.java (1)
FlippablePose2d(11-58)
src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java (2)
src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(25-91)src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java (1)
SwerveConstants(19-110)
src/main/java/frc/trigon/robot/commands/CommandConstants.java (1)
src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(25-91)
src/main/java/frc/trigon/robot/RobotContainer.java (2)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (1)
AutonomousCommands(33-237)src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(25-91)
src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java (5)
src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(25-91)src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java (1)
ObjectPoseEstimator(15-200)src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java (1)
SwerveCommands(15-163)src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(46-157)src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java (1)
GeneralCommands(15-65)
src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (2)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(46-157)src/main/java/lib/hardware/RobotHardwareStats.java (1)
RobotHardwareStats(6-78)
src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java (1)
src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(25-91)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (7)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(46-157)src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (1)
IntakeAssistCommand(22-190)src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(25-91)src/main/java/frc/trigon/robot/constants/FieldConstants.java (1)
FieldConstants(18-57)src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java (1)
SwerveCommands(15-163)src/main/java/lib/utilities/flippable/FlippablePose2d.java (1)
FlippablePose2d(11-58)src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java (1)
GeneralCommands(15-65)
src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java (1)
src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(25-91)
src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (1)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(46-157)
🔇 Additional comments (22)
src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java (1)
36-36: Formatting-only change looks good.No functional impact.
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (1)
86-88: LGTM: concise, debounced hasGamePiece() accessorGood use of the debounced BooleanEvent; exposes a clear API.
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (4)
243-250: LGTM: PREPARE_ state rename and values*Renames/readability improvements look good; voltages/angles remain consistent.
121-126: Confirm static BooleanEvent initialization timingUsing CommandScheduler.getInstance().getActiveButtonLoop() in a static initializer is usually fine; verify no early class-load issues. If any, move creation into Arm’s ctor.
230-233: Verify sensor polarity in sim vs. hardwareSim supplier returns 0 when holding; ensure SimpleSensor.getBinaryValue treats this as “true” on both sim and real hardware.
243-250: PREPARE_ rename verified — no remaining old usages*rg found zero matches for PREPARE_(SCORE_L[1-4]|NET_SCORE|PROCESSOR_SCORE); code uses PREPARE_L[1-4], PREPARE_NET, PREPARE_PROCESSOR.
src/main/java/frc/trigon/robot/constants/FieldConstants.java (1)
31-37: LGTM! Well-structured flippable pose constants for autonomous coral finding.The implementation correctly uses
FlippablePose2dto handle alliance-specific positioning and properly calculates the mirrored right pose from the left pose parameters.src/main/java/frc/trigon/robot/commands/CommandConstants.java (1)
10-10: Reference update aligns with project-wide refactoring.The change from
PathPlannerConstantstoAutonomousConstantsis consistent with the broader refactoring across the codebase.Also applies to: 48-48
src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
26-33: Well-structured autonomous constants for reef navigation.The new path constraints and tolerance values are appropriately defined for autonomous reef operations. The angular velocity limits (450°/s max, 900°/s² acceleration) and positional tolerances (8.5cm X, 3cm Y) appear reasonable for FRC robot control.
src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java (1)
71-71: Reference update consistent with project refactoring.The change to use
AutonomousConstants.ROBOT_CONFIGaligns with the project-wide renaming fromPathPlannerConstants.src/main/java/frc/trigon/robot/RobotContainer.java (1)
48-53: Camera parameter addition is valid — no action required.
CameraConstants.OBJECT_DETECTION_CAMERA is an ObjectDetectionCamera (src/main/java/frc/trigon/robot/constants/CameraConstants.java:16–19), ObjectPoseEstimator’s constructor accepts an ObjectDetectionCamera (src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java:29–34), and RobotContainer passes the constant correctly.src/main/java/frc/trigon/robot/subsystems/intake/Intake.java (1)
92-99: Well-structured state preparation method.The
prepareForStatemethod provides a clean way to prepare the intake for a target state without applying voltage, which is useful for autonomous positioning before game piece collection.src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java (1)
22-28: LGTM! Clean implementation of prepare-for-state command.The command factory follows the established pattern and properly manages subsystem requirements with appropriate lifecycle callbacks. The StartEndCommand ensures the intake stops when interrupted, maintaining safety.
src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java (2)
18-18: Consider making CORAL_POSE_ESTIMATOR field final.The static field should be marked as final to prevent accidental reassignment and clearly indicate it's a constant reference.
- private static final ObjectPoseEstimator CORAL_POSE_ESTIMATOR = RobotContainer.CORAL_POSE_ESTIMATOR; + private static final ObjectPoseEstimator CORAL_POSE_ESTIMATOR = RobotContainer.CORAL_POSE_ESTIMATOR;Wait, the field is already marked as final. This suggestion is not needed.
21-21: Updated constructor signature aligns with autonomous system refactor.The simplified constructor no longer requires a game piece type parameter, indicating that the system now tracks a single type of game piece (coral). This is consistent with the broader PR changes.
src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java (1)
16-16: LGTM! Clean migration to AutonomousConstants.The migration from PathPlannerConstants to AutonomousConstants is consistently applied across all usage points. The changes maintain the same functionality while aligning with the new autonomous system architecture.
Also applies to: 35-35, 38-38, 160-160, 194-194
src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java (1)
11-11: Constants migration completed successfully.The replacement of PathPlannerConstants with AutonomousConstants is properly implemented for all derived constants. The ROBOT_CONFIG reference now consistently points to the centralized configuration.
Also applies to: 47-47, 58-59
src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (2)
23-32: PID controller visibility and initialization look good.The PID controllers are now package-private (removing the private modifier) which allows for better testability while maintaining encapsulation. The addition of Y and THETA controllers provides complete 2D tracking capability.
46-46: CORAL_POSE_ESTIMATOR integration properly implemented.The migration from OBJECT_POSE_ESTIMATOR to CORAL_POSE_ESTIMATOR is consistently applied throughout the command. The null checks ensure safe operation when no coral pieces are detected.
Also applies to: 73-74, 80-80
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (2)
189-194: Good practice using LoggedNetworkBoolean arrayExcellent implementation for creating the logged network boolean array with proper initialization and descriptive names.
211-219: Good safety check for pose resetThe command properly checks if the robot is enabled before resetting the pose, preventing accidental pose resets during matches.
src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java (1)
185-187: Rename verified — no remaining references to COLLECT_ALGAE_FROM_ in repo.*
No occurrences of COLLECT_ALGAE_FROM_L2/L3/GROUND were found; new enum values are defined at src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java:185–187 and referenced at src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java:251–252.
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
Show resolved
Hide resolved
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
Show resolved
Hide resolved
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
Show resolved
Hide resolved
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
Show resolved
Hide resolved
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
Outdated
Show resolved
Hide resolved
There was a problem hiding this comment.
Actionable comments posted: 1
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (1)
src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (1)
61-69: Guard against null supplier to prevent NPEsWhen used from getDriveToCoralCommand, calculateDistanceFromTrackedGamePiece() can return null (camera dropouts), causing NPEs in resetPIDControllers and downstream suppliers.
Apply this defensive wrapper:
- public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier<Translation2d> distanceFromTrackedGamePiece) { - return new SequentialCommandGroup( - new InstantCommand(() -> resetPIDControllers(distanceFromTrackedGamePiece.get())), - SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> calculateTranslationPower(assistMode, distanceFromTrackedGamePiece.get()).getX(), - () -> calculateTranslationPower(assistMode, distanceFromTrackedGamePiece.get()).getY(), - () -> calculateThetaPower(assistMode, distanceFromTrackedGamePiece.get()) - ) - ); - } + public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier<Translation2d> distanceFromTrackedGamePiece) { + final Supplier<Boolean> hasTarget = () -> distanceFromTrackedGamePiece.get() != null; + return new ConditionalCommand( + new SequentialCommandGroup( + new InstantCommand(() -> { + final Translation2d d = distanceFromTrackedGamePiece.get(); + if (d != null) resetPIDControllers(d); + }), + SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + () -> { + final Translation2d d = distanceFromTrackedGamePiece.get(); + return d == null ? 0 : calculateTranslationPower(assistMode, d).getX(); + }, + () -> { + final Translation2d d = distanceFromTrackedGamePiece.get(); + return d == null ? 0 : calculateTranslationPower(assistMode, d).getY(); + }, + () -> { + final Translation2d d = distanceFromTrackedGamePiece.get(); + return d == null ? 0 : calculateThetaPower(assistMode, d); + } + ) + ), + Commands.none(), + hasTarget + ); + }
🧹 Nitpick comments (4)
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
243-250: Prep-state renames/mappings look consistent; minor dedupeValues are consistent, but PREPARE_* repeatedly uses 95°. Consider a shared constant to reduce drift during tuning.
+ private static final Rotation2d PREPARE_DEFAULT_ANGLE = Rotation2d.fromDegrees(95); ... - PREPARE_L2(Rotation2d.fromDegrees(95), 0), - PREPARE_L3(Rotation2d.fromDegrees(95), 0), - PREPARE_L4(Rotation2d.fromDegrees(95), 0), + PREPARE_L2(PREPARE_DEFAULT_ANGLE, 0), + PREPARE_L3(PREPARE_DEFAULT_ANGLE, 0), + PREPARE_L4(PREPARE_DEFAULT_ANGLE, 0),src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (2)
23-33: PID exposure and duplicationExposing three ProfiledPIDController instances as package‑private is fine, but X/Y/Theta gains duplicate values defined elsewhere (e.g., AutonomousConstants.*). Consider centralizing to avoid divergence between manual-assist and auto flows.
78-88: Public helper is fine — callers already guard; add Javadoc for null semanticscalculateDistanceFromTrackedGamePiece() is only invoked after RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null (IntakeAssistCommand ~lines 72–74), so the null return is already handled as "no-assist". Add a short Javadoc to the method stating it may return null when no tracked object is present.
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (1)
92-97: Assist handoff can still see target loss mid‑sequenceEven after find, vision dropouts can occur. The null‑guards added in IntakeAssistCommand will mitigate; consider an .until(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null) to exit early on loss.
- ).repeatedly(); + ).until(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null).repeatedly();
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (7)
src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java(0 hunks)src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java(4 hunks)src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java(1 hunks)src/main/java/frc/trigon/robot/constants/AutonomousConstants.java(1 hunks)src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java(2 hunks)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java(2 hunks)src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java(2 hunks)
💤 Files with no reviewable changes (1)
- src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java
🚧 Files skipped from review as they are similar to previous changes (1)
- src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java
🧰 Additional context used
🧠 Learnings (1)
📚 Learning: 2025-09-18T22:46:49.248Z
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.248Z
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/ArmConstants.java
🧬 Code graph analysis (3)
src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (5)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(46-157)src/main/java/lib/hardware/RobotHardwareStats.java (1)
RobotHardwareStats(6-78)src/main/java/lib/utilities/flippable/Flippable.java (1)
Flippable(19-87)src/main/java/lib/utilities/flippable/FlippablePose2d.java (1)
FlippablePose2d(11-58)src/main/java/frc/trigon/robot/constants/FieldConstants.java (1)
FieldConstants(16-47)
src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (1)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(46-157)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (5)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(46-157)src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (1)
IntakeAssistCommand(22-190)src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(27-105)src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java (1)
SwerveCommands(15-163)src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java (1)
GeneralCommands(15-65)
🔇 Additional comments (13)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (9)
84-90: Wait for target: update condition with AtomicReferenceIf adopting AtomicReference, align this check.
- new WaitUntilCommand(() -> TARGET_SCORING_POSE != null), + new WaitUntilCommand(() -> TARGET_SCORING_POSE.get() != null),
188-202: SCORED_L4S helpers: OKArray init/log keys and snapshot accessor look good.
33-35: Shared TARGET_SCORING_POSE: make thread‑safe and eliminate racy readsMultiple commands write/read TARGET_SCORING_POSE concurrently. Replace with AtomicReference and update usages to avoid races and NPEs.
+import java.util.concurrent.atomic.AtomicReference; ... - private static FlippablePose2d TARGET_SCORING_POSE = null; + private static final AtomicReference<FlippablePose2d> TARGET_SCORING_POSE = new AtomicReference<>(null);
113-119: canScore(): race to null and duplicate pose fetchesCache target and robot pose once to avoid TOCTOU and redundant calls.
- private static boolean canScore() { - return RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.PREPARE_L4) && - RobotContainer.ARM.atState(ArmConstants.ArmState.PREPARE_L4) && - TARGET_SCORING_POSE != null && - Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getX()) < AutonomousConstants.REEF_RELATIVE_X_TOLERANCE_METERS && - Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getY()) < AutonomousConstants.REEF_RELATIVE_Y_TOLERANCE_METERS; - } + private static boolean canScore() { + final FlippablePose2d target = TARGET_SCORING_POSE.get(); + if (target == null) return false; + final Pose2d robot = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Pose2d rel = robot.relativeTo(target.get()); + return RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.PREPARE_L4) && + RobotContainer.ARM.atState(ArmConstants.ArmState.PREPARE_L4) && + Math.abs(rel.getX()) < AutonomousConstants.REEF_RELATIVE_X_TOLERANCE_METERS && + Math.abs(rel.getY()) < AutonomousConstants.REEF_RELATIVE_Y_TOLERANCE_METERS; + }
167-174: Null safety and bounds when marking scored branchesTARGET_SCORING_POSE may be null or unresolved to a valid branch. Guard and validate index.
private static Command getAddCurrentScoringBranchToScoredBranchesCommand() { return new InstantCommand( () -> { - final int branchNumber = getBranchNumberFromScoringPose(TARGET_SCORING_POSE.get()); - SCORED_L4S[branchNumber].set(true); + final FlippablePose2d target = TARGET_SCORING_POSE.get(); + if (target == null) return; + final int branchNumber = getBranchNumberFromScoringPose(target.get()); + if (branchNumber >= 0 && branchNumber < SCORED_L4S.length) + SCORED_L4S[branchNumber].set(true); } ); }
176-186: Branch resolution always returns 0Implement the real mapping to avoid corrupting SCORED_L4S state.
- private static int getBranchNumberFromScoringPose(Pose2d scoringPose) { //TODO: this stuff - return 0; - } + private static int getBranchNumberFromScoringPose(Pose2d scoringPose) { + // Pseudocode using your existing enums/utilities; adjust imports as needed. + for (var clock : FieldConstants.ReefClockPosition.values()) { + for (var side : FieldConstants.ReefSide.values()) { + final Pose2d candidate = CoralPlacingCommands.ScoringLevel.L4 + .calculateTargetPlacingPosition(clock, side).get(); + if (candidate.getTranslation().getDistance(scoringPose.getTranslation()) < 0.01) + return clock.ordinal() * 2 + side.ordinal(); + } + } + return -1; + }
76-82: Blocking flow due to unimplemented target pose + NPE riskcalculateClosestOpenScoringPose() currently returns null, so:
- WaitUntil(TARGET_SCORING_POSE …) may never complete
- Subsequent driveToPose dereferences null
Apply both: use AtomicReference and guard the flow.
- new InstantCommand(() -> TARGET_SCORING_POSE = calculateClosestOpenScoringPose()), - new WaitUntilCommand(() -> TARGET_SCORING_POSE != null).raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), - SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly() + new InstantCommand(() -> TARGET_SCORING_POSE.set(calculateClosestOpenScoringPose())), + new WaitUntilCommand(() -> TARGET_SCORING_POSE.get() != null) + .raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), + SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE.get(), AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS)Until calculateClosestOpenScoringPose is implemented, consider falling back to a safe fixed FlippablePose2d to keep auton testable.
129-134: Open‑elevator trigger can NPE when target is unsetGuard the distance computation; otherwise vision/selection delays can crash this branch.
- () -> calculateDistanceToTargetScoringPose() < AutonomousConstants.MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR + () -> calculateDistanceToTargetScoringPose() < AutonomousConstants.MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATORAnd implement the null‑safe distance below.
136-141: Null‑safe distance to targetAvoid NPE if target is not yet selected.
- private static double calculateDistanceToTargetScoringPose() { - final Translation2d currentTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); - final Translation2d targetTranslation = TARGET_SCORING_POSE.get().getTranslation(); - return currentTranslation.getDistance(targetTranslation); - } + private static double calculateDistanceToTargetScoringPose() { + final FlippablePose2d target = TARGET_SCORING_POSE.get(); + if (target == null) return Double.POSITIVE_INFINITY; + final Translation2d current = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); + return current.getDistance(target.get().getTranslation()); + }src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
28-48: Make DEFAULT_AUTO_NAME empty; validate constraints & mirrored posesRobotContainer guards DEFAULT_AUTO_NAME with isEmpty() and equality checks (src/main/java/frc/trigon/robot/RobotContainer.java lines ~128/132) — setting it to "" will disable accidental default selection.
- public static final String DEFAULT_AUTO_NAME = "DefaultAutoName"; + public static final String DEFAULT_AUTO_NAME = "";
- DRIVE_TO_REEF_CONSTRAINTS (2.5 m/s, 4 m/s², 450°/s, 900°/s²): reasonable for early bring‑up — verify on‑robot with low‑speed tests and/or add software limits if uncertain.
- AUTO_FIND_CORAL poses: confirm AUTO_FIND_CORAL_POSE_RIGHT correctly mirrors LEFT given FlippablePose2d semantics and FieldConstants.FIELD_WIDTH_METERS.
src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java (2)
105-105: Good: explicit elevator position tolerance — verifiedPOSITION_TOLERANCE_METERS = 0.02 is declared at src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java:105 and is used directly in Elevator.atState (src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java:92–93). AutonomousCommands.canScore() calls RobotContainer.ELEVATOR.atState(...). No other references to POSITION_TOLERANCE_METERS were found.
185-187: Enum rename verified — no stale COLLECT_ALGAE_FROM_ usages remain*
Search found only updated enum names in src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java:185–187 and src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java:251–252.src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
120-120: Approve: tighten Arm ANGLE_TOLERANCE to 0.5°Verified: ArmConstants.ANGLE_TOLERANCE (0.5°) is only used in Arm.atAngle (src/main/java/frc/trigon/robot/subsystems/arm/Arm.java); intake uses its own IntakeConstants.ANGLE_TOLERANCE = 1.5° (src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java). Change is isolated and acceptable given the existing >90° safe-angle bypass.
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
Show resolved
Hide resolved
There was a problem hiding this comment.
Actionable comments posted: 2
🧹 Nitpick comments (3)
src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java (1)
106-106: Avoid two different elevator position tolerances (0.01 vs 0.02) unless intentionalYou now have HEIGHT_TOLERANCE_METERS = 0.01 and POSITION_TOLERANCE_METERS = 0.02 used in different readiness checks. This inconsistency can cause mismatched “ready” decisions between methods. Either unify them or document the intentional difference.
Option A — single source of truth (alias to existing height tolerance):
- static final double POSITION_TOLERANCE_METERS = 0.02; + // Keep a single tolerance for elevator position checks to avoid drift + static final double POSITION_TOLERANCE_METERS = HEIGHT_TOLERANCE_METERS;Option B — keep distinct values but add Javadoc explaining when each should be used (no diff shown).
src/main/java/frc/trigon/robot/RobotContainer.java (2)
116-116: Avoid @SuppressWarnings("All"). Narrow or remove.Suppressing all warnings hides real issues. Prefer removing or scoping to the specific warning.
Apply:
- @SuppressWarnings("All") + // Consider narrowing specific warnings if needed instead of suppressing all
118-139: PathPlannerAuto(String, boolean) usage confirmed; polish mirrored option labels.
- Verified: PathPlannerAuto(String autoName, boolean mirror) exists and the boolean flips/mirrors the loaded paths (usage is correct).
- Location: src/main/java/frc/trigon/robot/RobotContainer.java (lines 118–139).
- UX: change concatenated "Mirrored" to " (Mirrored)" for readability.
- autoChooser.addOption(autoMirrored.getName() + "Mirrored", autoMirrored); + autoChooser.addOption(autoMirrored.getName() + " (Mirrored)", autoMirrored); ... - autoChooser.addDefaultOption(autoMirrored.getName() + "Mirrored", autoMirrored); + autoChooser.addDefaultOption(autoMirrored.getName() + " (Mirrored)", autoMirrored); ... - autoChooser.addOption(autoMirrored.getName() + "Mirrored", autoMirrored); + autoChooser.addOption(autoMirrored.getName() + " (Mirrored)", autoMirrored);
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (5)
src/main/java/frc/trigon/robot/RobotContainer.java(4 hunks)src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java(2 hunks)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java(1 hunks)src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java(1 hunks)src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java(1 hunks)
🚧 Files skipped from review as they are similar to previous changes (1)
- src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java
🧰 Additional context used
🧠 Learnings (1)
📚 Learning: 2025-09-18T22:46:49.248Z
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.248Z
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/ArmConstants.java
🧬 Code graph analysis (2)
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (1)
src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java (1)
ElevatorConstants(28-198)
src/main/java/frc/trigon/robot/RobotContainer.java (3)
src/main/java/frc/trigon/robot/commands/CommandConstants.java (1)
CommandConstants(23-122)src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (1)
AutonomousCommands(32-236)src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(27-105)
🪛 GitHub Actions: Java CI with Gradle
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java
[error] 106-106: method atState(ElevatorState) is already defined in class Elevator
🔇 Additional comments (6)
src/main/java/frc/trigon/robot/RobotContainer.java (6)
9-9: Imports look correct and scoped.No issues spotted with the new imports; they align with the new autonomous wiring.
Also applies to: 14-14, 17-17, 43-43
141-145: Sane default handling.Falling back to "None" when no default match is found is a safe behavior.
152-154: Helper method for chooser entries is clean.Varargs utility keeps RobotContainer tidy.
146-150: Approve — explicit chooser names confirmedAutonomousCommands.getFloorAutonomousCommand(boolean) sets an explicit name via .withName("FloorAutonomous" + (isRight ? "Right" : "Left")) — src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java:36–38.
47-52: CORAL_POSE_ESTIMATOR rename + constructor arg — verifiedAll references updated: static import in src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java and qualified uses in src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java; no remaining OBJECT_POSE_ESTIMATOR occurrences found.
97-97: Approve: AutonomousConstants.init() used; PathPlannerConstants init removed.
Confirmed no occurrences of PathPlannerConstants in the repo; AutonomousConstants.init() is called at src/main/java/frc/trigon/robot/RobotContainer.java:97.
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java
Outdated
Show resolved
Hide resolved
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java
Outdated
Show resolved
Hide resolved
There was a problem hiding this comment.
Actionable comments posted: 2
🧹 Nitpick comments (2)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (1)
91-95: Unify reversal logic locally; avoid delegating to a 2‑arg overload with different semanticsCurrent code branches to the 2‑arg overload (which ignores the class field) when
isStateReversedis false. Compute the desired angle once here to keep behavior explicit and consistent.- public boolean atState(ArmConstants.ArmState targetState, boolean atPrepareState, boolean isStateReversed) { - if (isStateReversed) - return this.targetState == targetState && atAngle(subtractFrom360Degrees(atPrepareState ? targetState.prepareAngle : targetState.targetAngle)); - return atState(targetState, atPrepareState); - } + public boolean atState(ArmConstants.ArmState targetState, boolean atPrepareState, boolean isStateReversed) { + Rotation2d desired = atPrepareState ? targetState.prepareAngle : targetState.targetAngle; + if (isStateReversed) { + desired = subtractFrom360Degrees(desired); + } + return this.targetState == targetState && atAngle(desired); + }Please confirm callers rely on the 3‑arg form whenever reversal is intentional, and that no behavior change is expected by this unification.
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (1)
130-135: Guard distance computation on missing target pose (or rely on null-safe helper)If TARGET_SCORING_POSE is null, the condition will NPE via calculateDistanceToTargetScoringPose(). Either add a null check here or make the helper null-safe (see next comment).
- () -> calculateDistanceToTargetScoringPose() < AutonomousConstants.MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR + () -> { + final FlippablePose2d target = TARGET_SCORING_POSE.get(); + return target != null && calculateDistanceToTargetScoringPose() < AutonomousConstants.MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR; + }
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (4)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java(1 hunks)src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java(2 hunks)src/main/java/frc/trigon/robot/subsystems/arm/Arm.java(1 hunks)src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java(1 hunks)
🚧 Files skipped from review as they are similar to previous changes (1)
- src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java
🧰 Additional context used
🧬 Code graph analysis (1)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (11)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-156)src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (1)
IntakeAssistCommand(22-190)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-115)src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java (1)
ArmCommands(15-89)src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java (1)
ElevatorCommands(11-59)src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java (1)
IntakeCommands(11-45)src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java (1)
SwerveCommands(15-163)src/main/java/frc/trigon/robot/subsystems/transporter/TransporterCommands.java (1)
TransporterCommands(10-36)src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java (1)
GeneralCommands(15-65)src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java (1)
CoralPlacingCommands(24-203)
🔇 Additional comments (16)
src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java (2)
38-44: Rename to readiness gate is clear; logic unchangedSwitch to isReadyToScore() reads better and matches intent. LGTM.
109-113: Readiness check looks correctElevator prepared, arm at prepare angle, and pose tolerance via SWERVE.atPose(). LGTM.
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (14)
85-91: Wait on atomic pose before scoring sequenceUse .get() with AtomicReference for consistency.
- new WaitUntilCommand(() -> TARGET_SCORING_POSE != null), + new WaitUntilCommand(() -> TARGET_SCORING_POSE.get() != null),
143-163: Closest-open scoring pose search looks goodIterates all branches, skips scored, returns closest. LGTM.
Please confirm SCORED_L4S length is always 12 (6 clock positions × 2 sides) in FieldConstants; otherwise guard the index calculation.
59-64: ParallelRaceGroup flow LGTMDrive and scoring sequence race is appropriate.
66-75: Collect-coral gating is sensible.until(hasCoral) and .unless(hasCoral || hasGamePiece) reads well. LGTM.
100-105: Score command pipeline LGTMPrepare-until-canScore then place. Nice.
186-201: Network array init/getter LGTMLoggedNetworkBoolean array creation and snapshot reader are fine.
208-216: Reset pose while disabled LGTMSafety gate on DriverStation.isEnabled() is good.
224-233: Auto start pose retrieval LGTMFlipping start pose per alliance is correct. Handles IO/parse gracefully.
34-36: Shared mutable TARGET_SCORING_POSE can race; switch to AtomicReferenceMultiple commands read/write TARGET_SCORING_POSE concurrently; use AtomicReference and adjust call sites.
Apply:
@@ -import lib.utilities.flippable.FlippablePose2d; +import lib.utilities.flippable.FlippablePose2d; +import java.util.concurrent.atomic.AtomicReference; @@ - public static final LoggedNetworkBoolean[] SCORED_L4S = getEmptyL4LoggedNetworkBooleanArray(); - private static FlippablePose2d TARGET_SCORING_POSE = null; + public static final LoggedNetworkBoolean[] SCORED_L4S = getEmptyL4LoggedNetworkBooleanArray(); + private static final AtomicReference<FlippablePose2d> TARGET_SCORING_POSE = new AtomicReference<>(null);Follow‑up diffs for usages are provided in separate comments below.
77-83: Drive-to-reef uses non-atomic TARGET_SCORING_POSE; update to .set/.getPrevents TOCTOU/NPE and aligns with AtomicReference.
- return new SequentialCommandGroup( - new InstantCommand(() -> TARGET_SCORING_POSE = calculateClosestOpenScoringPose()), - new WaitUntilCommand(() -> TARGET_SCORING_POSE != null).raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), - SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly() - ); + return new SequentialCommandGroup( + new InstantCommand(() -> TARGET_SCORING_POSE.set(calculateClosestOpenScoringPose())), + new WaitUntilCommand(() -> TARGET_SCORING_POSE.get() != null) + .raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), + SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE.get(), AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly() + );
114-120: canScore() has TOCTOU on TARGET_SCORING_POSE and does duplicate workCache the pose once, short‑circuit null, and reuse the current pose.
- private static boolean canScore() { - return RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.SCORE_L4, true) && - RobotContainer.ARM.atState(ArmConstants.ArmState.SCORE_L4, true) && - TARGET_SCORING_POSE != null && - Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getX()) < AutonomousConstants.REEF_RELATIVE_X_TOLERANCE_METERS && - Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getY()) < AutonomousConstants.REEF_RELATIVE_Y_TOLERANCE_METERS; - } + private static boolean canScore() { + if (!RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.SCORE_L4, true)) return false; + if (!RobotContainer.ARM.atState(ArmConstants.ArmState.SCORE_L4, true)) return false; + final FlippablePose2d target = TARGET_SCORING_POSE.get(); + if (target == null) return false; + final Pose2d current = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Pose2d relative = current.relativeTo(target.get()); + return Math.abs(relative.getX()) < AutonomousConstants.REEF_RELATIVE_X_TOLERANCE_METERS + && Math.abs(relative.getY()) < AutonomousConstants.REEF_RELATIVE_Y_TOLERANCE_METERS; + }
137-141: Null dereference on TARGET_SCORING_POSE.get()Return +∞ when target is unknown to keep logic safe.
private static double calculateDistanceToTargetScoringPose() { - final Translation2d currentTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); - final Translation2d targetTranslation = TARGET_SCORING_POSE.get().getTranslation(); - return currentTranslation.getDistance(targetTranslation); + final FlippablePose2d target = TARGET_SCORING_POSE.get(); + if (target == null) return Double.POSITIVE_INFINITY; + final Translation2d current = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); + return current.getDistance(target.get().getTranslation()); }
165-171: NPE and silent mis-index when branch unknownGuard TARGET_SCORING_POSE and handle “not found” from getBranchNumberFromScoringPose().
private static Command getAddCurrentScoringBranchToScoredBranchesCommand() { return new InstantCommand( () -> { - final int branchNumber = getBranchNumberFromScoringPose(TARGET_SCORING_POSE.get()); - SCORED_L4S[branchNumber].set(true); + final FlippablePose2d target = TARGET_SCORING_POSE.get(); + if (target == null) return; + final int branchNumber = getBranchNumberFromScoringPose(target.get()); + if (branchNumber >= 0 && branchNumber < SCORED_L4S.length) + SCORED_L4S[branchNumber].set(true); } ); }
174-184: getBranchNumberFromScoringPose() should not default to 0Defaulting to 0 can corrupt scoring state. Return -1 to signal “not found”.
private static int getBranchNumberFromScoringPose(Pose2d scoringPose) { for (FieldConstants.ReefClockPosition currentClockPosition : FieldConstants.ReefClockPosition.values()) { for (FieldConstants.ReefSide currentSide : FieldConstants.ReefSide.values()) { final Pose2d reefSideScoringPose = CoralPlacingCommands.ScoringLevel.L4.calculateTargetPlacingPosition(currentClockPosition, currentSide).get(); if (reefSideScoringPose.getTranslation().getDistance(scoringPose.getTranslation()) < 0.01) return currentClockPosition.ordinal() * 2 + currentSide.ordinal(); } } - - return 0; + return -1; }
There was a problem hiding this comment.
Actionable comments posted: 1
♻️ Duplicate comments (11)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (11)
35-35: Critical: Thread-safety issue with shared mutable TARGET_SCORING_POSE.The
TARGET_SCORING_POSEfield is accessed and modified across multiple commands without synchronization (lines 78, 87, 115-117, 137, 166). In the command-based framework, multiple commands can run concurrently, leading to race conditions where:
- One command sets the pose while another reads it
- A pose becomes null between a null-check and dereferencing
- Multiple scoring sequences interfere with each other
Use thread-safe state management:
- private static FlippablePose2d TARGET_SCORING_POSE = null; + private static final AtomicReference<FlippablePose2d> TARGET_SCORING_POSE = new AtomicReference<>(null);Then update all usages:
- Line 78:
TARGET_SCORING_POSE.set(calculateClosestOpenScoringPose())- Line 79:
() -> TARGET_SCORING_POSE.get() != null- Line 80:
() -> TARGET_SCORING_POSE.get()- Line 87:
() -> TARGET_SCORING_POSE.get() != null- Lines 115-117: Cache
TARGET_SCORING_POSE.get()in a local variable first- Line 137: Cache
TARGET_SCORING_POSE.get()in a local variable first- Line 166: Cache
TARGET_SCORING_POSE.get()in a local variable firstAdd
import java.util.concurrent.atomic.AtomicReference;at the top.
92-97: Major: Potential NPE when tracked coral disappears during assist intake.The
getDriveToCoralCommandmethod callsIntakeAssistCommand.getAssistIntakeCommandwith a supplier that may return null if the tracked game piece disappears mid-execution. The assist command will dereference this supplier, potentially causing a NullPointerException.Wrap the assist command in a continuous conditional that falls back to driver control when tracking is lost:
public static Command getDriveToCoralCommand(boolean isRight) { return new SequentialCommandGroup( getFindCoralCommand(isRight).unless(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null).until(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null), - IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece).withTimeout(1.5) + GeneralCommands.getContinuousConditionalCommand( + IntakeAssistCommand.getAssistIntakeCommand( + IntakeAssistCommand.AssistMode.FULL_ASSIST, + IntakeAssistCommand::calculateDistanceFromTrackedGamePiece), + GeneralCommands.getFieldRelativeDriveCommand(), + () -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null + && IntakeAssistCommand.calculateDistanceFromTrackedGamePiece() != null + ).withTimeout(1.5) ).repeatedly(); }Alternatively, make
IntakeAssistCommand.getAssistIntakeCommandnull-safe internally by adding continuous null-checking.
163-170: Critical: Missing null check in getAddCurrentScoringBranchToScoredBranchesCommand.The lambda on line 166 accesses
TARGET_SCORING_POSE.get()without verifying it's non-null, which can cause a NullPointerException if the command executes before the target pose is set or after it's cleared.Add null safety to the lambda:
private static Command getAddCurrentScoringBranchToScoredBranchesCommand() { return new InstantCommand( () -> { + final FlippablePose2d targetPose = TARGET_SCORING_POSE; + if (targetPose == null) + return; - final int branchNumber = getBranchNumberFromScoringPose(TARGET_SCORING_POSE.get()); + final int branchNumber = getBranchNumberFromScoringPose(targetPose.get()); SCORED_L4S[branchNumber].set(true); } ); }
135-139: Critical: Missing null check in calculateDistanceToTargetScoringPose.The method dereferences
TARGET_SCORING_POSE.get()on line 137 without checking if it's null, which will throw a NullPointerException if called before the target pose is set.Add null safety:
private static double calculateDistanceToTargetScoringPose() { + final FlippablePose2d targetPose = TARGET_SCORING_POSE; + if (targetPose == null) { + return Double.POSITIVE_INFINITY; + } final Translation2d currentTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); - final Translation2d targetTranslation = TARGET_SCORING_POSE.get().getTranslation(); + final Translation2d targetTranslation = targetPose.get().getTranslation(); return currentTranslation.getDistance(targetTranslation); }
113-118: Critical: Multiple dereferences of TARGET_SCORING_POSE without local caching.The
canScore()method checksTARGET_SCORING_POSE != nullon line 115, then calls.get()twice on lines 116-117. In a multi-threaded environment, the value could become null between the check and access, causing a NullPointerException.Cache the reference once and reuse it:
private static boolean canScore() { + final FlippablePose2d targetPose = TARGET_SCORING_POSE; + if (targetPose == null) + return false; + + final Pose2d currentPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Pose2d relativePose = currentPose.relativeTo(targetPose.get()); + return RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.SCORE_L4, true) && - TARGET_SCORING_POSE != null && - Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getX()) < AutonomousConstants.REEF_RELATIVE_X_TOLERANCE_METERS && - Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getY()) < AutonomousConstants.REEF_RELATIVE_Y_TOLERANCE_METERS; + Math.abs(relativePose.getX()) < AutonomousConstants.REEF_RELATIVE_X_TOLERANCE_METERS && + Math.abs(relativePose.getY()) < AutonomousConstants.REEF_RELATIVE_Y_TOLERANCE_METERS; }
34-35: TARGET_SCORING_POSE lacks thread-safety.The shared mutable field
TARGET_SCORING_POSEis written on line 78 and read on lines 80, 87, 115-117, 137, and 166 without synchronization. In a concurrent environment (e.g., multiple commands running in parallel), this can lead to race conditions where readers see stale or null values between a null check and subsequent access.Consider using
AtomicReferencefor thread-safe access:- private static FlippablePose2d TARGET_SCORING_POSE = null; + private static final AtomicReference<FlippablePose2d> TARGET_SCORING_POSE = new AtomicReference<>(null);Then update all usages:
- Line 78:
TARGET_SCORING_POSE.set(calculateClosestOpenScoringPose())- Line 79:
() -> TARGET_SCORING_POSE.get() != null- Line 80:
() -> TARGET_SCORING_POSE.get()- Line 87:
() -> TARGET_SCORING_POSE.get() != null- Lines 115-117: Cache the result in a local variable after checking for null
- Line 137: Cache the result in a local variable
- Line 166: Cache the result in a local variable
113-118: Race condition in canScore() between null check and access.The method checks
TARGET_SCORING_POSE != nullon line 115, then calls.get()twice on lines 116-117. In a multi-threaded environment,TARGET_SCORING_POSEcould become null between the check and the accesses, causing a NullPointerException.Cache the pose value once:
private static boolean canScore() { + final FlippablePose2d targetPose = TARGET_SCORING_POSE; + if (targetPose == null) return false; + final Pose2d currentPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Pose2d relativePose = currentPose.relativeTo(targetPose.get()); return RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.SCORE_L4, true) && - TARGET_SCORING_POSE != null && - Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getX()) < AutonomousConstants.REEF_RELATIVE_X_TOLERANCE_METERS && - Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getY()) < AutonomousConstants.REEF_RELATIVE_Y_TOLERANCE_METERS; + Math.abs(relativePose.getX()) < AutonomousConstants.REEF_RELATIVE_X_TOLERANCE_METERS && + Math.abs(relativePose.getY()) < AutonomousConstants.REEF_RELATIVE_Y_TOLERANCE_METERS; }
135-139: Missing null check for TARGET_SCORING_POSE.The method
calculateDistanceToTargetScoringPose()callsTARGET_SCORING_POSE.get()on line 137 without checking ifTARGET_SCORING_POSEis null, which will throw a NullPointerException.Add a null check:
private static double calculateDistanceToTargetScoringPose() { + if (TARGET_SCORING_POSE == null) { + return Double.POSITIVE_INFINITY; + } final Translation2d currentTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); final Translation2d targetTranslation = TARGET_SCORING_POSE.get().getTranslation(); return currentTranslation.getDistance(targetTranslation); }
163-169: Missing null check in getAddCurrentScoringBranchToScoredBranchesCommand.The lambda on line 166 calls
TARGET_SCORING_POSE.get()without checking ifTARGET_SCORING_POSEis null, which will throw a NullPointerException.Add null safety:
private static Command getAddCurrentScoringBranchToScoredBranchesCommand() { return new InstantCommand( () -> { + if (TARGET_SCORING_POSE == null) return; final int branchNumber = getBranchNumberFromScoringPose(TARGET_SCORING_POSE.get()); SCORED_L4S[branchNumber].set(true); } ); }
92-97: NPE risk when tracked game piece is lost mid-command.The
getDriveToCoralCommanddereferencesIntakeAssistCommand::calculateDistanceFromTrackedGamePieceingetAssistIntakeCommand(line 95). IfCORAL_POSE_ESTIMATOR.getClosestObjectToRobot()becomes null after line 94 completes but before the assist command runs, the supplier will return null and cause a NullPointerException when dereferenced inside the PID controllers.Wrap the assist command in a continuous conditional that falls back to driver control when tracking is lost:
public static Command getDriveToCoralCommand(boolean isRight) { return new SequentialCommandGroup( getFindCoralCommand(isRight).unless(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null).until(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null), - IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece).withTimeout(1.5) + GeneralCommands.getContinuousConditionalCommand( + IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece), + GeneralCommands.getFieldRelativeDriveCommand(), + () -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null && IntakeAssistCommand.calculateDistanceFromTrackedGamePiece() != null + ).withTimeout(1.5) ).repeatedly(); }
172-182: Returning 0 as fallback silently maps to a valid branch.If no scoring pose matches (line 181), the method returns
0, which corresponds to a valid branch number (clock position 12, side RIGHT at index 0). This silently marks the wrong branch as scored, corrupting the tracking logic.Return a sentinel value or throw an exception:
private static int getBranchNumberFromScoringPose(Pose2d scoringPose) { for (FieldConstants.ReefClockPosition currentClockPosition : FieldConstants.ReefClockPosition.values()) { for (FieldConstants.ReefSide currentSide : FieldConstants.ReefSide.values()) { final Pose2d reefSideScoringPose = CoralPlacingCommands.ScoringLevel.L4.calculateTargetPlacingPosition(currentClockPosition, currentSide).get(); if (reefSideScoringPose.getTranslation().getDistance(scoringPose.getTranslation()) < 0.01) return currentClockPosition.ordinal() * 2 + currentSide.ordinal(); } } - return 0; + throw new IllegalArgumentException("Scoring pose does not match any reef branch: " + scoringPose); }Alternatively, return
-1as a sentinel and handle it in the caller.
🧹 Nitpick comments (3)
src/main/java/frc/trigon/robot/RobotContainer.java (3)
127-127: Consider narrowing the @SuppressWarnings annotation.The
@SuppressWarnings("All")annotation suppresses all warnings for this method, which can hide potential issues. Consider using more specific suppression tags (e.g.,"unchecked","deprecation") or addressing the warnings directly.
127-127: Replace overly broad @SuppressWarnings("All") with specific warnings.Using
@SuppressWarnings("All")suppresses all compiler warnings indiscriminately, which can hide legitimate issues. Specify the exact warnings you need to suppress.If the warning is about unchecked operations or raw types, apply this diff:
- @SuppressWarnings("All") + @SuppressWarnings({"unchecked", "rawtypes"}) private void buildAutoChooser() {Otherwise, remove the annotation entirely if no warnings are present.
129-161: Extract "Mirrored" suffix to a constant.The string literal
"Mirrored"is repeated 5 times (lines 141, 142, 144, 145, 148). Extracting it to a constant improves maintainability and prevents typos.Add a constant at the top of the method:
private void buildAutoChooser() { + final String MIRRORED_SUFFIX = "Mirrored"; autoChooser = new LoggedDashboardChooser<>("AutoChooser");Then replace all occurrences:
- autoChooser.addOption(autoMirrored.getName() + "Mirrored", autoMirrored); + autoChooser.addOption(autoMirrored.getName() + MIRRORED_SUFFIX, autoMirrored);
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (4)
src/main/java/frc/trigon/robot/RobotContainer.java(3 hunks)src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java(1 hunks)src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java(1 hunks)src/main/java/frc/trigon/robot/subsystems/intake/Intake.java(1 hunks)
🚧 Files skipped from review as they are similar to previous changes (1)
- src/main/java/frc/trigon/robot/subsystems/intake/Intake.java
🧰 Additional context used
🧬 Code graph analysis (3)
src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java (1)
src/main/java/frc/trigon/robot/constants/FieldConstants.java (1)
FieldConstants(18-121)
src/main/java/frc/trigon/robot/RobotContainer.java (2)
src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(27-104)src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (1)
AutonomousCommands(33-232)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (12)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-167)src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (1)
IntakeAssistCommand(22-190)src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(27-104)src/main/java/frc/trigon/robot/constants/FieldConstants.java (1)
FieldConstants(18-121)src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java (1)
ArmElevatorCommands(15-78)src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java (1)
ArmElevatorConstants(28-366)src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java (1)
EndEffectorCommands(10-27)src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java (1)
IntakeCommands(11-45)src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java (1)
SwerveCommands(15-163)src/main/java/frc/trigon/robot/subsystems/transporter/TransporterCommands.java (1)
TransporterCommands(9-44)src/main/java/lib/utilities/flippable/FlippablePose2d.java (1)
FlippablePose2d(11-58)src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java (1)
CoralPlacingCommands(23-254)
🔇 Additional comments (10)
src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java (1)
212-229: LGTM! Well-documented pose calculation method.The implementation correctly:
- Constructs the reef center pose using the clock position angle
- Applies conditional Y-transform flipping based on reef side and clock position
- Returns a properly configured FlippablePose2d with alliance flipping enabled
The Javadoc clearly explains the driver-station-relative left/right convention, which is helpful for operators.
src/main/java/frc/trigon/robot/RobotContainer.java (3)
129-161: Well-structured auto chooser setup with mirrored auto support.The new implementation provides better control over auto registration by:
- Explicitly creating both mirrored and non-mirrored PathPlannerAuto instances
- Handling default auto selection with proper fallback to "None"
- Integrating floor autonomous commands from the new AutonomousCommands API
The logic flow is clear and maintainable.
163-166: LGTM! Clean helper method for command registration.The helper method provides a convenient way to register multiple commands to the auto chooser using their names.
138-149: Verify mirrored auto names align withgetName()output and narrow suppression.
- PathPlannerAuto is provided by an external library—confirm that
getName()returns only the base path name (so concatenating"Mirrored"yields chooser entries that match the default-name check).- Change the broad
@SuppressWarnings("All")onbuildAutoChooser(around line 127) to only the specific warnings needed.src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (6)
141-161: LGTM! Closest open scoring pose calculation fully implemented.The method now correctly:
- Iterates through all reef clock positions and sides
- Skips already-scored branches using the
scoredBranchesAtL4array- Calculates distances using the new
calculateTargetPlacingPositionmethod from CoralPlacingCommands- Tracks and returns the closest unscored pose
This resolves the previous TODO/placeholder concern. The logic is sound and integrates well with the scoring tracking system.
172-182: LGTM! Branch number calculation fully implemented.The method now correctly:
- Iterates through all reef clock positions and sides
- Compares the scoring pose translation with each branch's target placing position
- Returns the proper branch index using
ordinal() * 2 + ordinal()encodingThis resolves the previous concern about always returning 0. The 0.01-meter distance threshold is reasonable for pose matching.
34-34: LGTM! Well-designed scoring branch tracking system.The
SCORED_L4Sarray and helper methods provide a clean abstraction for:
- Tracking which L4 branches have been scored (logged to NetworkTables)
- Converting between LoggedNetworkBoolean array and primitive boolean array
- Initializing the array with proper NetworkTables keys
The implementation is straightforward and integrates well with the scoring logic.
Also applies to: 184-198
37-64: Well-structured autonomous command pipeline.The new command factories provide a coherent autonomous flow:
- Floor autonomous loop: Cycles between driving to reef, scoring, and collecting coral
- Find coral: Drives to search pose and rotates to locate game pieces
- Collect coral: Coordinates intake, transporter, and drive-to-coral with proper termination conditions
- Score sequence: Manages loading, preparation, proximity checking, and placement
The command composition follows good practices with proper parallel/sequential grouping and termination predicates. The integration with subsystem commands (IntakeCommands, TransporterCommands, etc.) is clean.
Also applies to: 66-74, 76-90, 99-111, 120-133
141-161: LGTM! calculateClosestOpenScoringPose implementation is now complete.The method correctly iterates over all reef clock positions and sides, skips already-scored branches, and returns the closest open scoring pose. The logic is sound and the null check on line 160 properly handles the case where no open poses are available.
184-198: LGTM! Helper methods for branch tracking are well-structured.The
getEmptyL4LoggedNetworkBooleanArray()andgetScoredBranchesAtL4()methods correctly initialize and retrieve the scored branch state. The LoggedNetworkBoolean array enables dashboard monitoring and persistence.
| /** | ||
| * Calculates the target placing position using the clock position and the target reef side. | ||
| * The reef side transform will be flipped depending on operator input. | ||
| * To make it more intuitive for the operator to input the reef side, | ||
| * left will always correspond to the physical left side in the driver station, | ||
| * as opposed to "reef relative" left. | ||
| * | ||
| * @param reefClockPosition the desired clock position of the reef | ||
| * @param reefSide the desired side of the reef, left or right (as seen from the driver station) | ||
| * @return the target placing position | ||
| */ | ||
| public FlippablePose2d calculateTargetPlacingPosition(FieldConstants.ReefClockPosition reefClockPosition, FieldConstants.ReefSide reefSide) { | ||
| final Pose2d reefCenterPose = new Pose2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, reefClockPosition.clockAngle); | ||
| final double yTransform = reefSide.shouldFlipYTransform(reefClockPosition) ? -positiveYTransformMeters : positiveYTransformMeters; | ||
| final Transform2d transform = new Transform2d(xTransformMeters, yTransform, rotationTransform); | ||
|
|
||
| return new FlippablePose2d(reefCenterPose.plus(transform), true); | ||
| } |
There was a problem hiding this comment.
Guard against calling calculateTargetPlacingPosition on L1.
The method will produce an invalid pose when called on L1 because xTransformMeters is Double.NaN, positiveYTransformMeters is Double.NaN, and rotationTransform is null (line 183). This will result in a Pose2d with NaN coordinates and a NullPointerException when constructing the Transform2d on line 226.
Apply this diff to add a guard:
public FlippablePose2d calculateTargetPlacingPosition(FieldConstants.ReefClockPosition reefClockPosition, FieldConstants.ReefSide reefSide) {
+ if (this == L1) {
+ throw new UnsupportedOperationException("L1 does not support calculateTargetPlacingPosition");
+ }
final Pose2d reefCenterPose = new Pose2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, reefClockPosition.clockAngle);
final double yTransform = reefSide.shouldFlipYTransform(reefClockPosition) ? -positiveYTransformMeters : positiveYTransformMeters;
final Transform2d transform = new Transform2d(xTransformMeters, yTransform, rotationTransform);
return new FlippablePose2d(reefCenterPose.plus(transform), true);
}📝 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.
| /** | |
| * Calculates the target placing position using the clock position and the target reef side. | |
| * The reef side transform will be flipped depending on operator input. | |
| * To make it more intuitive for the operator to input the reef side, | |
| * left will always correspond to the physical left side in the driver station, | |
| * as opposed to "reef relative" left. | |
| * | |
| * @param reefClockPosition the desired clock position of the reef | |
| * @param reefSide the desired side of the reef, left or right (as seen from the driver station) | |
| * @return the target placing position | |
| */ | |
| public FlippablePose2d calculateTargetPlacingPosition(FieldConstants.ReefClockPosition reefClockPosition, FieldConstants.ReefSide reefSide) { | |
| final Pose2d reefCenterPose = new Pose2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, reefClockPosition.clockAngle); | |
| final double yTransform = reefSide.shouldFlipYTransform(reefClockPosition) ? -positiveYTransformMeters : positiveYTransformMeters; | |
| final Transform2d transform = new Transform2d(xTransformMeters, yTransform, rotationTransform); | |
| return new FlippablePose2d(reefCenterPose.plus(transform), true); | |
| } | |
| /** | |
| * Calculates the target placing position using the clock position and the target reef side. | |
| * The reef side transform will be flipped depending on operator input. | |
| * To make it more intuitive for the operator to input the reef side, | |
| * left will always correspond to the physical left side in the driver station, | |
| * as opposed to "reef relative" left. | |
| * | |
| * @param reefClockPosition the desired clock position of the reef | |
| * @param reefSide the desired side of the reef, left or right (as seen from the driver station) | |
| * @return the target placing position | |
| */ | |
| public FlippablePose2d calculateTargetPlacingPosition(FieldConstants.ReefClockPosition reefClockPosition, FieldConstants.ReefSide reefSide) { | |
| if (this == L1) { | |
| throw new UnsupportedOperationException("L1 does not support calculateTargetPlacingPosition"); | |
| } | |
| final Pose2d reefCenterPose = new Pose2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, reefClockPosition.clockAngle); | |
| final double yTransform = reefSide.shouldFlipYTransform(reefClockPosition) ? -positiveYTransformMeters : positiveYTransformMeters; | |
| final Transform2d transform = new Transform2d(xTransformMeters, yTransform, rotationTransform); | |
| return new FlippablePose2d(reefCenterPose.plus(transform), true); | |
| } |
🤖 Prompt for AI Agents
In
src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java
around lines 212 to 229, add a guard at the start of
calculateTargetPlacingPosition to detect when reefClockPosition ==
FieldConstants.ReefClockPosition.L1 and fail fast instead of continuing (because
xTransformMeters and positiveYTransformMeters are NaN and rotationTransform is
null for L1); throw an appropriate unchecked exception (e.g.,
IllegalArgumentException or IllegalStateException) with a clear message
indicating that calculateTargetPlacingPosition is not valid for L1 so callers
know why it failed.
Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com>
Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com>
Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com>
Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com>
There was a problem hiding this comment.
Actionable comments posted: 3
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (1)
src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (1)
61-67: Guard against null distances before using the supplier.
distanceFromTrackedGamePiece.get()is invoked three times in this block. When the camera momentarily loses the coral,calculateDistanceFromTrackedGamePiece()returnsnull, so any of these calls will throw and abort the command (and, in auto, the entire routine). Add a wait/conditional around the assist so you only reset the PID and compute powers when the supplier yields a non-null translation, and fall back to driver control otherwise.
♻️ Duplicate comments (1)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (1)
92-97: Keep the assist sequence null-safe.This call still dereferences
calculateDistanceFromTrackedGamePiece()without ensuring a live target, so the assist crashes as soon as tracking drops mid-run. Wrap the PID assist in a continuous conditional (or wait) that re-checksCORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != nullbefore every supplier access, and fall back to driver control when it isn’t.
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (4)
src/main/java/frc/trigon/robot/RobotContainer.java(3 hunks)src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java(3 hunks)src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java(1 hunks)src/main/java/frc/trigon/robot/constants/CameraConstants.java(1 hunks)
🧰 Additional context used
🧬 Code graph analysis (4)
src/main/java/frc/trigon/robot/constants/CameraConstants.java (2)
src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java (1)
AprilTagCamera(15-184)src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java (1)
AprilTagCameraConstants(17-66)
src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (1)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-167)
src/main/java/frc/trigon/robot/RobotContainer.java (3)
src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java (1)
ObjectPoseEstimator(15-200)src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(27-104)src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (1)
AutonomousCommands(33-226)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (16)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-167)src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (1)
IntakeAssistCommand(22-190)src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(27-104)src/main/java/frc/trigon/robot/constants/FieldConstants.java (1)
FieldConstants(18-121)src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java (1)
ArmElevatorCommands(15-78)src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java (1)
ArmElevatorConstants(28-366)src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java (1)
EndEffectorCommands(10-27)src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java (1)
EndEffectorConstants(18-102)src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java (1)
IntakeCommands(11-45)src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java (1)
IntakeConstants(30-234)src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java (1)
SwerveCommands(15-163)src/main/java/frc/trigon/robot/subsystems/transporter/TransporterCommands.java (1)
TransporterCommands(9-44)src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java (1)
TransporterConstants(21-120)src/main/java/lib/utilities/flippable/FlippablePose2d.java (1)
FlippablePose2d(11-58)src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java (1)
CoralCollectionCommands(18-68)src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java (1)
GeneralCommands(19-96)
| addCommandsToChooser( | ||
| AutonomousCommands.getFloorAutonomousCommand(true), | ||
| AutonomousCommands.getFloorAutonomousCommand(false) | ||
| ); | ||
| } | ||
|
|
||
| private void addCommandsToChooser(Command... commands) { | ||
| for (Command command : commands) | ||
| autoChooser.addOption(command.getName(), command); | ||
| } |
There was a problem hiding this comment.
Restore default selection when DEFAULT_AUTO_NAME targets custom commands.
Because hasDefault is finalized before calling addCommandsToChooser(...), any DEFAULT_AUTO_NAME that points at FloorAutonomousRight/Left (or future manual commands) will never mark the chooser’s default. We always fall through to addDefaultOption("None", …), so the operator loses their configured default autonomous. Please roll the default handling into the helper so these commands can satisfy the same selection logic as the PathPlanner autos.
- addCommandsToChooser(
- AutonomousCommands.getFloorAutonomousCommand(true),
- AutonomousCommands.getFloorAutonomousCommand(false)
- );
+ hasDefault = addCommandsToChooser(
+ hasDefault,
+ AutonomousCommands.getFloorAutonomousCommand(true),
+ AutonomousCommands.getFloorAutonomousCommand(false)
+ );
+
+ if (!hasDefault)
+ autoChooser.addDefaultOption("None", Commands.none());
+ else
+ autoChooser.addOption("None", Commands.none());
- }
-
- private void addCommandsToChooser(Command... commands) {
- for (Command command : commands)
- autoChooser.addOption(command.getName(), command);
+ }
+
+ private boolean addCommandsToChooser(boolean hasDefault, Command... commands) {
+ boolean hasDefaultAfter = hasDefault;
+ for (Command command : commands) {
+ final boolean matchesDefault = !AutonomousConstants.DEFAULT_AUTO_NAME.isEmpty()
+ && AutonomousConstants.DEFAULT_AUTO_NAME.equals(command.getName());
+ if (matchesDefault && !hasDefaultAfter) {
+ autoChooser.addDefaultOption(command.getName(), command);
+ hasDefaultAfter = true;
+ } else {
+ autoChooser.addOption(command.getName(), command);
+ }
+ }
+ return hasDefaultAfter;
}📝 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.
| addCommandsToChooser( | |
| AutonomousCommands.getFloorAutonomousCommand(true), | |
| AutonomousCommands.getFloorAutonomousCommand(false) | |
| ); | |
| } | |
| private void addCommandsToChooser(Command... commands) { | |
| for (Command command : commands) | |
| autoChooser.addOption(command.getName(), command); | |
| } | |
| // Register floor-autonomous commands and update hasDefault if one matches DEFAULT_AUTO_NAME | |
| hasDefault = addCommandsToChooser( | |
| hasDefault, | |
| AutonomousCommands.getFloorAutonomousCommand(true), | |
| AutonomousCommands.getFloorAutonomousCommand(false) | |
| ); | |
| // Ensure “None” is added appropriately as default or just as another option | |
| if (!hasDefault) | |
| autoChooser.addDefaultOption("None", Commands.none()); | |
| else | |
| autoChooser.addOption("None", Commands.none()); | |
| } | |
| private boolean addCommandsToChooser(boolean hasDefault, Command... commands) { | |
| boolean hasDefaultAfter = hasDefault; | |
| for (Command command : commands) { | |
| final boolean matchesDefault = !AutonomousConstants.DEFAULT_AUTO_NAME.isEmpty() | |
| && AutonomousConstants.DEFAULT_AUTO_NAME.equals(command.getName()); | |
| if (matchesDefault && !hasDefaultAfter) { | |
| autoChooser.addDefaultOption(command.getName(), command); | |
| hasDefaultAfter = true; | |
| } else { | |
| autoChooser.addOption(command.getName(), command); | |
| } | |
| } | |
| return hasDefaultAfter; | |
| } |
Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com>
Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com>
There was a problem hiding this comment.
Actionable comments posted: 4
♻️ Duplicate comments (2)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (2)
125-129: Missing null check forTARGET_SCORING_POSE.Line 127 calls
TARGET_SCORING_POSE.get()without verifying thatTARGET_SCORING_POSEis not null. This will throw an NPE ifTARGET_SCORING_POSEis null.Add a null guard:
private static double calculateDistanceToTargetScoringPose() { + if (TARGET_SCORING_POSE == null) { + return Double.POSITIVE_INFINITY; + } final Translation2d currentTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); final Translation2d targetTranslation = TARGET_SCORING_POSE.get().getTranslation(); return currentTranslation.getDistance(targetTranslation); }Based on past review comments.
153-160: Missing null check when accessingTARGET_SCORING_POSE.Line 156 calls
TARGET_SCORING_POSE.get()without verifying thatTARGET_SCORING_POSEis not null. IfTARGET_SCORING_POSEis null when this command executes, it will throw an NPE.Add a null guard:
private static Command getAddCurrentScoringBranchToScoredBranchesCommand() { return new InstantCommand( () -> { + if (TARGET_SCORING_POSE == null) return; final int branchNumber = getBranchNumberFromScoringPose(TARGET_SCORING_POSE.get()); SCORED_L4S[branchNumber].set(true); } ); }Based on past review comments.
🧹 Nitpick comments (1)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (1)
103-108: CacheTARGET_SCORING_POSElocally to avoid repeated dereferencing.Lines 106-107 call
TARGET_SCORING_POSE.get()twice. While the null check on line 105 should protect against NPE, caching the value locally improves clarity and guards against accidental modification.Apply this diff:
private static boolean canScore() { + final FlippablePose2d target = TARGET_SCORING_POSE; + if (target == null) return false; + return (RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.SCORE_L4.prepareState, true) || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.SCORE_L4.prepareState)) && - TARGET_SCORING_POSE != null && - Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getX()) < AutonomousConstants.REEF_RELATIVE_X_TOLERANCE_METERS && - Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getY()) < AutonomousConstants.REEF_RELATIVE_Y_TOLERANCE_METERS; + Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(target.get()).getX()) < AutonomousConstants.REEF_RELATIVE_X_TOLERANCE_METERS && + Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(target.get()).getY()) < AutonomousConstants.REEF_RELATIVE_Y_TOLERANCE_METERS; }Based on past review comments.
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (2)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java(1 hunks)src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java(1 hunks)
✅ Files skipped from review due to trivial changes (1)
- src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java
🧰 Additional context used
🧬 Code graph analysis (1)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (13)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-167)src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (1)
IntakeAssistCommand(22-190)src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(27-104)src/main/java/frc/trigon/robot/constants/FieldConstants.java (1)
FieldConstants(18-121)src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java (1)
ArmElevatorCommands(15-78)src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java (1)
ArmElevatorConstants(28-366)src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java (1)
EndEffectorCommands(10-27)src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java (1)
EndEffectorConstants(18-102)src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java (1)
SwerveCommands(15-163)src/main/java/lib/utilities/flippable/FlippablePose2d.java (1)
FlippablePose2d(11-58)src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java (1)
CoralCollectionCommands(18-68)src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java (1)
GeneralCommands(19-96)src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java (1)
CoralPlacingCommands(23-254)
🔇 Additional comments (11)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (11)
33-35: LGTM!Clear factory method for floor autonomous with appropriate naming.
37-42: LGTM!Sequencing logic is clear and correct.
44-53: LGTM!Clear command to find coral by driving to a predetermined pose and rotating.
55-60: LGTM!Parallel race correctly orchestrates driving and scoring sequence.
62-70: LGTM!Parallel collection logic with appropriate termination guards.
80-86: LGTM!Sequence correctly waits for
TARGET_SCORING_POSEbefore scoring.
96-101: LGTM with caveat.Scoring sequence logic is correct, but
canScore()has null safety issues flagged in a separate comment.
110-116: LGTM!Parallel scoring command with appropriate timeout.
118-123: LGTM with caveat.Command logic is correct, but
calculateDistanceToTargetScoringPose()has a null safety issue flagged in a separate comment.
131-151: LGTM! Implementation now complete.Past reviews flagged this method as unimplemented. The search logic is now correctly implemented: iterates all reef branches, skips scored ones, and returns the closest open pose. The null return on line 150 is appropriate when no open branches exist and is guarded by callers (e.g., line 75).
Based on past review comments.
174-188: LGTM!Helper methods for managing scored branch state are correct. Array size of 12 matches the 6 clock positions × 2 sides.
| */ | ||
| public class AutonomousCommands { | ||
| public static final LoggedNetworkBoolean[] SCORED_L4S = getEmptyL4LoggedNetworkBooleanArray(); | ||
| private static FlippablePose2d TARGET_SCORING_POSE = null; |
There was a problem hiding this comment.
Shared mutable state requires careful null handling.
Past reviews flagged TARGET_SCORING_POSE for concurrency issues. While WPILib commands typically run on a single thread, the real risk here is null pointer exceptions when multiple command sequences access this field. Lines 105-107, 127, and 156 call .get() on TARGET_SCORING_POSE without guarding against it becoming null mid-execution.
Consider one of these approaches:
Option A (safer): Make TARGET_SCORING_POSE an AtomicReference<FlippablePose2d> and update all reads/writes:
- private static FlippablePose2d TARGET_SCORING_POSE = null;
+ private static final AtomicReference<FlippablePose2d> TARGET_SCORING_POSE = new AtomicReference<>(null);Then update line 74, line 75, etc., to use .set() and .get().
Option B (simpler): Cache the value locally in each method before dereferencing:
private static boolean canScore() {
final FlippablePose2d target = TARGET_SCORING_POSE;
if (target == null) return false;
// ... use target.get() ...
}Based on past review comments.
🤖 Prompt for AI Agents
In
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
around line 31, TARGET_SCORING_POSE is a shared mutable static that is
dereferenced without null-safety elsewhere (lines referenced in the review) and
can cause NPEs; either convert the static to an AtomicReference<FlippablePose2d>
and update all writes to use .set(...) and reads to use .get() (preferred for
thread-safety), or, as a simpler alternative, modify each method that
dereferences TARGET_SCORING_POSE (e.g., the methods around lines 74, 105–107,
127, 156) to first cache the value into a final local variable, check it for
null and early-return or handle the null case before calling .get(); apply the
chosen approach consistently across all read/write sites.
| public static Command getDriveToReefCommand() { | ||
| return new SequentialCommandGroup( | ||
| new InstantCommand(() -> TARGET_SCORING_POSE = calculateClosestOpenScoringPose()), | ||
| new WaitUntilCommand(() -> TARGET_SCORING_POSE != null).raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), | ||
| SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly() | ||
| ); | ||
| } |
There was a problem hiding this comment.
Potential NPE if TARGET_SCORING_POSE becomes null after line 75.
Line 76 repeatedly drives to TARGET_SCORING_POSE, but if calculateClosestOpenScoringPose() returns null (line 150) or if TARGET_SCORING_POSE is reset to null elsewhere, the supplier () -> TARGET_SCORING_POSE will throw an NPE when the drive command evaluates it.
Add a null guard in the supplier or ensure TARGET_SCORING_POSE is never null when accessed:
- SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly()
+ SwerveCommands.getDriveToPoseCommand(
+ () -> TARGET_SCORING_POSE != null ? TARGET_SCORING_POSE : new FlippablePose2d(new Pose2d(), false),
+ AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS
+ ).repeatedly()Or refactor to ensure calculateClosestOpenScoringPose() never returns null (e.g., return a default pose).
📝 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.
| public static Command getDriveToReefCommand() { | |
| return new SequentialCommandGroup( | |
| new InstantCommand(() -> TARGET_SCORING_POSE = calculateClosestOpenScoringPose()), | |
| new WaitUntilCommand(() -> TARGET_SCORING_POSE != null).raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), | |
| SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly() | |
| ); | |
| } | |
| public static Command getDriveToReefCommand() { | |
| return new SequentialCommandGroup( | |
| new InstantCommand(() -> TARGET_SCORING_POSE = calculateClosestOpenScoringPose()), | |
| new WaitUntilCommand(() -> TARGET_SCORING_POSE != null) | |
| .raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), | |
| SwerveCommands.getDriveToPoseCommand( | |
| () -> TARGET_SCORING_POSE != null | |
| ? TARGET_SCORING_POSE | |
| : new FlippablePose2d(new Pose2d(), false), | |
| AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS | |
| ).repeatedly() | |
| ); | |
| } |
🤖 Prompt for AI Agents
In
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
around lines 72–78, the supplier () -> TARGET_SCORING_POSE used by the drive
command can produce an NPE if TARGET_SCORING_POSE is null; change the flow so
the drive command uses a guaranteed non-null pose: capture the result of
calculateClosestOpenScoringPose() into a final local variable (or into a
dedicated final cached field) inside the InstantCommand and then pass a supplier
that returns that local cached value (or returns a documented default pose or
throws a controlled exception) instead of directly referencing the mutable
TARGET_SCORING_POSE; alternatively add a null-guarding supplier that checks for
null and safely handles it (e.g., returns lastKnownPose or cancels/stops the
command) so the drive command never dereferences a null.
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
Show resolved
Hide resolved
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
Show resolved
Hide resolved
Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com>
Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com>
There was a problem hiding this comment.
Actionable comments posted: 2
♻️ Duplicate comments (2)
src/main/java/frc/trigon/robot/constants/CameraConstants.java (1)
23-32: Do not ship zero vision covariance.
new StandardDeviations(0, 0)tells the pose estimator this camera is perfect, so it will overpower every other sensor and destabilize localization. Keep the previous calibrated values or plug in conservative non-zero defaults until real measurements are taken.src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (1)
163-172: Stop returning branch index 0 on “no match”.Falling through to
return 0silently reuses a valid branch index when no pose matches, corruptingSCORED_L4S. Throw or return a sentinel instead and let the caller handle it.
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (3)
src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java(6 hunks)src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java(1 hunks)src/main/java/frc/trigon/robot/constants/CameraConstants.java(1 hunks)
🧰 Additional context used
🧬 Code graph analysis (3)
src/main/java/frc/trigon/robot/constants/CameraConstants.java (2)
src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java (1)
AprilTagCamera(15-184)src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java (1)
AprilTagCameraConstants(17-66)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (8)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-167)src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (1)
IntakeAssistCommand(22-193)src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(27-104)src/main/java/frc/trigon/robot/constants/FieldConstants.java (1)
FieldConstants(18-121)src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java (1)
SwerveCommands(15-163)src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java (1)
CoralCollectionCommands(18-68)src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java (1)
GeneralCommands(19-96)src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java (1)
CoralPlacingCommands(23-254)
src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (4)
src/main/java/lib/hardware/RobotHardwareStats.java (1)
RobotHardwareStats(6-78)src/main/java/frc/trigon/robot/constants/OperatorConstants.java (1)
OperatorConstants(12-99)src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java (1)
SwerveCommands(15-163)src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-167)
src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java
Show resolved
Hide resolved
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
Show resolved
Hide resolved
Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com>
There was a problem hiding this comment.
Actionable comments posted: 1
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (1)
src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java (1)
51-55: Verify the order of operations.The code sets
IS_HOLDING_ALGAE = true(line 52) before unloading coral (line 55), but the AI summary states this flag was moved to occur after the unload step. This inconsistency suggests either:
- The AI summary is incorrect, or
- The intended change was not properly applied
Additionally, setting
IS_HOLDING_ALGAE = truebefore actually collecting algae or clearing the coral creates a state inconsistency where the robot claims to hold algae when it may still be holding coral.Consider moving the
InstantCommandthat sets the flags to after line 55:return new SequentialCommandGroup( GeneralCommands.getResetFlipArmOverrideCommand(), - new InstantCommand(() -> { - IS_HOLDING_ALGAE = true; - SHOULD_COLLECT_FROM_LOLLIPOP = false; - }), CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece), + new InstantCommand(() -> { + IS_HOLDING_ALGAE = true; + SHOULD_COLLECT_FROM_LOLLIPOP = false; + }), getInitiateFloorAlgaeCollectionCommand().until(RobotContainer.END_EFFECTOR::hasGamePiece),
♻️ Duplicate comments (1)
src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (1)
60-68: Critical: Address the existing NPE risk flagged in past reviews.The past review comment (lines 60-67) correctly identified that
distanceFromTrackedGamePiece.get()is called multiple times without caching. If tracking is lost between theonlyWhilecheck and these supplier calls during command execution, an NPE can occur.Apply this diff to cache the supplier result:
public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier<Translation2d> distanceFromTrackedGamePiece, double intakeAssistScalar) { return new SequentialCommandGroup( - new InstantCommand(() -> resetPIDControllers(distanceFromTrackedGamePiece.get())), - SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> calculateTranslationPower(assistMode, distanceFromTrackedGamePiece.get(), intakeAssistScalar).getX(), - () -> calculateTranslationPower(assistMode, distanceFromTrackedGamePiece.get(), intakeAssistScalar).getY(), - () -> calculateThetaPower(assistMode, distanceFromTrackedGamePiece.get(), intakeAssistScalar) - ) + new InstantCommand(() -> { + final Translation2d distance = distanceFromTrackedGamePiece.get(); + if (distance == null) return; + resetPIDControllers(distance); + }), + new DeferredCommand(() -> { + final Translation2d distance = distanceFromTrackedGamePiece.get(); + if (distance == null) + return new InstantCommand(); // no-op + return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + () -> calculateTranslationPower(assistMode, distance, intakeAssistScalar).getX(), + () -> calculateTranslationPower(assistMode, distance, intakeAssistScalar).getY(), + () -> calculateThetaPower(assistMode, distance, intakeAssistScalar) + ); + }, Set.of()) ); }
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (5)
src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java(6 hunks)src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java(2 hunks)src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java(3 hunks)src/main/java/frc/trigon/robot/constants/OperatorConstants.java(1 hunks)src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java(1 hunks)
🧰 Additional context used
🧬 Code graph analysis (4)
src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java (1)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-167)
src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java (5)
src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (1)
IntakeAssistCommand(22-201)src/main/java/frc/trigon/robot/constants/OperatorConstants.java (1)
OperatorConstants(12-99)src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java (1)
GeneralCommands(19-96)src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java (1)
EndEffectorCommands(10-27)src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-167)
src/main/java/frc/trigon/robot/constants/OperatorConstants.java (1)
src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (1)
IntakeAssistCommand(22-201)
src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (3)
src/main/java/frc/trigon/robot/constants/OperatorConstants.java (1)
OperatorConstants(12-99)src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java (1)
SwerveCommands(15-163)src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-167)
🔇 Additional comments (15)
src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java (1)
339-339: LGTM! Verify the new elevator position works correctly for unloading coral.The new value (0.603m) aligns with
SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERSand is consistently used across multiple states (REST, REST_WITH_CORAL, etc.). This standardization appears intentional.Please confirm through testing that:
- The coral piece unloads correctly at the new height
- No mechanical interference or collisions occur
- The UNLOAD_CORAL operation completes successfully
src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java (1)
88-91: Ignore floor state concern in align command.
getAlignToReefCommandis only invoked ingetReefAlgaeCollectionCommand, which setsIS_HOLDING_ALGAEimmediately after unloading coral; the floor collection sequence doesn’t call align. No changes needed.Likely an incorrect or invalid review comment.
src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java (3)
30-30: LGTM! Clean integration of IntakeAssistCommand.The
alongWithpattern properly runs the intake assist command in parallel with the coral collection sequence, using the configured default assist mode.
47-47: LGTM! Appropriate visibility reduction.Reducing visibility from
privateto package-private is appropriate given the new usage pattern where this method is called internally within the command factory.
43-44: Verified thatgetUnloadCoralCommandcorrectly terminates on!RobotContainer.END_EFFECTOR.hasGamePiece()and is only ever scheduled when a game piece is present.src/main/java/frc/trigon/robot/constants/OperatorConstants.java (1)
38-39: Major behavior change: Verify the new defaults are intended.These changes significantly alter intake assist behavior:
INTAKE_ASSIST_SCALARchanged from0.0to1, enabling full PID-driven assistance (previously disabled)DEFAULT_INTAKE_ASSIST_MODEchanged toFULL_ASSIST, enabling autonomous driving in X, Y, and theta axesThis means the robot will now autonomously drive to game pieces during intake operations. Ensure this has been tested and the PID gains are properly tuned for safe operation.
src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (9)
23-32: LGTM! Appropriate visibility for internal access.Making the PID controllers package-private allows access from related command factories while maintaining encapsulation.
78-88: LGTM! Good null safety and public API.Making this method public and adding proper null checking is appropriate. The method now provides a reusable utility for calculating distance to tracked game pieces.
90-102: LGTM! Proper null safety added.The null check at lines 91-92 prevents downstream NPEs when no game piece is tracked. The scalar parameter is correctly threaded through the calculation chain.
104-116: LGTM! Enhanced null safety and logging.The null check and added logging help with debugging and prevent errors when tracking is lost. The scalar parameter integration is correct.
130-139: LGTM! Correct scalar integration.The
intakeAssistScalarparameter is properly used in the blend calculation between PID output and joystick input.
141-149: LGTM! Consistent scalar usage.The theta assist power calculation correctly incorporates the scalar parameter for consistent behavior across all axes.
159-161: LGTM! Generalized calculation method.The signature change from a hard-coded constant to a
scalarparameter makes this method more reusable and testable.
163-167: LGTM! Correct theta PID initialization.The THETA_PID_CONTROLLER reset properly accounts for the 180-degree offset to align with the game piece orientation.
178-188: LGTM! Updated documentation.The javadoc updates correctly reflect that the assist behavior is now scaled by a configurable parameter rather than a hard-coded constant.
src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java
Outdated
Show resolved
Hide resolved
Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com>
There was a problem hiding this comment.
Actionable comments posted: 1
🧹 Nitpick comments (2)
src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java (2)
6-6: Remove unused import.The
CoralCollectionCommandsimport appears to be unused. Remove it to keep the codebase clean.Apply this diff:
-import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands;
112-119: Consider caching ALGAE rotations to avoid repeated calls.The
isLollipopmethod is called within a loop (line 80), and each call retrieves ALGAE rotations viagetTargetObjectsRotations. While the performance impact is minimal for typical game piece counts, caching the ALGAE rotations array would eliminate redundant lookups.Example refactor in
getTargetObjectsRotations:public Rotation3d[] getTargetObjectsRotations(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { if (targetGamePiece == SimulatedGamePieceConstants.GamePieceType.CORAL) { + Rotation3d[] algaeRotations = objectDetectionCameraInputs.visibleObjectRotations[SimulatedGamePieceConstants.GamePieceType.ALGAE.id]; ArrayList<Rotation3d> rotations = new ArrayList<>(); for (Rotation3d rotation : objectDetectionCameraInputs.visibleObjectRotations[targetGamePiece.id]) { - if (!isLollipop(rotation.toRotation2d())) + if (!isLollipop(rotation.toRotation2d(), algaeRotations)) rotations.add(rotation); } return rotations.toArray(new Rotation3d[0]); } return objectDetectionCameraInputs.visibleObjectRotations[targetGamePiece.id]; } -private boolean isLollipop(Rotation2d objectYaw) { - for (Rotation3d algaeYaw : getTargetObjectsRotations(SimulatedGamePieceConstants.GamePieceType.ALGAE)) { +private boolean isLollipop(Rotation2d objectYaw, Rotation3d[] algaeRotations) { + for (Rotation3d algaeYaw : algaeRotations) { final double difference = Math.abs(algaeYaw.toRotation2d().minus(objectYaw).getDegrees()); if (difference < ObjectDetectionCameraConstants.LOLLIPOP_TOLERANCE.getDegrees()) return true; } return false; }
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (3)
src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java(3 hunks)src/main/java/frc/trigon/robot/constants/OperatorConstants.java(1 hunks)src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java(3 hunks)
🚧 Files skipped from review as they are similar to previous changes (1)
- src/main/java/frc/trigon/robot/constants/OperatorConstants.java
🧰 Additional context used
🧬 Code graph analysis (2)
src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java (4)
src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java (1)
PhotonObjectDetectionCameraIO(16-85)src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java (1)
SimulationObjectDetectionCameraIO(17-159)src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java (1)
SimulatedGamePieceConstants(13-174)src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java (1)
ObjectDetectionCameraConstants(6-10)
src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java (5)
src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (1)
IntakeAssistCommand(22-201)src/main/java/frc/trigon/robot/constants/OperatorConstants.java (1)
OperatorConstants(12-99)src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java (1)
GeneralCommands(19-96)src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java (1)
EndEffectorCommands(10-27)src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-167)
🔇 Additional comments (3)
src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java (2)
29-29: Verify IntakeAssistCommand termination behavior.The
IntakeAssistCommandruns in parallel with the collection sequence but may not terminate when coral is collected. According to theIntakeAssistCommandimplementation, it continues untilgetClosestObjectToRobot()returns null, which may not occur immediately after collection completes (line 47:until(RobotContainer.TRANSPORTER::hasCoral)).This could cause unwanted robot movement after the coral is already collected.
Consider modifying the command to ensure
IntakeAssistCommandterminates when the transporter has coral:- ).alongWith(new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE)); + ).alongWith(new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE).until(RobotContainer.TRANSPORTER::hasCoral));
42-43: LGTM! Improved safety and simplified termination logic.The
runWhenwrapper ensures the EndEffector only begins unloading after the ArmElevator reaches the target state, preventing potential mechanical issues. Simplifying the termination condition to depend solely on the EndEffector state makes the logic clearer and more maintainable.src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java (1)
77-84: LGTM! CORAL filtering logic is correct.The filtering logic correctly excludes CORAL rotations that are within
LOLLIPOP_TOLERANCEof ALGAE rotations, preventing false positives from the "lollipop" configuration (ALGAE on top of CORAL).The O(n*m) complexity is acceptable for typical game piece counts during a match.
src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java
Show resolved
Hide resolved
Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com>
Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com>
There was a problem hiding this comment.
Actionable comments posted: 1
♻️ Duplicate comments (3)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (3)
32-32: Shared mutable state requires defensive null checks.Past reviews flagged
TARGET_SCORING_POSEfor potential NPE issues. While WPILib commands typically run on a single thread, the field can still be accessed in inconsistent states across command sequences. Lines 115-117, 137, and 166 call.get()onTARGET_SCORING_POSEwithout sufficient null guards.Apply defensive null handling at each access site:
Lines 115-117 (
canScore): Cache the value once and reuse:private static boolean canScore() { + final FlippablePose2d targetPose = TARGET_SCORING_POSE; + if (targetPose == null) return false; + return (RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.SCORE_L4.prepareState, true) || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.SCORE_L4.prepareState)) && - TARGET_SCORING_POSE != null && - Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getX()) < AutonomousConstants.REEF_RELATIVE_X_TOLERANCE_METERS && - Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getY()) < AutonomousConstants.REEF_RELATIVE_Y_TOLERANCE_METERS; + Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(targetPose.get()).getX()) < AutonomousConstants.REEF_RELATIVE_X_TOLERANCE_METERS && + Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(targetPose.get()).getY()) < AutonomousConstants.REEF_RELATIVE_Y_TOLERANCE_METERS; }Lines 135-139 (
calculateDistanceToTargetScoringPose): Add null check:private static double calculateDistanceToTargetScoringPose() { + if (TARGET_SCORING_POSE == null) { + return Double.POSITIVE_INFINITY; + } final Translation2d currentTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); final Translation2d targetTranslation = TARGET_SCORING_POSE.get().getTranslation(); return currentTranslation.getDistance(targetTranslation); }Lines 163-170 (
getAddCurrentScoringBranchToScoredBranchesCommand): Add null check:private static Command getAddCurrentScoringBranchToScoredBranchesCommand() { return new InstantCommand( () -> { + if (TARGET_SCORING_POSE == null) return; final int branchNumber = getBranchNumberFromScoringPose(TARGET_SCORING_POSE.get()); SCORED_L4S[branchNumber].set(true); } ); }Based on past review comments.
82-88: Handle "no open branches" case and guard against NPE in drive supplier.Two issues here flagged by past reviews:
Hanging on no open branches: If
calculateClosestOpenScoringPose()returnsnull(line 84), theWaitUntilCommandon line 85 will never finish, and the race partner also never ends, hanging the autonomous sequence.NPE in drive supplier: Line 86 passes
() -> TARGET_SCORING_POSEto the drive command. IfTARGET_SCORING_POSEisnull, dereferencing it will throw an NPE.Handle the null case explicitly:
public static Command getDriveToReefCommand() { return new SequentialCommandGroup( new InstantCommand(() -> TARGET_SCORING_POSE = calculateClosestOpenScoringPose()), + new ConditionalCommand( + new SequentialCommandGroup( - new WaitUntilCommand(() -> TARGET_SCORING_POSE != null).raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), - SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly() + new WaitUntilCommand(() -> TARGET_SCORING_POSE != null).raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), + SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly() + ), + Commands.none(), // or a fallback command + () -> TARGET_SCORING_POSE != null + ) ); }Based on past review comments.
172-182: Returning 0 on no match silently corrupts scoring state.Line 181 returns
0when no matching branch is found, but0is a valid branch index. This will silently mark the wrong branch as scored, corrupting the autonomous state.Return a sentinel value or throw an exception:
private static int getBranchNumberFromScoringPose(Pose2d scoringPose) { for (FieldConstants.ReefClockPosition currentClockPosition : FieldConstants.ReefClockPosition.values()) { for (FieldConstants.ReefSide currentSide : FieldConstants.ReefSide.values()) { final Pose2d reefSideScoringPose = CoralPlacingCommands.ScoringLevel.L4.calculateTargetPlacingPosition(currentClockPosition, currentSide).get(); if (reefSideScoringPose.getTranslation().getDistance(scoringPose.getTranslation()) < 0.01) return currentClockPosition.ordinal() * 2 + currentSide.ordinal(); } } - return 0; + throw new IllegalArgumentException("No matching branch found for scoring pose: " + scoringPose); }Based on past review comments.
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (2)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java(1 hunks)src/main/java/frc/trigon/robot/constants/AutonomousConstants.java(1 hunks)
🧰 Additional context used
🧬 Code graph analysis (2)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (14)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-167)src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (1)
IntakeAssistCommand(22-201)src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(27-109)src/main/java/frc/trigon/robot/constants/FieldConstants.java (1)
FieldConstants(18-121)src/main/java/frc/trigon/robot/constants/OperatorConstants.java (1)
OperatorConstants(12-99)src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java (1)
ArmElevatorCommands(15-78)src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java (1)
ArmElevatorConstants(28-366)src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java (1)
EndEffectorCommands(10-27)src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java (1)
EndEffectorConstants(18-102)src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java (1)
SwerveCommands(15-163)src/main/java/lib/utilities/flippable/FlippablePose2d.java (1)
FlippablePose2d(11-58)src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java (1)
CoralCollectionCommands(19-67)src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java (1)
GeneralCommands(19-96)src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java (1)
CoralPlacingCommands(23-254)
src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
src/main/java/frc/trigon/robot/constants/FieldConstants.java (1)
FieldConstants(18-121)
src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
Outdated
Show resolved
Hide resolved
Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com>
Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com>
Summary by CodeRabbit
New Features
Improvements
Fixes
Removals