Arm logic #16
Arm logic #16
Conversation
|
Caution Review failedThe pull request is closed. WalkthroughExpose arm/elevator state and add cross-device safety checks; clamp commanded arm angles and elevator positions based on each other; swap several StartEndCommand usages to FunctionalCommand/ExecuteEndCommand; update constants, tuning, current limits, visualization origins, and add LOAD_CORAL handling. Changes
Sequence Diagram(s)sequenceDiagram
autonumber
actor Operator
participant Cmd as Command layer
participant Arm as Arm subsystem
participant Elev as Elevator subsystem
Operator->>Cmd: request set/prepare target (state, maybe reversed)
Cmd->>Arm: setEndEffectorState(targetState) %% init end-effector first for FunctionalCommand
Cmd->>Arm: setArmState(targetState, isReversed) %% execute sets arm target (may use subtractFrom360)
Cmd->>Arm: stop() when ended
rect rgba(222,242,255,0.55)
Note right of Arm: resolve target angle\n(apply subtractFrom360 if reversed)\nclamp via minimumArmSafeAngle(elevatorPos)
Arm->>Elev: getPositionMeters()
Arm->>Arm: compute minimumArmSafeAngle(elevatorPos)
end
rect rgba(255,243,230,0.55)
Note right of Elev: resolve target height\n(check Arm.isArmAboveSafeAngle())
Cmd->>Elev: setTargetPositionRotations(...)
Elev->>Arm: getAngle() / isArmAboveSafeAngle()
Elev->>Elev: compute minimumSafeHeight(armAngle, ARM_LENGTH_METERS)
Elev->>Elev: clamp targetRotations → command motor
end
Arm-->>Cmd: status / atAngle?
Elev-->>Cmd: status / atTargetHeight?
Estimated code review effort🎯 4 (Complex) | ⏱️ ~60 minutes Possibly related PRs
Poem
Pre-merge checks and finishing touches❌ Failed checks (1 warning)
✅ Passed checks (2 passed)
📜 Recent review detailsConfiguration used: CodeRabbit UI Review profile: CHILL Plan: Pro 📒 Files selected for processing (2)
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: 5
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/ArmConstants.java (2)
141-146: FusedCANcoder remote sensor ID likely wrongYou're using FusedCANcoder but set
FeedbackRemoteSensorIDto the TalonFX ID. It should be the CANcoder’s device ID, or you’ll fuse against the wrong source.Apply:
- config.Feedback.FeedbackRemoteSensorID = ARM_MASTER_MOTOR.getID(); + config.Feedback.FeedbackRemoteSensorID = ANGLE_ENCODER.getID();
160-170: Master current limiting not enabledFollower uses stator current limit (enabled). Master sets only
SupplyCurrentLimitwithout enabling it, so the limit won’t take effect.Prefer aligning with follower (stator), or at least enable supply:
- config.CurrentLimits.SupplyCurrentLimit = ARM_MOTOR_CURRENT_LIMIT; + // Option A: enable supply limit + config.CurrentLimits.SupplyCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = ARM_MOTOR_CURRENT_LIMIT; + // Option B (preferred for symmetry): use stator current limit like the follower + // config.CurrentLimits.StatorCurrentLimitEnable = true; + // config.CurrentLimits.StatorCurrentLimit = ARM_MOTOR_CURRENT_LIMIT;src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (1)
50-56: Telemetry units bug: rotations logged as meters
linearPositiongets rotations, and velocity is raw motor rotations/s but labeled m/s.Apply:
- log.motor("Elevator") - .linearPosition(Units.Meters.of(getPositionRotations())) - .linearVelocity(Units.MetersPerSecond.of(masterMotor.getSignal(TalonFXSignal.VELOCITY))) + log.motor("Elevator") + .linearPosition(Units.Meters.of(getPositionMeters())) + .linearVelocity(Units.MetersPerSecond.of(rotationsToMeters(masterMotor.getSignal(TalonFXSignal.VELOCITY)))) .voltage(Units.Volts.of(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)));
🧹 Nitpick comments (9)
src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java (1)
36-66: StartEnd → ExecuteEnd changes alter semantics (run every tick vs once)ExecuteEndCommand runs the setter each scheduler cycle (FunctionalCommand onExecute), increasing CAN traffic compared to one‑shot StartEnd. If the intent is a one‑shot setpoint, keep StartEnd or add an “InitializeEndCommand”.
Do you want me to provide a minimal InitializeEndCommand wrapper?
src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java (3)
13-20: Typo: “Debbuging” → “Debugging”- public static Command getDebbugingCommand() { + public static Command getDebuggingCommand() {
22-28: Use ExecuteEndCommand for consistency and identical lifecycleOther setters switched to ExecuteEndCommand. Align this one as well.
- return new StartEndCommand( + return new ExecuteEndCommand( RobotContainer.ELEVATOR::loadCoralFromElevator, RobotContainer.ELEVATOR::stop, RobotContainer.ELEVATOR ).onlyIf(() -> RobotContainer.ARM.atState(ArmConstants.ArmState.REST));
30-36: Execute-every-tick semantics: confirm intentSame note as ArmCommands: these now execute the setter each cycle. If unintended, revert to StartEnd or add an init‑only wrapper.
Also applies to: 39-43
src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java (2)
18-18: Avoid cross‑subsystem constant coupling.
Importing ArmConstants from ElevatorConstants intertwines subsystems. Consider moving shared geometry/safety values to a neutral container (e.g., SafetyConstants/GeometryConstants) to prevent initialization/order surprises.
95-96: Document safe‑zone intent and invariants.
Add brief Javadoc clarifying the frame (meters from elevator zero), why MIN=0.05, and that MAX = MIN + arm length by design.src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (3)
9-12: Direct RobotContainer dependency inside subsystem.
Consistent with project style, but consider injecting elevator access via constructor or a provider to ease testing and reduce static coupling.
86-88: Method name clarity.
isArmAboveSafeZone() checks angle >= 90°, not the elevator “safe zone.” Consider rename to isArmAtOrAboveHorizontal() to avoid confusion with elevator safe‑zone terminology.
171-173: Prefer radian-negation to “360 − θ” for normalization.
Using −θ avoids creating a 360° literal and relies on Rotation2d’s normalization.- public static Rotation2d subtractFrom360(Rotation2d angleToSubtract) { - return Rotation2d.fromDegrees(360 - angleToSubtract.getDegrees()); - } + public static Rotation2d subtractFrom360(Rotation2d angleToSubtract) { + return Rotation2d.fromRadians(-angleToSubtract.getRadians()); + }
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (7)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java(6 hunks)src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java(2 hunks)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java(7 hunks)src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java(3 hunks)src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java(2 hunks)src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java(6 hunks)src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java(1 hunks)
🧰 Additional context used
🧬 Code graph analysis (6)
src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java (3)
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-258)src/main/java/lib/commands/ExecuteEndCommand.java (1)
ExecuteEndCommand(9-21)src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-119)
src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java (2)
src/main/java/lib/commands/ExecuteEndCommand.java (1)
ExecuteEndCommand(9-21)src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-119)
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (3)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-119)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-258)src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java (1)
ElevatorConstants(28-191)
src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java (2)
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-258)src/main/java/lib/hardware/RobotHardwareStats.java (1)
RobotHardwareStats(6-78)
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (2)
src/main/java/lib/hardware/RobotHardwareStats.java (1)
RobotHardwareStats(6-78)src/main/java/lib/utilities/Conversions.java (1)
Conversions(8-258)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (3)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-119)src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java (1)
ElevatorConstants(28-191)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-258)
🔇 Additional comments (19)
src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java (1)
95-98: Visualization origin change LGTMOrigin translation update looks fine and isolated to visualization.
Please confirm the new origin aligns with the latest CAD so the field sim matches geometry.
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (8)
53-55: Gear ratio change: verify downstream config and kGsRatio 50→40 affects kV/kA feedforward scaling and soft limits. Gains here were updated; ensure any external constraints/visualization using rotations are still correct.
60-62: Non‑zero sim MM constraints LGTMSimulation defaults look reasonable.
65-65: Public ARM_LENGTH used by Elevator safetyMaking this public is consistent with cross‑subsystem safety math.
Confirm the value (0.52 m) matches the current end‑to‑end reach used by the elevator clamp.
96-100: SysId voltage step changeStep magnitude 1.5→3.0 V doubles excitation. Fine, but ensure your SysId presets/log expectations are updated.
112-115: Visualization origin change LGTM
181-183: Follower stator current limit enable LGTM
199-201: End effector stator current limit enable LGTM
238-238: SCORE_L2 at 180°: check interference envelopesThis large swing tightens elevator clamp requirements. Ensure the elevator safe‑zone math prevents collisions at this pose.
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (2)
80-83: Public getter LGTM
84-86: Safe‑zone helper LGTMName suggests “above top of zone”; behavior matches (>= max). Fine.
src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java (4)
5-8: Explicit Phoenix6 signal imports are clearer — LGTM.
Good move replacing the wildcard import for readability and stability.
78-79: No functional change detected here.
Spacing-only; nothing to do.
115-121: PID/FF constants: assuming whitespace‑only.
No action.
168-171: Verify new setpoints won’t violate arm/elevator interference.
LOAD_CORAL added and SCORE_L1/L2 moved to 0.203 m. Please confirm these are safe with the new arm clamp logic and field geometry. If unverified, gate behind a “safe move” check or put under a tuning flag.Would you like a small sim check that sweeps elevator/arm to each state and logs clamp activations?
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (4)
102-104: Angle diff update for reversed states — LGTM.
The 360−θ formulation matches the new reversed targeting behavior.
113-115: Public angle accessor — LGTM.
Centralizes angle reads on the CANcoder signal. Good.
127-132: Reversed target state selection — LGTM.
Passing subtractFrom360(targetAngle) into the unified setter keeps logic consistent.
176-178: Visualization z-translation tied to elevator height — LGTM.
Matches new cross‑subsystem visualization strategy.
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 (3)
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (3)
34-38: Fix duplicate CAN IDs (ARM_MASTER_MOTOR_ID and ANGLE_ENCODER_ID are both 13).CTRE devices on the same bus must have unique IDs. This will cause CAN faults and undefined behavior.
Apply a unique ID for the CANcoder (example below; pick the real one you wired):
- ANGLE_ENCODER_ID = 13, + ANGLE_ENCODER_ID = 33,Also ensure all references (including FeedbackRemoteSensorID) use the CANcoder’s ID.
141-144: Use the CANcoder’s device ID for FusedCANcoder remote sensor.You’re passing the motor ID, which breaks fused-sensor feedback.
Apply:
- config.Feedback.FeedbackRemoteSensorID = ARM_MASTER_MOTOR.getID(); + config.Feedback.FeedbackRemoteSensorID = ANGLE_ENCODER.getID();
160-170: Master current limit isn’t enabled; unify with follower/end-effector.Only SupplyCurrentLimit value is set, but not enabled. Result: no effective limiting. Mirror stator limiting like the others (and optionally enable supply).
- config.CurrentLimits.SupplyCurrentLimit = ARM_MOTOR_CURRENT_LIMIT; + // Prefer stator limiting for fast protection (match follower/end-effector) + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.StatorCurrentLimit = ARM_MOTOR_CURRENT_LIMIT; + // Optional: also enable supply limiting if desired + // config.CurrentLimits.SupplyCurrentLimitEnable = true; + // config.CurrentLimits.SupplyCurrentLimit = ARM_MOTOR_CURRENT_LIMIT;
🧹 Nitpick comments (1)
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
156-159: Use the defined jerk constant to avoid duplication.- config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10; + config.MotionMagic.MotionMagicJerk = ARM_DEFAULT_MAXIMUM_JERK;
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (2)
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java(7 hunks)src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java(3 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/subsystems/arm/ArmConstants.java (2)
src/main/java/lib/hardware/RobotHardwareStats.java (1)
RobotHardwareStats(6-78)src/main/java/lib/utilities/Conversions.java (1)
Conversions(8-258)
🔇 Additional comments (8)
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (8)
181-183: Follower stator current limit: LGTM.Matches the intended protection model.
199-201: Confirm 80 A stator limit is safe for the end-effector motor.Double-check against motor specs and breaker sizing.
145-151: Controller gains: verify stability and units.Values look sim-tuned (kP=100, kD=1, kG=0.1117). Ensure real-robot gains are set (current code zeros them) or gated behind a deploy-time profile.
60-62: Motion Magic constraints: confirm units after gear-ratio changes.Cruise velocity/accel are in sensor units (rot/s, rot/s^2) post RotorToSensorRatio. With ARM_GEAR_RATIO=40 and FusedCANcoder, validate these numbers match actual arm dynamics.
53-53: Gear ratio change to 40: verify mechanical ratio and cancoder mounting.If the CANcoder measures joint angle, RotorToSensorRatio = gear ratio is correct; otherwise adjust.
65-65: ARM_LENGTH_METERS now public: OK—ensure dependent constants updated.Confirm ElevatorConstants and any reach/safe-zone math have been recalibrated to 0.52 m.
113-115: Visualization origin update: LGTM.Matches shorter arm length; no issues spotted.
238-238: SCORE_L2 at 180°: verify mechanical limits and cable routing.Full inversion can strain wiring and exceed soft limits. Ensure clamping/wrapping logic prevents unsafe travel.
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/subsystems/elevator/Elevator.java (1)
52-55: SysId log units are wrong (rotations used as meters); convert position and velocity to meterslinearPosition should use getPositionMeters(). linearVelocity should be rotations→meters as well.
Apply:
- log.motor("Elevator") - .linearPosition(Units.Meters.of(getPositionRotations())) - .linearVelocity(Units.MetersPerSecond.of(masterMotor.getSignal(TalonFXSignal.VELOCITY))) + log.motor("Elevator") + .linearPosition(Units.Meters.of(getPositionMeters())) + .linearVelocity(Units.MetersPerSecond.of(rotationsToMeters(masterMotor.getSignal(TalonFXSignal.VELOCITY)))) .voltage(Units.Volts.of(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)));
♻️ Duplicate comments (1)
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (1)
100-104: Use Rotation2d.getCos() and clamp min height to safe-zone boundsUse the WPILib Rotation2d API (per team learning) and clamp the computed minimum to [MIN_SAFE, MAX_SAFE] to avoid negatives/overshoot for angles >90° or wrap cases. Degrees vs radians issue is fixed now; this keeps it robust.
Apply:
- double minimumSafeHeightMeters = RobotContainer.ARM.isArmAboveSafeZone() ? 0 : Math.cos(RobotContainer.ARM.getAngle().getRadians()) * ArmConstants.ARM_LENGTH_METERS + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; - double minimumSafeHeightRotations = metersToRotations(minimumSafeHeightMeters); - masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, minimumSafeHeightRotations))); + final var armAngle = RobotContainer.ARM.getAngle(); + final double minCalculatedMeters = + armAngle.getCos() * ArmConstants.ARM_LENGTH_METERS + + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; + final double minimumSafeHeightMeters = RobotContainer.ARM.isArmAboveSafeZone() + ? 0 + : MathUtil.clamp( + minCalculatedMeters, + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS, + ElevatorConstants.MAXIMUM_ELEVATOR_SAFE_ZONE_METERS); + final double minimumSafeHeightRotations = metersToRotations(minimumSafeHeightMeters); + masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, minimumSafeHeightRotations)));Add import:
+import edu.wpi.first.math.MathUtil;
🧹 Nitpick comments (2)
src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java (2)
22-28: StartEndCommand here never finishes; prefer a one-shot command (Instant) or add a finish conditionAs written, the command will run indefinitely once scheduled and hold the requirement until interrupted. If the intent is to just set the target once and let the default command maintain it, use InstantCommand (or add an explicit .until(...) if you need it to hold briefly).
Apply:
- return new StartEndCommand( - RobotContainer.ELEVATOR::prepareLoadCoral, - RobotContainer.ELEVATOR::stop, - RobotContainer.ELEVATOR - ).onlyIf(() -> RobotContainer.ARM.atState(ArmConstants.ArmState.REST)); + return new InstantCommand( + RobotContainer.ELEVATOR::prepareLoadCoral, + RobotContainer.ELEVATOR + ).onlyIf(() -> RobotContainer.ARM.atState(ArmConstants.ArmState.REST));Add import:
+import edu.wpi.first.wpilibj2.command.InstantCommand;
30-36: ExecuteEndCommand repeatedly sets targets every cycle; consider StartEnd/Instant for idempotent settersSetting the same target on every execute is unnecessary noise. Prefer StartEndCommand (set on init, stop on end) or InstantCommand (set once, finish) to reduce bus chatter and avoid needless writes. Behavior stays the same from the robot’s perspective.
Also applies to: 38-44
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (3)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java(6 hunks)src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java(3 hunks)src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java(2 hunks)
🚧 Files skipped from review as they are similar to previous changes (1)
- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java
🧰 Additional context used
🧠 Learnings (1)
📚 Learning: 2025-09-11T08:52:03.279Z
Learnt from: levyishai
PR: Programming-TRIGON/RobotCodeOffSeason2025#11
File: src/main/java/frc/trigon/robot/subsystems/arm/Arm.java:117-124
Timestamp: 2025-09-11T08:52:03.279Z
Learning: When working with Rotation2d objects in WPILib, prefer using the built-in trigonometric methods like Rotation2d.cos() and Rotation2d.sin() instead of Math.cos(rotation.getRadians()) and Math.sin(rotation.getRadians()). This is more readable and semantically appropriate.
Applied to files:
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java
🧬 Code graph analysis (2)
src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java (3)
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-258)src/main/java/lib/commands/ExecuteEndCommand.java (1)
ExecuteEndCommand(9-21)src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-119)
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (4)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-119)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-258)src/main/java/lib/utilities/Conversions.java (1)
Conversions(8-258)src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java (1)
ElevatorConstants(28-191)
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java
Outdated
Show resolved
Hide resolved
about the outside diff comment the metersPerSecond already does all the conversions so no need for metersToRotations. |
| public static Rotation2d subtractFrom360(Rotation2d angleToSubtract) { | ||
| return Rotation2d.fromDegrees(360 - angleToSubtract.getDegrees()); | ||
| } | ||
|
|
There was a problem hiding this comment.
I really don't like this function. It's just simple math and shouldn't really need to be its own function.
Also, look at the constants in the Rotation2d class. This is overcomplicated
There was a problem hiding this comment.
the rotation 2d stuff always gives me a rotation between 180 and -180 or -pie/2 to pie/2(for radians) this is the cleanest method i could think of
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java
Outdated
Show resolved
Hide resolved
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java
Outdated
Show resolved
Hide resolved
src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.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: 1
♻️ Duplicate comments (1)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (1)
152-153: Fix confirmed: reversed prepareForState now sets the angle.
This addresses the earlier bug where the computed angle was discarded.
🧹 Nitpick comments (2)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (2)
86-88: Name/doc mismatch: this checks angle≥90°, not “safe zone.”
Consider a clearer name (e.g., isArmPastVertical) or a short Javadoc to avoid confusion with the elevator “safe zone.”
171-173: Rotation2d loses the “360−θ” distinction; clarify intent or return rotations.
360−θ expressed as Rotation2d is equivalent to −θ. If you truly need the “other branch,” consider returning a double rotations value (0..1) and using it directly in withPosition, or at least document this behavior.
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (1)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java(6 hunks)
🧰 Additional context used
🧠 Learnings (1)
📚 Learning: 2025-09-11T08:52:03.291Z
Learnt from: levyishai
PR: Programming-TRIGON/RobotCodeOffSeason2025#11
File: src/main/java/frc/trigon/robot/subsystems/arm/Arm.java:117-124
Timestamp: 2025-09-11T08:52:03.291Z
Learning: When working with Rotation2d objects in WPILib, prefer using the built-in trigonometric methods like Rotation2d.cos() and Rotation2d.sin() instead of Math.cos(rotation.getRadians()) and Math.sin(rotation.getRadians()). This is more readable and semantically appropriate.
Applied to files:
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java
🧬 Code graph analysis (1)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (3)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-119)src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java (1)
ElevatorConstants(28-191)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-258)
🔇 Additional comments (5)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (5)
9-11: LGTM: new imports are appropriate and used.
102-102: Verify reversed-angle semantics with Rotation2d normalization.
Rotation2d normalizes to [-180,180); subtractFrom360Degrees(target) may become negative (e.g., 350°→-10°). This is fine for difference math, but later setpoints use getRotations(), which will also normalize and can unintentionally clamp with Math.max(...) against a nonnegative safe angle. Confirm this still commands the intended “reversed” pose.
113-115: LGTM: angle read path is correct.
CANcoder rotations → Rotation2d is appropriate.
129-129: LGTM: reversed target selection path.
Passing the reversed target angle and voltage together is clean.
177-177: LGTM: viz translated by elevator height.
Improves realism of the mechanism view.
There was a problem hiding this comment.
Actionable comments posted: 0
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/subsystems/arm/ArmConstants.java (1)
161-161: Master motor current limit not enabled.You set SupplyCurrentLimit but did not enable it; config won’t take effect.
- config.CurrentLimits.SupplyCurrentLimit = ARM_MOTOR_CURRENT_LIMIT; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = ARM_MOTOR_CURRENT_LIMIT;Optional: mirror follower and use StatorCurrentLimit on the master for torque limiting consistency.
♻️ Duplicate comments (3)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (2)
152-154: Fix applied: reversed prepareForState now commands the angle.This resolves the earlier bug where the computed reversed angle was discarded.
159-164: Critical: acos domain/NaN risk; clamp and guard the setpoint.cosMinimumAngle can exit [-1,1] leading to NaN and an invalid MotionMagic setpoint. Clamp and check finiteness before sending.
+import edu.wpi.first.math.MathUtil; @@ - final double distanceFromSafeZone = RobotContainer.ELEVATOR.getPositionMeters() - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; - final double cosMinimumAngle = distanceFromSafeZone / ArmConstants.ARM_LENGTH_METERS; - final Rotation2d minimumSafeAngle = Rotation2d.fromRadians(RobotContainer.ELEVATOR.isElevatorAboveSafeZone() ? 0 : Math.acos(cosMinimumAngle)); - armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), minimumSafeAngle.getRotations()))); + final double elevatorHeight = RobotContainer.ELEVATOR.getPositionMeters(); + final boolean aboveSafe = RobotContainer.ELEVATOR.isElevatorAboveSafeZone(); + final double heightFromSafe = elevatorHeight - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; + final double cosMin = MathUtil.clamp(heightFromSafe / ArmConstants.ARM_LENGTH_METERS, -1.0, 1.0); + final Rotation2d minimumSafeAngle = aboveSafe + ? Rotation2d.fromRadians(0) + : Rotation2d.fromRadians(Math.acos(cosMin)); + final double commandedRot = Math.max(targetAngle.getRotations(), minimumSafeAngle.getRotations()); + if (Double.isFinite(commandedRot)) { + armMasterMotor.setControl(positionRequest.withPosition(commandedRot)); + }src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
96-100: Fix compile error: use Units.Seconds (plural).The last argument uses Units.Second which doesn’t compile; switch to Units.Seconds.
static final SysIdRoutine.Config ARM_SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(1.5).per(Units.Seconds), Units.Volts.of(3), - Units.Second.of(1000) + Units.Seconds.of(1000) );
🧹 Nitpick comments (2)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (2)
86-88: Name clarifier (optional): isArmAboveSafeZone is angle‑based.Method checks angle ≥ 90°, not elevator height. Consider renaming to isBeyondSafeAngle() for clarity, or add Javadoc.
171-173: Optional: simplify 360°−θ helper.360°−θ ≡ −θ mod 360. Consider:
- return Rotation2d.fromRadians(-angleToSubtract.getRadians());
Add a brief Javadoc noting intended range to avoid normalization confusion.
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (2)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java(6 hunks)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java(7 hunks)
🧰 Additional context used
🧠 Learnings (1)
📚 Learning: 2025-09-11T08:52:03.291Z
Learnt from: levyishai
PR: Programming-TRIGON/RobotCodeOffSeason2025#11
File: src/main/java/frc/trigon/robot/subsystems/arm/Arm.java:117-124
Timestamp: 2025-09-11T08:52:03.291Z
Learning: When working with Rotation2d objects in WPILib, prefer using the built-in trigonometric methods like Rotation2d.cos() and Rotation2d.sin() instead of Math.cos(rotation.getRadians()) and Math.sin(rotation.getRadians()). This is more readable and semantically appropriate.
Applied to files:
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java
🧬 Code graph analysis (2)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (4)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-119)src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java (1)
MotorSubsystem(25-173)src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java (1)
ElevatorConstants(28-191)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-259)
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (2)
src/main/java/lib/hardware/RobotHardwareStats.java (1)
RobotHardwareStats(6-78)src/main/java/lib/utilities/Conversions.java (1)
Conversions(8-258)
🔇 Additional comments (13)
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (9)
60-62: LGTM — sim Motion Magic constraints look reasonable.
65-65: LGTM — expose ARM_LENGTH_METERS for cross‑subsystem safety math.
112-115: LGTM — updated visualization origin is fine.
117-117: LGTM — MAXIMUM_ARM_SAFE_ZONE = 90° matches usage in Arm.isArmAboveSafeZone().
146-153: LGTM — sim gains updated coherently with Arm_Cosine gravity feedforward.
182-184: LGTM — follower stator current limit enabled.
200-201: LGTM — end effector stator current limit enabled.
239-239: Sanity‑check SCORE_L2 = 180°.Just confirm this target doesn’t violate mechanical limits or collide given the elevator safe‑zone constraints.
53-53: Confirm FusedCANcoder ratio fields & MotionMagic unitsSet both RotorToSensorRatio (rotor↔CANcoder) and SensorToMechanismRatio (sensor↔mechanism/gearbox); MotionMagic cruise/accel expect mechanism rotations/sec and rotations/sec^2. Ensure ARM_GEAR_RATIO = 40 is applied as SensorToMechanismRatio (or that RotorToSensorRatio is set correctly if the CANcoder is mounted on the rotor) when configuring FeedbackSensorSource=FusedCANcoder in src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java:53.
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (4)
100-104: LGTM — reversed atTargetAngle uses subtractFrom360Degrees correctly.
113-115: LGTM — getAngle reads CANcoder rotations via Rotation2d.
127-132: LGTM — reversed state targeting passes 360°−θ.
176-179: LGTM — visualization translates with elevator height and uses arm pitch.
There was a problem hiding this comment.
Actionable comments posted: 0
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/subsystems/elevator/Elevator.java (1)
52-56: SysId logs use wrong units (rotations labeled as meters).Pass meters for position and meters/sec for velocity.
log.motor("Elevator") - .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)))) .voltage(Units.Volts.of(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)));
♻️ Duplicate comments (4)
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
96-100: Fix SysId Units.Second typo (won’t compile).Use Units.Seconds.
static final SysIdRoutine.Config ARM_SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(1.5).per(Units.Seconds), Units.Volts.of(3), - Units.Second.of(1000) + Units.Seconds.of(1000) );src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (2)
92-96: prepareLoadCoral bypasses safety clamp. Route through setter.Directly commanding position skips the interlock in setTargetPositionRotations.
void prepareLoadCoral() { this.targetState = ElevatorConstants.ElevatorState.LOAD_CORAL; scalePositionRequestSpeed(targetState.speedScalar); - masterMotor.setControl(positionRequest.withPosition(metersToRotations(ElevatorConstants.ElevatorState.LOAD_CORAL.targetPositionMeters))); + setTargetPositionRotations(metersToRotations(ElevatorConstants.ElevatorState.LOAD_CORAL.targetPositionMeters)); }To ensure there are no other bypasses, run:
#!/bin/bash # Find direct position writes that skip clamping rg -n -C2 'setControl\(positionRequest\.withPosition\(' --type=java
104-109: Clamp minimum safe height and use Rotation2d API.Without clamping, cos>90° yields negatives; also prefer Rotation2d.getCos().
- final double elevatorHeightFromArm = Math.cos(RobotContainer.ARM.getAngle().getRadians()) * ArmConstants.ARM_LENGTH_METERS; - final double minimumSafeHeightMeters = (RobotContainer.ARM.isArmAboveSafeZone() ? 0 : elevatorHeightFromArm) + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; - final double minimumSafeHeightRotations = metersToRotations(minimumSafeHeightMeters); - masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, minimumSafeHeightRotations))); + final var armAngle = RobotContainer.ARM.getAngle(); + final double minCalculatedMeters = + armAngle.getCos() * ArmConstants.ARM_LENGTH_METERS + + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; + final double minimumSafeHeightMeters = RobotContainer.ARM.isArmAboveSafeZone() + ? 0 + : edu.wpi.first.math.MathUtil.clamp( + minCalculatedMeters, + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS, + ElevatorConstants.MAXIMUM_ELEVATOR_SAFE_ZONE_METERS); + final double minimumSafeHeightRotations = metersToRotations(minimumSafeHeightMeters); + masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, minimumSafeHeightRotations)));If not already imported, add:
+import edu.wpi.first.math.MathUtil;src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (1)
159-167: Safe‑angle math: base on MIN safe zone and clamp to [-1,1].Using MAX as baseline over‑restricts motion; clamp domain to avoid NaN and compute smoothly from MIN→MAX.
- final boolean isAboveSafeZone = RobotContainer.ELEVATOR.isElevatorAboveSafeZone(); - final double heightFromSafeZone = RobotContainer.ELEVATOR.getElevatorHeightFromSafeZone(); - final double cosOfMinimumSafeAngle = MathUtil.clamp(heightFromSafeZone / ArmConstants.ARM_LENGTH_METERS, 0, 1); - final Rotation2d minSafeAngle = isAboveSafeZone - ? Rotation2d.fromRadians(0) - : Rotation2d.fromRadians(Math.acos(cosOfMinimumSafeAngle)); + final boolean isAboveSafeZone = RobotContainer.ELEVATOR.isElevatorAboveSafeZone(); + final double heightFromMinSafeZone = + RobotContainer.ELEVATOR.getPositionMeters() + - frc.trigon.robot.subsystems.elevator.ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; + final double cosOfMinimumSafeAngle = + MathUtil.clamp(heightFromMinSafeZone / ArmConstants.ARM_LENGTH_METERS, -1.0, 1.0); + final Rotation2d minSafeAngle = isAboveSafeZone + ? Rotation2d.fromRadians(0) + : Rotation2d.fromRadians(Math.acos(cosOfMinimumSafeAngle)); armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), minSafeAngle.getRotations())));If preferred, add explicit import and use it:
+import frc.trigon.robot.subsystems.elevator.ElevatorConstants;
🧹 Nitpick comments (3)
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
184-186: Inconsistent current‑limit type between master/follower.Master uses SupplyCurrentLimit (Line 163) while follower uses StatorCurrentLimit here. Verify this is intentional; otherwise align both for predictable limits.
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (1)
88-90: Method name is misleading.getElevatorHeightFromSafeZone returns “above MAX safe zone,” not “from (MIN) safe zone.” Consider renaming to getHeightAboveMaxSafeZone or document its semantics.
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (1)
175-177: Minor: avoid magic 360.Consider a constant FULL_ROTATION = Rotation2d.fromDegrees(360) to avoid ad‑hoc literals.
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (4)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java(6 hunks)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java(7 hunks)src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java(3 hunks)src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java(6 hunks)
🚧 Files skipped from review as they are similar to previous changes (1)
- src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java
🧰 Additional context used
🧠 Learnings (1)
📚 Learning: 2025-09-11T08:52:03.291Z
Learnt from: levyishai
PR: Programming-TRIGON/RobotCodeOffSeason2025#11
File: src/main/java/frc/trigon/robot/subsystems/arm/Arm.java:117-124
Timestamp: 2025-09-11T08:52:03.291Z
Learning: When working with Rotation2d objects in WPILib, prefer using the built-in trigonometric methods like Rotation2d.cos() and Rotation2d.sin() instead of Math.cos(rotation.getRadians()) and Math.sin(rotation.getRadians()). This is more readable and semantically appropriate.
Applied to files:
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.javasrc/main/java/frc/trigon/robot/subsystems/arm/Arm.java
🧬 Code graph analysis (3)
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (4)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-119)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-261)src/main/java/lib/utilities/Conversions.java (1)
Conversions(8-258)src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java (1)
ElevatorConstants(28-198)
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (2)
src/main/java/lib/hardware/RobotHardwareStats.java (1)
RobotHardwareStats(6-78)src/main/java/lib/utilities/Conversions.java (1)
Conversions(8-258)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (2)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-119)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-261)
🔇 Additional comments (4)
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
116-120: Safe‑zone constant addition looks good.MAXIMUM_ARM_SAFE_ZONE = 90° is clear and self‑documented.
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (1)
80-82: Public position accessor looks good.Simple, consistent with rotationsToMeters helper.
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (2)
86-88: Safe‑zone check addition is clear.Using Rotation2d degrees for comparison is fine given the constant is defined in degrees.
113-115: Angle accessor LGTM.Direct CANcoder rotations wrapped as Rotation2d keeps callers clean.
There was a problem hiding this comment.
Actionable comments posted: 2
♻️ Duplicate comments (1)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (1)
166-169: atAngle() can never be true when tolerance is 0°; use <=.Current check rejects exact equality; with 0° tolerance it’s always false.
- return currentToTargetAngleDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); + return currentToTargetAngleDifferenceDegrees <= ArmConstants.ANGLE_TOLERANCE.getDegrees();
🧹 Nitpick comments (6)
src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java (1)
37-42: Avoid re-writing end-effector voltage every cycle; update only the arm.Use setArmState(...) in execute to reduce bus writes while still recomputing the safe angle each loop.
- return new FunctionalCommand( - () -> RobotContainer.ARM.setEndEffectorState(targetState), - () -> RobotContainer.ARM.setTargetState(targetState, isStateReversed), - interrupted -> RobotContainer.ARM.stop(), - () -> false, - RobotContainer.ARM - ); + return new FunctionalCommand( + () -> RobotContainer.ARM.setEndEffectorState(targetState), + () -> RobotContainer.ARM.setArmState(targetState, isStateReversed), + interrupted -> RobotContainer.ARM.stop(), + () -> false, + RobotContainer.ARM + );src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (3)
53-59: Consider logging mechanism velocity, not rotor velocity.TalonFX VELOCITY is rotor RPS; if you want arm joint speed, convert by gear ratio or read the fused mechanism source.
86-88: Name clarity nit: “isArmAboveSafeAngle” -> “isAtOrAboveSafeAngle”.Improves readability of the boundary condition.
171-173: Nit: avoid magic 360; use Rotation2d ops or a constant.Minor readability tweak.
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (2)
119-120: Set a small non‑zero angle tolerance for robustness.Zero tolerance plus sensor noise makes “at state” flaky; 1–2° is typical.
- static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0); + static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(1.5);
57-57: Simplify no-op offset expression.Expression reduces to −ANGLE_ENCODER_GRAVITY_OFFSET; cleaner as below.
- static final double POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 : 0 + Conversions.degreesToRotations(0) - ANGLE_ENCODER_GRAVITY_OFFSET; + static final double POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 : -ANGLE_ENCODER_GRAVITY_OFFSET;
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (4)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.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(8 hunks)src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java(3 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
🧠 Learnings (1)
📚 Learning: 2025-09-11T08:52:03.291Z
Learnt from: levyishai
PR: Programming-TRIGON/RobotCodeOffSeason2025#11
File: src/main/java/frc/trigon/robot/subsystems/arm/Arm.java:117-124
Timestamp: 2025-09-11T08:52:03.291Z
Learning: When working with Rotation2d objects in WPILib, prefer using the built-in trigonometric methods like Rotation2d.cos() and Rotation2d.sin() instead of Math.cos(rotation.getRadians()) and Math.sin(rotation.getRadians()). This is more readable and semantically appropriate.
Applied to files:
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java
🧬 Code graph analysis (3)
src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java (2)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-119)src/main/java/lib/commands/ExecuteEndCommand.java (1)
ExecuteEndCommand(9-21)
src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (2)
src/main/java/lib/hardware/RobotHardwareStats.java (1)
RobotHardwareStats(6-78)src/main/java/lib/utilities/Conversions.java (1)
Conversions(8-258)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (2)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-119)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-262)
🔇 Additional comments (3)
src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java (2)
47-51: LGTM: onEnd added; prevents NPE and ensures stop on interruption.
56-61: Confirmed — ExecuteEndCommand never finishes; current usages are safe.ExecuteEndCommand uses a false isFinished (never terminates). getPrepareForStateCommand is used with .until(RobotContainer.ARM::atTargetAngle) in src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java, and getSetTargetStateCommand is set as the ARM default in src/main/java/frc/trigon/robot/RobotContainer.java — no onTrue bindings found.
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (1)
100-102: Confirm angle reference frame is consistent across Arm/Elevator.getAngle() returns raw CANcoder rotations; ensure Elevator’s safe‑zone math expects this frame (no gravity offset). If a gravity‑offset frame is intended, unify here.
There was a problem hiding this comment.
Actionable comments posted: 2
♻️ Duplicate comments (3)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (3)
168-171: Equality should count as “at angle”; 0° tolerance is currently always false.Use <= so exact matches pass; consider a small non‑zero tolerance in constants.
- return currentToTargetAngleDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); + return currentToTargetAngleDifferenceDegrees <= ArmConstants.ANGLE_TOLERANCE.getDegrees();
151-152: Critical: clamp uses getRotations() and breaks for targets >180° (wrap-around).Rotation2d wraps 270° to −90° rotations; Math.max picks the wrong side. Compare in [0,360) then map back to motor domain.
- armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), minimumArmSafeAngle().getRotations()))); + final Rotation2d minSafe = minimumArmSafeAngle(); + final double targetDeg = MathUtil.inputModulus(targetAngle.getDegrees(), 0.0, 360.0); + final double minDeg = MathUtil.inputModulus(minSafe.getDegrees(), 0.0, 360.0); + final double commandedDeg = Math.max(targetDeg, minDeg); + // Map back to motor's [-180,180) representation and send as rotations. + final double motorDeg = MathUtil.inputModulus(commandedDeg, -180.0, 180.0); + armMasterMotor.setControl(positionRequest.withPosition(motorDeg / 360.0));
154-161: Allow full geometry and prevent NaN: clamp to [-1,1], not [0,1].When the elevator is below the safe zone, the ratio is negative and the required minimum angle can exceed 90°. Clamping to [0,1] forces 90° and can be unsafe. Also minor: use fromDegrees(0).
- final double cosOfMinimumSafeAngle = MathUtil.clamp(heightFromSafeZone / ArmConstants.ARM_LENGTH_METERS, 0, 1); - return isElevatorAboveSafeZone - ? Rotation2d.fromRadians(0) - : Rotation2d.fromRadians(Math.acos(cosOfMinimumSafeAngle)); + final double cosOfMinimumSafeAngle = + MathUtil.clamp(heightFromSafeZone / ArmConstants.ARM_LENGTH_METERS, -1.0, 1.0); + return isElevatorAboveSafeZone + ? Rotation2d.fromDegrees(0) + : Rotation2d.fromRadians(Math.acos(cosOfMinimumSafeAngle));
🧹 Nitpick comments (1)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (1)
173-175: Normalize 360−x into [0,360).Avoid negative results if angleToSubtract >360 or edge cases.
- return Rotation2d.fromDegrees(360 - angleToSubtract.getDegrees()); + return Rotation2d.fromDegrees( + MathUtil.inputModulus(360.0 - angleToSubtract.getDegrees(), 0.0, 360.0) + );
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (1)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java(4 hunks)
🧰 Additional context used
🧠 Learnings (1)
📚 Learning: 2025-09-11T08:52:03.291Z
Learnt from: levyishai
PR: Programming-TRIGON/RobotCodeOffSeason2025#11
File: src/main/java/frc/trigon/robot/subsystems/arm/Arm.java:117-124
Timestamp: 2025-09-11T08:52:03.291Z
Learning: When working with Rotation2d objects in WPILib, prefer using the built-in trigonometric methods like Rotation2d.cos() and Rotation2d.sin() instead of Math.cos(rotation.getRadians()) and Math.sin(rotation.getRadians()). This is more readable and semantically appropriate.
Applied to files:
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java
🧬 Code graph analysis (1)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (2)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-119)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-262)
🔇 Additional comments (3)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (3)
90-98: Reversed-state predicate looks good.Using subtractFrom360Degrees(...) with atAngle(...) correctly reuses the tolerance logic.
131-148: Good separation of concerns for arm vs end‑effector.setArmState(...) and setEndEffectorState(...) decouple angle/voltage targets cleanly.
177-183: Visualization Z-translation by elevator height looks correct.No issues spotted.
| public boolean isArmAboveSafeAngle() { | ||
| return getAngle().getDegrees() >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees(); | ||
| } |
There was a problem hiding this comment.
Fix wrap-around in isArmAboveSafeAngle (Rotation2d.getDegrees is [-180,180)).
Angles >180° read as negative and will fail this check. Normalize before comparing.
- return getAngle().getDegrees() >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees();
+ final double angleDeg = MathUtil.inputModulus(getAngle().getDegrees(), 0.0, 360.0);
+ return angleDeg >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees();📝 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 isArmAboveSafeAngle() { | |
| return getAngle().getDegrees() >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees(); | |
| } | |
| public boolean isArmAboveSafeAngle() { | |
| final double angleDeg = MathUtil.inputModulus(getAngle().getDegrees(), 0.0, 360.0); | |
| return angleDeg >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees(); | |
| } |
🤖 Prompt for AI Agents
In src/main/java/frc/trigon/robot/subsystems/arm/Arm.java around lines 86 to 88,
isArmAboveSafeAngle uses Rotation2d.getDegrees() which returns values in
[-180,180) so angles >180° read negative and break the comparison; normalize the
measured angle (and the constant) to a consistent [0,360) range before comparing
(e.g., if deg < 0 add 360), then compare the normalized measured angle against
the normalized MAXIMUM_ARM_SAFE_ANGLE to determine if the arm is above the safe
angle.
There was a problem hiding this comment.
anytime the arm is above 90 degrees itll ignore the logic so it doesnt matter
There was a problem hiding this comment.
@ShmayaR Thank you for the clarification about the system behavior above 90°.
I understand that your current system logic bypasses this safety check when the arm is above 90°, which would indeed make the wrap-around issue less critical in practice. However, I'd like to point out a couple of considerations:
-
Latent bug risk: If the system behavior ever changes or if the arm physically operates in the 180°+ range, angles like 270° would read as -90° and incorrectly return
falsewhen they should returntrue. -
Code robustness: Even if the current logic prevents problematic scenarios, defensive programming would make the code more maintainable and less prone to future bugs.
Could you clarify what specifically happens when the arm reaches angles above 180°? Does the system have hard stops, or are there other safety mechanisms that prevent operation in that range?
The fix would be minimal and wouldn't impact performance, but I understand if it's not a priority given your current system constraints.
🐰💭
✏️ Learnings added
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.
Note: Learnings are effective only in the context of similar code segments. To apply general coding guidelines, please configure review instructions. You can manage existing learnings in the app.
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/subsystems/elevator/Elevator.java (1)
52-55: Enforce clamp — stop direct MotionMagic writes that bypass minimum-safe height
- Elevator: src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java:96 contains masterMotor.setControl(positionRequest.withPosition(...)) which bypasses the clamp used in setTargetPositionRotations (masterMotor.setControl(...Math.max(...)) at :103). Replace the LOAD_CORAL branch with setTargetPositionRotations(metersToRotations(...)) or apply Math.max(..., minimumSafeElevatorHeightRotations()) before calling setControl.
- Also note: src/main/java/frc/trigon/robot/subsystems/intake/Intake.java:108 performs angleMotor.setControl(...) — confirm whether a similar safety clamp is required.
♻️ Duplicate comments (6)
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (3)
52-55: SysId linear metrics are wrong units (rotations used as meters; velocity unconverted).
Use meters for linearPosition and meters/sec for linearVelocity; convert from rotations. This mirrors prior feedback.Apply:
log.motor("Elevator") - .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)))) .voltage(Units.Volts.of(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)));
95-98: LOAD_CORAL bypasses safety clamp; route through setTargetPositionRotations.
Directly commanding the motor skips the arm/elevator interlock. Same issue noted earlier.- if (targetState == ElevatorConstants.ElevatorState.LOAD_CORAL) { - masterMotor.setControl(positionRequest.withPosition(metersToRotations(targetState.targetPositionMeters))); - return; - } + if (targetState == ElevatorConstants.ElevatorState.LOAD_CORAL) { + setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters)); + return; + }
106-114: Cosine should use Rotation2d API and min height should be clamped to [MIN, MAX].
Prevents negatives/overshoot and aligns with WPILib idioms.+import edu.wpi.first.math.MathUtil; @@ - private double minimumSafeElevatorHeightRotations() { - final double armCos = RobotContainer.ARM.getAngle().getRadians(); - final double elevatorHeightFromArm = Math.cos(armCos) * ArmConstants.ARM_LENGTH_METERS; - final double minimumElevatorSafeZone = ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; - final double minimumSafeHeightMeters = (RobotContainer.ARM.isArmAboveSafeAngle() - ? 0 : elevatorHeightFromArm) - + minimumElevatorSafeZone; - return metersToRotations(minimumSafeHeightMeters); - } + private double minimumSafeElevatorHeightRotations() { + final var armAngle = RobotContainer.ARM.getAngle(); + final double minCalculatedMeters = + armAngle.getCos() * ArmConstants.ARM_LENGTH_METERS + + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; + final double minSafeMeters = RobotContainer.ARM.isArmAboveSafeAngle() + ? 0 + : MathUtil.clamp( + minCalculatedMeters, + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS, + ElevatorConstants.MAXIMUM_ELEVATOR_SAFE_ZONE_METERS); + return metersToRotations(minSafeMeters); + }src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (3)
175-178: Equality should pass tolerance check.
Use <= so 0° difference with 0° tolerance is true.- return currentToTargetAngleDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); + return currentToTargetAngleDifferenceDegrees <= ArmConstants.ANGLE_TOLERANCE.getDegrees();
86-88: Rotation2d wrap-around bug in isArmAboveSafeAngle ([-180,180) domain).
Normalize before comparing; otherwise angles >180° read negative and fail the check.- return getAngle().getDegrees() >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees(); + final double angleDeg = MathUtil.inputModulus(getAngle().getDegrees(), 0.0, 360.0); + return angleDeg >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees();
157-159: Critical: clamp uses Math.max on getRotations(), broken by Rotation2d wrap.
Targets >180° can become negative rotations and lose to a smaller positive min angle. Normalize to [0,360) for comparison, then map back.- armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), minimumArmSafeAngle().getRotations()))); + final double targetDeg = normalize0to360(targetAngle.getDegrees()); + final double minDeg = normalize0to360(minimumArmSafeAngle().getDegrees()); + final double commandedDeg = Math.max(targetDeg, minDeg); + // Map back to motor domain [-180,180) -> rotations + final double commandedDegForMotor = MathUtil.inputModulus(commandedDeg, -180.0, 180.0); + final double commandedRot = commandedDegForMotor / 360.0; + armMasterMotor.setControl(positionRequest.withPosition(commandedRot));Add helper in this class:
private static double normalize0to360(double degrees) { double d = degrees % 360.0; return d < 0 ? d + 360.0 : d; }
🧹 Nitpick comments (4)
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (1)
102-105: Optional: log when elevator target is clamped.
Helps tuning/debug.- masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, minimumSafeElevatorHeightRotations()))); + final double minRot = minimumSafeElevatorHeightRotations(); + final double commanded = Math.max(targetPositionRotations, minRot); + if (commanded != targetPositionRotations) { + Logger.recordOutput("Elevator/ClampedTargetRotations", commanded); + } + masterMotor.setControl(positionRequest.withPosition(commanded));src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (3)
138-155: Naming/API duplication (setArmState vs setTargetState).
Consider consolidating to one entrypoint to avoid divergence.
180-182: Optional: avoid magic 360; use Rotation2d constants or a named FULL_ROTATION.
Improves readability.-private static Rotation2d subtractFrom360Degrees(Rotation2d angleToSubtract) { - return Rotation2d.fromDegrees(360 - angleToSubtract.getDegrees()); -} +private static final Rotation2d FULL_ROTATION = Rotation2d.fromDegrees(360); +private static Rotation2d subtractFrom360Degrees(Rotation2d angleToSubtract) { + return FULL_ROTATION.minus(angleToSubtract); +}
61-66: Introduce explicit raw vs gravity-offset angle accessors
getAngle() is consumed by logging, safety checks, transforms, and closed-loop code—shifting it to include the gravity offset would alter its behavior everywhere. Instead, preserve getAngle() as the raw encoder reading and add a dedicated gravity-offset accessor:
- In Arm.java, keep
public Rotation2d getAngle()
returningRotation2d.fromRotations(angleEncoder.getSignal(...))- Add
public Rotation2d getGravityOffsetAngle()
that applies+ ArmConstants.POSITION_OFFSET_FROM_GRAVITY_OFFSET- In updateMechanism(), call
getGravityOffsetAngle()for both measured and reference angles- Document both methods in Javadoc so consumers know which frame they’re using
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (2)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java(4 hunks)src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java(3 hunks)
🧰 Additional context used
🧠 Learnings (1)
📚 Learning: 2025-09-11T08:52:03.291Z
Learnt from: levyishai
PR: Programming-TRIGON/RobotCodeOffSeason2025#11
File: src/main/java/frc/trigon/robot/subsystems/arm/Arm.java:117-124
Timestamp: 2025-09-11T08:52:03.291Z
Learning: When working with Rotation2d objects in WPILib, prefer using the built-in trigonometric methods like Rotation2d.cos() and Rotation2d.sin() instead of Math.cos(rotation.getRadians()) and Math.sin(rotation.getRadians()). This is more readable and semantically appropriate.
Applied to files:
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java
🧬 Code graph analysis (2)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (2)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-119)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-262)
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (3)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-119)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-262)src/main/java/lib/utilities/Conversions.java (1)
Conversions(8-258)
🔇 Additional comments (8)
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (3)
12-19: LGTM: imports needed for safety/telemetry.
28-28: LGTM: FOC flag on motion request retained.
80-83: No redundant meters-to-meters conversions found; getPositionMeters() usage is correct. The only conversion in ElevatorSimulation from meters back to rotations is intentional for simulating encoder values.src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (5)
92-92: LGTM: reversed atState returns computed result.
97-97: LGTM: atState single-arg delegates to angle check.
100-105: LGTM: atTargetAngle handles reversed state.
161-168: LGTM: minimumArmSafeAngle clamps acos domain and respects elevator safe zone.
186-188: LGTM: visualization Z uses elevator height.
There was a problem hiding this comment.
Actionable comments posted: 0
♻️ Duplicate comments (3)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (3)
107-108: Verify consistent angle frame usage across the codebase.The getAngle() method returns raw CANcoder rotations while other code applies POSITION_OFFSET_FROM_GRAVITY_OFFSET (seen in updateMechanism). This creates inconsistent reference frames that could affect safety calculations and elevator coordination.
Based on retrieved learnings, prefer centralizing the gravity offset in getAngle() or ensuring all callers consistently handle the frame. Run this verification:
#!/bin/bash # Verify angle frame consistency across subsystem interactions echo "=== Checking for POSITION_OFFSET_FROM_GRAVITY_OFFSET usage ===" rg -n --type=java "POSITION_OFFSET_FROM_GRAVITY_OFFSET" echo -e "\n=== Checking Arm.getAngle() usages ===" rg -n --type=java -C2 "getAngle\(\)" | grep -A2 -B2 "ARM\." echo -e "\n=== Checking elevator safety math ===" rg -n --type=java -C3 "getAngle\(\).*cos|cos.*getAngle"
175-177: Fix tolerance check for exact angle matching.Using
<instead of<=makes atAngle() always return false when ANGLE_TOLERANCE is 0°, and the method fails when the difference exactly equals the tolerance.- return currentToTargetAngleDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); + return currentToTargetAngleDifferenceDegrees <= ArmConstants.ANGLE_TOLERANCE.getDegrees();
158-159: Critical: Wrap-around bug in angle clamping breaks safety.Using
Math.max(targetAngle.getRotations(), minimumArmSafeAngle().getRotations())can choose unsafe angles due to Rotation2d wrapping. For example, a 270° target becomes -0.25 rotations and loses to a positive minimum safe angle, potentially commanding an unsafe position.Apply this fix to compare angles in normalized space:
- private void setTargetAngle(Rotation2d targetAngle) { - armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), minimumArmSafeAngle().getRotations()))); - } + private void setTargetAngle(Rotation2d targetAngle) { + final Rotation2d minSafeAngle = minimumArmSafeAngle(); + final double targetDeg = normalize0to360(targetAngle.getDegrees()); + final double minDeg = normalize0to360(minSafeAngle.getDegrees()); + final double commandedDeg = Math.max(targetDeg, minDeg); + // Convert back to motor domain [-180,180) -> rotations + final double commandedDegForMotor = MathUtil.inputModulus(commandedDeg, -180.0, 180.0); + final double commandedRot = commandedDegForMotor / 360.0; + armMasterMotor.setControl(positionRequest.withPosition(commandedRot)); + }Add this helper method to the class:
private static double normalize0to360(double degrees) { double d = degrees % 360.0; return d < 0 ? d + 360.0 : d; }
🧹 Nitpick comments (1)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (1)
180-182: Use WPILib constant instead of manual calculation.The current implementation
Rotation2d.k180deg.getDegrees() * 2manually calculates 360°, but this could be replaced with a cleaner approach using WPILib constants.As suggested in past reviews, consider using a constant for 360° and using Rotation2d operations:
- private static Rotation2d subtractFrom360Degrees(Rotation2d angleToSubtract) { - return Rotation2d.fromDegrees(Rotation2d.k180deg.getDegrees() * 2 - angleToSubtract.getDegrees()); - } + private static final Rotation2d FULL_ROTATION = Rotation2d.fromDegrees(360.0); + + private static Rotation2d subtractFrom360Degrees(Rotation2d angleToSubtract) { + return FULL_ROTATION.minus(angleToSubtract); + }Note: If wrap-around between 180° and -180° is still a concern with this approach, the current implementation may be kept as-is since it explicitly handles the team's specific requirements.
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (2)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java(4 hunks)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java(8 hunks)
🚧 Files skipped from review as they are similar to previous changes (1)
- src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java
🧰 Additional context used
🧠 Learnings (1)
📚 Learning: 2025-09-11T08:52:03.291Z
Learnt from: levyishai
PR: Programming-TRIGON/RobotCodeOffSeason2025#11
File: src/main/java/frc/trigon/robot/subsystems/arm/Arm.java:117-124
Timestamp: 2025-09-11T08:52:03.291Z
Learning: When working with Rotation2d objects in WPILib, prefer using the built-in trigonometric methods like Rotation2d.cos() and Rotation2d.sin() instead of Math.cos(rotation.getRadians()) and Math.sin(rotation.getRadians()). This is more readable and semantically appropriate.
Applied to files:
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java
🧬 Code graph analysis (1)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (2)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-119)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-262)
🔇 Additional comments (1)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (1)
184-189: LGTM: Proper elevator integration for visualization.The visualization now correctly incorporates the elevator position in the Z-axis translation, which will provide accurate 3D positioning of the arm mechanism. The transform calculation properly combines elevator height with arm rotation.
|
|
||
| private static Rotation2d subtractFrom360Degrees(Rotation2d angleToSubtract) { | ||
| return Rotation2d.fromDegrees(360 - angleToSubtract.getDegrees()); | ||
| return Rotation2d.fromDegrees(Rotation2d.k180deg.getDegrees() * 2 - angleToSubtract.getDegrees()); |
There was a problem hiding this comment.
This doesn't change anything, it just overcomplicates it for no reason. Just revert it to the way it was before. I don't care enough about this.
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: 0
♻️ Duplicate comments (3)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (3)
86-88: Normalize angle to avoid Rotation2d wrap-around pitfalls.getDegrees() is in [-180,180). For angles > 180°, this check can be wrong. Minimal safe fix:
- return getAngle().getDegrees() >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees(); + final double angleDeg = MathUtil.inputModulus(getAngle().getDegrees(), 0.0, 360.0); + return angleDeg >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees();
175-178: Critical: zero tolerance makes atAngle always false.With ANGLE_TOLERANCE = 0°, using < never returns true. Use <= and consider a small non-zero tolerance in constants (1–2°).
- return currentToTargetAngleDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); + return currentToTargetAngleDifferenceDegrees <= ArmConstants.ANGLE_TOLERANCE.getDegrees();
157-159: Critical — fix wrap-around in Arm.setTargetAngle by normalizing to [0,360) before clampingRotation2d.getRotations() is in [-0.5, 0.5); comparing rotations directly with Math.max misorders angles >180° (e.g. 270° → -0.25) and can block valid targets. Replace the direct rotations clamp with degree-based normalization, compare, then map back to rotations.
File: src/main/java/frc/trigon/robot/subsystems/arm/Arm.java — setTargetAngle (around lines 157–159)
- armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), minimumArmSafeAngle().getRotations()))); + final Rotation2d minSafe = minimumArmSafeAngle(); + final double targetDeg = normalize0to360(targetAngle.getDegrees()); + final double minDeg = normalize0to360(minSafe.getDegrees()); + final double commandedDeg = Math.max(targetDeg, minDeg); + // Map back to motor domain [-180,180) -> rotations + final double commandedDegForMotor = MathUtil.inputModulus(commandedDeg, -180.0, 180.0); + final double commandedRot = commandedDegForMotor / 360.0; + armMasterMotor.setControl(positionRequest.withPosition(commandedRot));Add helper in this class (outside this hunk):
private static double normalize0to360(double degrees) { double d = degrees % 360.0; return d < 0 ? d + 360.0 : d; }I searched the repo for the Math.max(...getRotations(...)) pattern and found this occurrence only; other withPosition(angle.getRotations()) uses exist (SwerveModule, Intake) but do not use Math.max clamping.
🧹 Nitpick comments (4)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (4)
10-10: Be mindful of tight coupling to RobotContainer.Acceptable for FRC, but consider injecting the elevator dependency (or an interface) to improve testability.
161-168: Safe-angle computation is robust; tiny numeric/style nit.Clamping height/L to [0,1] avoids NaN. Prefer doubles for clarity:
- final double cosOfMinimumSafeAngle = MathUtil.clamp(heightFromSafeZone / ArmConstants.ARM_LENGTH_METERS, 0, 1); + final double cosOfMinimumSafeAngle = MathUtil.clamp(heightFromSafeZone / ArmConstants.ARM_LENGTH_METERS, 0.0, 1.0);
180-183: Minor: avoid magic 360; prefer a named constant.For readability:
- return Rotation2d.fromDegrees(Rotation2d.k180deg.getDegrees() * 2 - angleToSubtract.getDegrees()); + return Rotation2d.fromDegrees(360.0 - angleToSubtract.getDegrees());Optionally define
private static final Rotation2d FULL_ROTATION = Rotation2d.fromDegrees(360);and useFULL_ROTATION.minus(angleToSubtract).
184-188: Visualization may need gravity-offset angle.updateMechanism applies POSITION_OFFSET_FROM_GRAVITY_OFFSET, but calculateVisualizationPose uses raw getAngle(). If the visual expects the same frame, add the offset:
- new Rotation3d(0, getAngle().getRadians(), 0) + new Rotation3d(0, getAngle().plus(Rotation2d.fromRotations(ArmConstants.POSITION_OFFSET_FROM_GRAVITY_OFFSET)).getRadians(), 0)Confirm the intended frame for the 3D pose.
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (1)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java(4 hunks)
🧰 Additional context used
🧠 Learnings (2)
📚 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/Arm.java
📚 Learning: 2025-09-11T08:52:03.291Z
Learnt from: levyishai
PR: Programming-TRIGON/RobotCodeOffSeason2025#11
File: src/main/java/frc/trigon/robot/subsystems/arm/Arm.java:117-124
Timestamp: 2025-09-11T08:52:03.291Z
Learning: When working with Rotation2d objects in WPILib, prefer using the built-in trigonometric methods like Rotation2d.cos() and Rotation2d.sin() instead of Math.cos(rotation.getRadians()) and Math.sin(rotation.getRadians()). This is more readable and semantically appropriate.
Applied to files:
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java
🧬 Code graph analysis (1)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (2)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-119)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-262)
🔇 Additional comments (10)
src/main/java/frc/trigon/robot/subsystems/arm/Arm.java (10)
5-5: Import is appropriate.MathUtil is used correctly for clamping below.
90-93: Reversed-state check looks good.The subtract-from-360 logic is applied and returned.
96-98: LGTM: state/angle match.Clear and correct.
100-105: LGTM: unified atTargetAngle() API.The reversed logic is handled and returned properly.
107-109: Clarify angle frame (raw vs gravity‑offset).getAngle() returns raw CANcoder rotations; visualization adds the offset elsewhere. Please add a brief Javadoc stating this returns RAW encoder angle and where offsets are applied, to avoid misuse.
121-126: Reversed target state handling reads correctly.subtractFrom360Degrees(...) is used and applied.
133-136: End-effector voltage set after angle: OK.Order is reasonable.
138-151: setArmState overloads: OK.Behavior matches intent; reversed path sets angle once.
153-156: End-effector state helper: OK.Thin wrapper improves readability.
170-173: LGTM: end-effector voltage command.Matches mechanism visualization and motor command.
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: 1
♻️ Duplicate comments (3)
src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java (1)
54-60: LGTM! Proper ExecuteEndCommand usage for prepare-for-state logic.The use of
ExecuteEndCommandis appropriate here since this command needs to continuously update the arm state during execution and properly stop the arm when the command ends. This aligns with the team's stated requirement to continuously update the minimum angle without breaking the command flow.This addresses previous discussions about command choice for continuously updating state.
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (2)
52-55: SysId units are wrong: rotations logged as meters; velocity not converted to m/sLog position/velocity in meters and m/s. Use your rotations→meters conversion for both.
Apply:
- .linearPosition(Units.Meters.of(getPositionRotations())) - .linearVelocity(Units.MetersPerSecond.of(masterMotor.getSignal(TalonFXSignal.VELOCITY))) + .linearPosition(Units.Meters.of(rotationsToMeters(getPositionRotations()))) + .linearVelocity(Units.MetersPerSecond.of(rotationsToMeters(masterMotor.getSignal(TalonFXSignal.VELOCITY))))If you have a helper that already returns m/s, call it here instead of the raw Talon signal.
95-98: Safety clamp is bypassed for LOAD_CORALDirectly commanding positionRequest skips setTargetPositionRotations() and its minimum-height clamp, risking a collision when the arm is down.
Apply:
- if (targetState == ElevatorConstants.ElevatorState.LOAD_CORAL) { - masterMotor.setControl(positionRequest.withPosition(metersToRotations(targetState.targetPositionMeters))); - return; - } + if (targetState == ElevatorConstants.ElevatorState.LOAD_CORAL) { + setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters)); + return; + }
🧹 Nitpick comments (3)
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (3)
106-114: Use Rotation2d.getCos() and clamp computed minimum within safe zoneAvoid angle wrap/normalization pitfalls and prevent negatives/overshoot; also clarify variable naming.
Apply:
- private double getMinimumSafeElevatorHeightRotations() { - final double armCos = RobotContainer.ARM.getAngle().getRadians(); - final double elevatorHeightFromArm = Math.cos(armCos) * ArmConstants.ARM_LENGTH_METERS; - final double minimumElevatorSafeZone = ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; - final double minimumSafeHeightMeters = (RobotContainer.ARM.isArmAboveSafeAngle() - ? 0 : elevatorHeightFromArm) - + minimumElevatorSafeZone; - return metersToRotations(minimumSafeHeightMeters); - } + private double getMinimumSafeElevatorHeightRotations() { + final var armAngle = RobotContainer.ARM.getAngle(); + final double minCalculatedMeters = + armAngle.getCos() * ArmConstants.ARM_LENGTH_METERS + + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; + final double minimumSafeHeightMeters = RobotContainer.ARM.isArmAboveSafeAngle() + ? 0 + : MathUtil.clamp( + minCalculatedMeters, + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS, + ElevatorConstants.MAXIMUM_ELEVATOR_SAFE_ZONE_METERS); + return metersToRotations(minimumSafeHeightMeters); + }Add (if not already):
+import edu.wpi.first.math.MathUtil;
84-91: Consider documenting/clamping semantics of “safe zone” helpersgetElevatorHeightFromMinimumSafeZone() can be negative; if callers don’t expect negatives, clamp at 0 or document the contract.
102-104: Clamp target within safe bounds
Also enforce an upper bound usingMAXIMUM_ELEVATOR_SAFE_ZONE_METERS. For example:- masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, getMinimumSafeElevatorHeightRotations()))); + final double minRot = getMinimumSafeElevatorHeightRotations(); + final double maxRot = metersToRotations(ElevatorConstants.MAXIMUM_ELEVATOR_SAFE_ZONE_METERS); + masterMotor.setControl( + positionRequest.withPosition(MathUtil.clamp(targetPositionRotations, minRot, maxRot)));Add import:
+import edu.wpi.first.math.MathUtil;
📜 Review details
Configuration used: CodeRabbit UI
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (2)
src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java(2 hunks)src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java(3 hunks)
🧰 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/elevator/Elevator.java
🧬 Code graph analysis (2)
src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java (2)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-119)src/main/java/lib/commands/ExecuteEndCommand.java (1)
ExecuteEndCommand(9-21)
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (3)
src/main/java/frc/trigon/robot/RobotContainer.java (1)
RobotContainer(45-119)src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java (1)
ArmConstants(32-262)src/main/java/lib/utilities/Conversions.java (1)
Conversions(8-258)
🔇 Additional comments (3)
src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java (1)
50-52: LGTM! Consistent delegation pattern.Good implementation of the single-argument convenience method that delegates to the two-argument version with
isStateReversed = false.src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java (2)
80-82: Public getter LGTMExposing position in meters improves reuse and reduces unit mistakes.
12-12: Non-functional import/FOC tweaksImport additions and withEnableFOC formatting look fine.
Also applies to: 14-14, 18-18, 28-28
| void setTargetState(ElevatorConstants.ElevatorState targetState) { | ||
| this.targetState = targetState; | ||
| scalePositionRequestSpeed(targetState.speedScalar); | ||
| if (targetState == ElevatorConstants.ElevatorState.LOAD_CORAL) { | ||
| masterMotor.setControl(positionRequest.withPosition(metersToRotations(targetState.targetPositionMeters))); | ||
| return; | ||
| } | ||
| setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters)); | ||
| } |
There was a problem hiding this comment.
🧩 Analysis chain
One more guard: prevent future bypasses of the clamp
Search for any other direct masterMotor.setControl(positionRequest.withPosition(...)) writes in Elevator to ensure all paths route through setTargetPositionRotations().
Also applies to: 102-114
🏁 Script executed:
#!/bin/bash
rg -nP --type=java -C2 'setControl\s*\(\s*positionRequest\.withPosition\(' src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.javaLength of output: 535
Prevent clamp bypass in LOAD_CORAL branch
src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java — the LOAD_CORAL branch in setTargetState directly calls masterMotor.setControl(positionRequest.withPosition(...)) (line 96), bypassing the clamp in setTargetPositionRotations (line 103). Replace that direct call with setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters)) or apply the same Math.max clamp before calling masterMotor.setControl.
🤖 Prompt for AI Agents
In src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java around lines
92 to 100, the LOAD_CORAL branch in setTargetState directly invokes
masterMotor.setControl(positionRequest.withPosition(...)) which bypasses the
clamping logic inside setTargetPositionRotations; replace the direct masterMotor
call with a call to
setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters))
so the same Math.max clamp is applied (or compute the rotations, apply the same
clamp used in setTargetPositionRotations, then call masterMotor.setControl with
the clamped value).
Summary by CodeRabbit