Autonomous #18
Autonomous #18
Conversation
WalkthroughThe PR migrates PathPlannerConstants to AutonomousConstants, introduces coral-specific pose estimation, overhauls autonomous flows (reef approach, scoring, coral collection), updates auto chooser construction, and adds small subsystem APIs (arm, elevator, intake). Swerve and command logic are adjusted to use the new constants and estimators, with new mirrored/flip-aware field poses. Changes
Sequence Diagram(s)sequenceDiagram
auton->>RobotContainer: Initialize AutonomousConstants.init()
RobotContainer->>AutoBuilder: getAllAutoNames()
loop For each auto name
RobotContainer->>PathPlannerAuto: Create non-mirrored auto
RobotContainer->>PathPlannerAuto: Create mirrored auto
RobotContainer->>Chooser: addOption(name, autoCommand)
end
Note over Chooser: Default set via AutonomousConstants.DEFAULT_AUTO_NAME or "None"
sequenceDiagram
auton->>AutonomousCommands: getFloorAutonomousCommand(isRight)
AutonomousCommands->>AutonomousCommands: calculateClosestScoringPose()
par Drive to reef
AutonomousCommands->>Swerve: getDriveToReefCommand() using DRIVE_TO_REEF_CONSTRAINTS
Swerve-->>AutonomousCommands: Pose reached / distance updates
and Prepare to score
AutonomousCommands->>Elevator: set/prepare states gated by MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR
AutonomousCommands->>Arm: set states
end
AutonomousCommands->>AutonomousCommands: getFeedCoralCommand() when canFeed()
Note over AutonomousCommands: Updates SCORED_L4S upon completion
AutonomousCommands->>AutonomousCommands: getCollectCoralCommand(isRight)
par Find and approach coral
AutonomousCommands->>FieldConstants: AUTO_FIND_CORAL_POSE_LEFT/RIGHT
AutonomousCommands->>Swerve: drive to coral pose
and Intake assist
AutonomousCommands->>IntakeAssistCommand: run with CORAL_POSE_ESTIMATOR
end
Estimated code review effort🎯 4 (Complex) | ⏱️ ~75 minutes Possibly related PRs
Poem
Pre-merge checks and finishing touches❌ Failed checks (1 warning)
✅ Passed checks (2 passed)
✨ Finishing touches
🧪 Generate unit tests
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: 4
🧹 Nitpick comments (8)
src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java (1)
105-105: Make tolerance a true constant.
If not intended to be tuned at runtime, mark as final.Apply:
-static double POSITION_TOLERANCE_METERS = 0.02; +static final double POSITION_TOLERANCE_METERS = 0.02;src/main/java/frc/trigon/robot/RobotContainer.java (1)
153-156: Consider fixing the typo in the method name.The method name has a typo:
addCommandsTo_choosershould beaddCommandsToChooser(without the underscore).Apply this fix:
- private void addCommandsTo_chooser(Command... commands) { + private void addCommandsToChooser(Command... commands) {src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (2)
78-78: Method visibility change impacts encapsulation.The
calculateDistanceFromTrackedCGamePiecemethod visibility changed from private to public. WhileAutonomousCommandsdoes use this method on line 59, consider if this calculation should be exposed as a public API or if there's a better architectural approach.Consider keeping this method package-private instead of fully public to limit the API surface:
-public static Translation2d calculateDistanceFromTrackedCGamePiece() { +static Translation2d calculateDistanceFromTrackedCGamePiece() {
78-88: Typo in method name: "Gamepiece" vs "CGamePiece".The method name contains "CGamePiece" which appears to be a typo. Should be "GamePiece" for consistency.
-public static Translation2d calculateDistanceFromTrackedCGamePiece() { +public static Translation2d calculateDistanceFromTrackedGamePiece() {Also update the call site in
AutonomousCommands.javaline 59:-IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedCGamePiece) +IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece)src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (4)
26-27: Consider avoiding static imports for clarity.Using a static import for
CORAL_POSE_ESTIMATORreduces code readability. It's unclear where this comes from without checking imports.-import static frc.trigon.robot.RobotContainer.CORAL_POSE_ESTIMATOR; -Then use the fully qualified reference
RobotContainer.CORAL_POSE_ESTIMATORin the code for better clarity.
58-59: Typo in method reference matches source typo.The method reference uses
calculateDistanceFromTrackedCGamePiecewhich contains the typo mentioned earlier.Update after fixing the typo in
IntakeAssistCommand:-IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedCGamePiece) +IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece)
51-54: Complex condition logic may be confusing.The coral collection command has both
untilandunlessconditions that involve similar checks. This could lead to unexpected behavior.Consider simplifying the logic for clarity:
-) - .until(RobotContainer.INTAKE::hasCoral) - .unless(() -> RobotContainer.INTAKE.hasCoral() || RobotContainer.ARM.hasGamePiece()); +).until(() -> RobotContainer.INTAKE.hasCoral() || RobotContainer.ARM.hasGamePiece());This way, the command ends when any subsystem has a game piece, which appears to be the intent.
199-205: Magic numbers in position tolerance checks.The
canFeedmethod uses hardcoded tolerance values (0.085, 0.03) without explanation or constants.Extract these to named constants for maintainability:
+ private static final double SCORING_X_TOLERANCE_METERS = 0.085; + private static final double SCORING_Y_TOLERANCE_METERS = 0.03; + private static boolean canFeed() { return RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.SCORE_L4) && RobotContainer.ARM.atState(ArmConstants.ArmState.SCORE_L4) && TARGET_SCORING_POSE != null && - Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getX()) < 0.085 && - Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getY()) < 0.03; + Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getX()) < SCORING_X_TOLERANCE_METERS && + Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getY()) < SCORING_Y_TOLERANCE_METERS; }
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (15)
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/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/elevator/Elevator.java(1 hunks)src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java(1 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 (12)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (1)
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-262)
src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java (1)
src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(25-88)
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/commands/CommandConstants.java (1)
src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(25-88)
src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java (1)
src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(25-88)
src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java (2)
src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(25-88)src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java (1)
SwerveConstants(19-110)
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/utilities/LocalADStarAK.java (1)
LocalADStarAK(19-150)
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (13)
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-88)src/main/java/frc/trigon/robot/constants/FieldConstants.java (1)
FieldConstants(18-57)src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java (1)
ArmCommands(13-62)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-262)src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java (1)
ElevatorCommands(12-37)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)
IntakeCommands(11-45)src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java (1)
IntakeConstants(30-226)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/RobotContainer.java (4)
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(31-239)src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(25-88)src/main/java/frc/trigon/robot/constants/CameraConstants.java (1)
CameraConstants(9-20)
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 (21)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (1)
86-88: LGTM! Well-designed abstraction for autonomous game piece detection.The method provides a clean abstraction over the distance sensor's debounced boolean event, which is essential for autonomous routines to query game piece status reliably.
src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java (1)
36-36: No functional changes—formatting is fine.src/main/java/frc/trigon/robot/commands/CommandConstants.java (1)
48-48: Migration looks correct.
Using AutonomousConstants.ROBOT_CONFIG.moduleLocations here is consistent with the new constants hub.src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
26-27: Confirm PathConstraints units and reef distance
- Confirmed: PathConstraints ctor is (maxVel m/s, maxAccel m/s^2, maxAngularVel rad/s, maxAngularAccel rad/s^2); angular terms are radians — converting 450 and 900°→rad is correct.
- Verify: MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR = 2.2 m against official field drawings/CAD (or measured clearance); update the constant or add a source/comment if this is a safety margin.
src/main/java/frc/trigon/robot/constants/FieldConstants.java (1)
31-37: LGTM! Well-structured autonomous pose constants.The pose constants for coral finding are properly implemented using
FlippablePose2dfor alliance-aware positioning. The right pose correctly mirrors the left by inverting both the Y-coordinate (using field width) and rotation.src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java (2)
12-12: Consistent refactoring from PathPlannerConstants to AutonomousConstants.The import change aligns with the broader codebase migration to the renamed constants class.
71-71: LGTM! Consistent constant reference update.The drive motor slip current now correctly references
AutonomousConstants.ROBOT_CONFIGfollowing the renaming.src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java (1)
22-28: Well-designed prepare command for state transitions.The new
getPrepareForStateCommandprovides a clean way to pre-position the intake without voltage, useful for autonomous sequences where you want to move to position before engaging collection.src/main/java/frc/trigon/robot/subsystems/intake/Intake.java (1)
92-99: Good implementation of zero-voltage positioning.The
prepareForStatemethod correctly sets the target state and moves to the angle with zero voltage. This is useful for pre-positioning during autonomous without engaging the intake motor.src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java (3)
16-16: Consistent import update.Import correctly changed from PathPlannerConstants to AutonomousConstants.
35-38: LGTM! All AutonomousConstants references updated consistently.The SwerveSetpointGenerator initialization, previousSetpoint creation, drivePathPlanner feedforward scaling, and resetSetpoint method all correctly use AutonomousConstants instead of the old PathPlannerConstants.
Also applies to: 160-160, 194-194
28-29: No action needed: AutoLogOutput and Logger imports are in use
AutoLogOutput annotations are applied on getModuleStates (“Swerve/CurrentStates”) and getTargetStates (“Swerve/TargetStates”), and Logger.recordOutput calls appear in atAngle, ProfiledRotationPIDController, updateNetworkTables, etc., confirming effective use of both imports.src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java (1)
11-11: All PathPlannerConstants references successfully migrated.The import and all constant initializations (KINEMATICS, MAXIMUM_SPEED_METERS_PER_SECOND, MAXIMUM_ROTATIONAL_SPEED_RADIANS_PER_SECOND) now correctly reference AutonomousConstants.ROBOT_CONFIG.
Also applies to: 47-47, 58-59
src/main/java/frc/trigon/robot/RobotContainer.java (5)
9-9: Clean import organization for autonomous functionality.The new imports for PathPlannerAuto, AutonomousCommands, AutonomousConstants, and List are properly organized to support the autonomous features.
Also applies to: 14-14, 17-17, 31-33, 44-44
48-53: CORAL_POSE_ESTIMATOR properly initialized with camera parameter.The renamed ObjectPoseEstimator field now includes the OBJECT_DETECTION_CAMERA parameter, which is the correct 4-argument constructor form. This enables proper coral tracking for autonomous collection.
98-98: Initialization sequence updated correctly.AutonomousConstants.init() replaces the old PathPlannerConstants initialization, maintaining proper startup order.
117-117: Good practice: SuppressWarnings annotation added.The @SuppressWarnings("All") annotation acknowledges potential warnings in the auto chooser building logic, which is acceptable for dashboard configuration code.
118-151: Well-structured dynamic auto chooser with mirroring support.The implementation provides excellent flexibility:
- Dynamically discovers all available autos via AutoBuilder.getAllAutoNames()
- Creates both normal and mirrored versions for each auto
- Properly handles default selection based on AutonomousConstants.DEFAULT_AUTO_NAME
- Adds floor autonomous commands for left/right coral collection
This is a solid foundation for autonomous mode selection.
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (2)
116-121: Good use of conditional command execution.The
getOpenElevatorWhenCloseToReefCommandimplementation properly gates elevator movement based on distance to the reef, which is a good safety practice.
35-37: Clear command naming and structure.The
getFloorAutonomousCommandprovides a clear entry point with descriptive naming that includes the side parameter in the command name for dashboard clarity.src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java (1)
23-32: Incorrect — PID controllers are only used locally (no package exposure).Declared at src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java:24–30 and referenced only within that file (lines 94, 95, 131, 153–155). No usages from AutonomousCommands or other classes; make them private if you intended encapsulation.
Likely an incorrect or invalid review comment.
|
|
||
| public static Command getCoralSequenceCommand() { | ||
| return new SequentialCommandGroup( | ||
| //TODO: when scoring commands implemented CoralPlacementCommands.getLoadCoralCommand(), |
There was a problem hiding this comment.
Commented TODO blocks command sequence.
Line 91 has a commented-out command that appears critical to the sequence.
The coral loading command is essential for the scoring sequence. Without it, the robot won't have coral to score:
-//TODO: when scoring commands implemented CoralPlacementCommands.getLoadCoralCommand(),
+ // TODO: Implement CoralPlacementCommands.getLoadCoralCommand()
+ Commands.print("WARNING: CoralPlacementCommands.getLoadCoralCommand() not implemented"),This at least makes the issue visible during testing.
📝 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.
| //TODO: when scoring commands implemented CoralPlacementCommands.getLoadCoralCommand(), | |
| // TODO: Implement CoralPlacementCommands.getLoadCoralCommand() | |
| Commands.print("WARNING: CoralPlacementCommands.getLoadCoralCommand() not implemented"), |
🤖 Prompt for AI Agents
In
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
around line 91, a critical CoralPlacementCommands.getLoadCoralCommand() call is
commented out, which breaks the scoring sequence by not loading coral; restore
the command by uncommenting that line (or reinserting the equivalent call) into
the command sequence, ensure the CoralPlacementCommands class and
getLoadCoralCommand() are accessible/imported, and run compile/tests to verify
the autonomous flow includes the coral-loading step.
| public static FlippablePose2d calculateClosestScoringPose(boolean shouldOnlyCheckOpenBranches) { | ||
| final boolean[] scoredBranchesAtL4 = getScoredBranchesAtL4(); | ||
| final Pose2d currentRobotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); | ||
|
|
||
| double closestDistance = Double.POSITIVE_INFINITY; | ||
| Pose2d closestScoringPose = null; //TODO: this stuff | ||
| // for (FieldConstants.ReefClockPosition currentClockPosition : reefClockPositions) { | ||
| // for (FieldConstants.ReefSide currentSide : FieldConstants.ReefSide.values()) { | ||
| // if (shouldOnlyCheckOpenBranches && scoredBranchesAtL4[currentClockPosition.ordinal() * 2 + currentSide.ordinal()]) | ||
| // continue; | ||
| // final Pose2d reefSideScoringPose = CoralPlacingCommands.ScoringLevel.L4.calculateTargetPlacingPosition(currentClockPosition, currentSide).get(); | ||
| // final double distance = currentRobotPose.getTranslation().getDistance(reefSideScoringPose.getTranslation()); | ||
| // if (distance < closestDistance) { | ||
| // closestDistance = distance; | ||
| // if (shouldStayBehindAlgae) | ||
| // closestScoringPose = reefSideScoringPose.transformBy(new Transform2d(new Translation2d(0.1, 0), new Rotation2d())); | ||
| // else | ||
| // closestScoringPose = reefSideScoringPose; | ||
| // } | ||
| // } | ||
| // } | ||
|
|
||
| return closestScoringPose == null ? null : new FlippablePose2d(closestScoringPose, false); | ||
| } |
There was a problem hiding this comment.
Incomplete implementation with TODOs blocks functionality.
The calculateClosestScoringPose method has critical TODOs (lines 142-157) that are commented out, making the method non-functional. It will always return null, causing downstream failures.
This incomplete implementation will cause:
TARGET_SCORING_POSEto always be nullgetDriveToReefCommandto hang in theWaitUntilCommandon line 84- The autonomous sequence to never progress
Either complete the implementation or add a temporary hardcoded pose for testing:
- Pose2d closestScoringPose = null; //TODO: this stuff
+ // TODO: Implement reef scoring pose calculation
+ // For now, return a test pose to allow autonomous testing
+ Pose2d closestScoringPose = new Pose2d(2.0, 4.0, Rotation2d.fromDegrees(0));📝 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 FlippablePose2d calculateClosestScoringPose(boolean shouldOnlyCheckOpenBranches) { | |
| final boolean[] scoredBranchesAtL4 = getScoredBranchesAtL4(); | |
| final Pose2d currentRobotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); | |
| double closestDistance = Double.POSITIVE_INFINITY; | |
| Pose2d closestScoringPose = null; //TODO: this stuff | |
| // for (FieldConstants.ReefClockPosition currentClockPosition : reefClockPositions) { | |
| // for (FieldConstants.ReefSide currentSide : FieldConstants.ReefSide.values()) { | |
| // if (shouldOnlyCheckOpenBranches && scoredBranchesAtL4[currentClockPosition.ordinal() * 2 + currentSide.ordinal()]) | |
| // continue; | |
| // final Pose2d reefSideScoringPose = CoralPlacingCommands.ScoringLevel.L4.calculateTargetPlacingPosition(currentClockPosition, currentSide).get(); | |
| // final double distance = currentRobotPose.getTranslation().getDistance(reefSideScoringPose.getTranslation()); | |
| // if (distance < closestDistance) { | |
| // closestDistance = distance; | |
| // if (shouldStayBehindAlgae) | |
| // closestScoringPose = reefSideScoringPose.transformBy(new Transform2d(new Translation2d(0.1, 0), new Rotation2d())); | |
| // else | |
| // closestScoringPose = reefSideScoringPose; | |
| // } | |
| // } | |
| // } | |
| return closestScoringPose == null ? null : new FlippablePose2d(closestScoringPose, false); | |
| } | |
| public static FlippablePose2d calculateClosestScoringPose(boolean shouldOnlyCheckOpenBranches) { | |
| final boolean[] scoredBranchesAtL4 = getScoredBranchesAtL4(); | |
| final Pose2d currentRobotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); | |
| double closestDistance = Double.POSITIVE_INFINITY; | |
| // TODO: Implement reef scoring pose calculation | |
| // For now, return a test pose to allow autonomous testing | |
| Pose2d closestScoringPose = new Pose2d(2.0, 4.0, Rotation2d.fromDegrees(0)); | |
| // for (FieldConstants.ReefClockPosition currentClockPosition : reefClockPositions) { | |
| // for (FieldConstants.ReefSide currentSide : FieldConstants.ReefSide.values()) { | |
| // if (shouldOnlyCheckOpenBranches && scoredBranchesAtL4[currentClockPosition.ordinal() * 2 + currentSide.ordinal()]) | |
| // continue; | |
| // final Pose2d reefSideScoringPose = CoralPlacingCommands.ScoringLevel.L4.calculateTargetPlacingPosition(currentClockPosition, currentSide).get(); | |
| // final double distance = currentRobotPose.getTranslation().getDistance(reefSideScoringPose.getTranslation()); | |
| // if (distance < closestDistance) { | |
| // closestDistance = distance; | |
| // if (shouldStayBehindAlgae) | |
| // closestScoringPose = reefSideScoringPose.transformBy(new Transform2d(new Translation2d(0.1, 0), new Rotation2d())); | |
| // else | |
| // closestScoringPose = reefSideScoringPose; | |
| // } | |
| // } | |
| // } | |
| return closestScoringPose == null ? null : new FlippablePose2d(closestScoringPose, false); | |
| } |
🤖 Prompt for AI Agents
In
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
around lines 137 to 160, the method calculateClosestScoringPose is left
commented out and always returns null; restore a working implementation by
iterating through reefClockPositions and both FieldConstants.ReefSide values,
skip branches already scored using scoredBranchesAtL4 when
shouldOnlyCheckOpenBranches is true, call
CoralPlacingCommands.ScoringLevel.L4.calculateTargetPlacingPosition(currentClockPosition,
currentSide) and guard the Optional.get() usage (skip if absent), compute
distance to
RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(),
track the nearest Pose2d and set closestScoringPose accordingly (apply the small
backing Transform2d only if the existing shouldStayBehindAlgae flag/variable is
true), then return new FlippablePose2d(closestScoringPose, false) or null if
none found; alternatively for quick testing replace the commented block with a
single validated hardcoded Pose2d (and wrap it in FlippablePose2d) so
TARGET_SCORING_POSE is non-null.
| private static int getBranchNumberFromScoringPose(Pose2d scoringPose) { //TODO: this stuff | ||
| // 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; | ||
| } |
There was a problem hiding this comment.
Non-functional method always returns 0.
The getBranchNumberFromScoringPose method has its implementation commented out and always returns 0, which will cause incorrect scoring branch tracking.
This will cause all scores to be recorded in branch 0, breaking the branch tracking logic. Consider adding a temporary implementation or throwing an exception:
- return 0;
+ // TODO: Implement branch number calculation
+ throw new UnsupportedOperationException("getBranchNumberFromScoringPose not yet implemented");📝 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.
| private static int getBranchNumberFromScoringPose(Pose2d scoringPose) { //TODO: this stuff | |
| // 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; | |
| } | |
| private static int getBranchNumberFromScoringPose(Pose2d scoringPose) { //TODO: this stuff | |
| // 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(); | |
| // } | |
| // } | |
| // TODO: Implement branch number calculation | |
| throw new UnsupportedOperationException("getBranchNumberFromScoringPose not yet implemented"); | |
| } |
🤖 Prompt for AI Agents
In
src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java
around lines 171-181, the getBranchNumberFromScoringPose method currently always
returns 0 because its implementation is commented out; restore/implement the
lookup: iterate over FieldConstants.ReefClockPosition.values() and
ReefSide.values(), call
CoralPlacingCommands.ScoringLevel.L4.calculateTargetPlacingPosition(currentClockPosition,
currentSide) and if present compare the translations' distance to the given
scoringPose using a small tolerance (e.g., 0.01) and return
currentClockPosition.ordinal() * 2 + currentSide.ordinal() when matched; if no
match is found, throw an IllegalArgumentException with a clear message instead
of returning 0 so callers are alerted to invalid input.
| public boolean atState(ElevatorConstants.ElevatorState targetState) { | ||
| return Math.abs(getPositionMeters() - targetState.targetPositionMeters) > ElevatorConstants.POSITION_TOLERANCE_METERS; | ||
| } | ||
|
|
There was a problem hiding this comment.
atState logic is inverted (will report true when outside tolerance).
Should return true when within tolerance.
Apply:
- public boolean atState(ElevatorConstants.ElevatorState targetState) {
- return Math.abs(getPositionMeters() - targetState.targetPositionMeters) > ElevatorConstants.POSITION_TOLERANCE_METERS;
- }
+ public boolean atState(ElevatorConstants.ElevatorState targetState) {
+ return Math.abs(getPositionMeters() - targetState.targetPositionMeters)
+ <= ElevatorConstants.POSITION_TOLERANCE_METERS;
+ }📝 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 boolean atState(ElevatorConstants.ElevatorState targetState) { | |
| return Math.abs(getPositionMeters() - targetState.targetPositionMeters) > ElevatorConstants.POSITION_TOLERANCE_METERS; | |
| } | |
| public boolean atState(ElevatorConstants.ElevatorState targetState) { | |
| return Math.abs(getPositionMeters() - targetState.targetPositionMeters) | |
| <= ElevatorConstants.POSITION_TOLERANCE_METERS; | |
| } |
🤖 Prompt for AI Agents
In src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java around lines
92 to 95, the atState method currently returns true when the position difference
is greater than the tolerance (inverted); change the comparison so it returns
true when Math.abs(getPositionMeters() - targetState.targetPositionMeters) is
less than or equal to ElevatorConstants.POSITION_TOLERANCE_METERS (i.e., use <=
or < as your convention) so atState reports true when within tolerance.
There was a problem hiding this comment.
Actionable comments posted: 0
🧹 Nitpick comments (1)
src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java (1)
47-47: Fix the logging output - this appears to log PID setpoint instead of actual distance.The log entry claims to record "XDistanceFromTrackedGamePiece" but actually logs the PID controller's setpoint position, which may not represent the actual X distance from the tracked game piece. This could be misleading for debugging.
Apply this diff to log the actual X distance:
- Logger.recordOutput("GamePieceAutoDrive/XDistanceFromTrackedGamePiece", AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.getSetpoint().position); + Logger.recordOutput("GamePieceAutoDrive/XDistanceFromTrackedGamePiece", distanceFromTrackedGamePiece.getX());
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (1)
src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java(2 hunks)
🧰 Additional context used
🧬 Code graph analysis (1)
src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java (5)
src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)
AutonomousConstants(25-88)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)
🔇 Additional comments (10)
src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java (10)
9-9: LGTM!The import change from PathPlannerConstants to AutonomousConstants aligns with the broader codebase refactoring described in the AI summary.
13-13: LGTM!Adding the Logger import for output logging is appropriate for the new logging functionality.
18-18: LGTM!The migration from OBJECT_POSE_ESTIMATOR to CORAL_POSE_ESTIMATOR is consistent with the RobotContainer changes and properly references the new pose estimator for coral-specific detection.
27-27: LGTM!The conditional logic correctly uses the new CORAL_POSE_ESTIMATOR for checking object presence.
40-40: LGTM!Consistent migration to CORAL_POSE_ESTIMATOR for tracking closest objects.
53-53: LGTM!The PID controller reset properly initializes with the current distance and velocity using the new AutonomousConstants.
55-56: LGTM!Both X and Y PID controller calculations correctly use the AutonomousConstants controllers and the distance from tracked game piece.
64-64: LGTM!The distance threshold check properly uses the new AutonomousConstants value for determining when to open the intake during collection.
69-69: LGTM!Consistent use of CORAL_POSE_ESTIMATOR for angle calculation aligns with the overall migration pattern.
21-21: No remaining constructor invocations; breaking change is safe.
Started adding autonomous. Kinda need integration to be finished in order to test and finish implementation
Summary by CodeRabbit
New Features
Refactor