Mechanism2D in AdvantageScope testing #151

RobototesProgrammers · 2025-06-04T01:45:01Z

No description provided.

jamesdooley4 · 2025-09-30T01:59:26Z

src/main/java/frc/robot/Robot.java

    LiveWindow.disableAllTelemetry();
    LiveWindow.enableTelemetry(PDH);
+    mech = new Mechanism2d(1, 2);
+    root = mech.getRoot("climber", 0.5 + Units.inchesToMeters(5.5), Units.inchesToMeters(19.5));


? Not sure what the "climber" is, as this year's robot isn't supposed to climb.

Funnel pivot?

jamesdooley4 · 2025-09-30T02:01:49Z

src/main/java/frc/robot/Robot.java

+    root = mech.getRoot("climber", 0.5 + Units.inchesToMeters(5.5), Units.inchesToMeters(19.5));
+    SmartDashboard.putData("Mechanism", mech);
+    m_elevator =
+        root.append(new MechanismLigament2d("elevator", 1, 90, 2, new Color8Bit(Color.kRed)));


Worth a comment that this is a single-stage elevator even though the robot actually uses a multi-stage elevator.

jamesdooley4 · 2025-09-30T02:06:11Z

src/main/java/frc/robot/subsystems/ElevatorSubsystem.java

  public static final double CORAL_QUICK_INTAKE = 1.6;
  public static final double MIN_EMPTY_GROUND_INTAKE = 4.5;
  public static final double MIN_FULL_GROUND_INTAKE = 8.0;
+  private static final double MOTOR_ROTATIONS_PER_METER = 40; // Inaccurate


Should be able to make this accurate by measuring how far the elevator extends (in meters) at CORAL_LEVEL_FOUR_PRE_POS and then simply change this to 37.5 / <actual relative elevator extension>, where <actual relative elevator extension> is what's measured with a tape measure.

jamesdooley4 · 2025-09-30T02:09:36Z

src/main/java/frc/robot/Robot.java

+  Mechanism2d mech;
+  MechanismRoot2d root;
+  MechanismLigament2d m_elevator;
+  MechanismLigament2d m_wrist;


Would be nicer to encapsulate the Mechanism (and similar "report physical position") code in a different class instead of adding it to the main Robot class. These are generally called 'digital twins', so could use something like DigitalTwin.java and have robotPeriodic() simply call an update() function on it.

jamesdooley4 · 2025-10-06T05:26:01Z

src/main/java/frc/robot/subsystems/ElevatorSubsystem.java

      m_motorOneSimState.setRawRotorPosition(targetPos);
      m_motorTwoSimState.setRawRotorPosition(targetPos);
+
+      elevatorPose3d.set(new Pose3d(new Pose2d(0.0, getHeightMeters(), new Rotation2d(0.0,0.0))));


When running this in the simulator, it's continually logging this error here:

Error at java.base/java.lang.Thread.getStackTrace(Thread.java:1619): x and y components of Rotation2d are zero at java.base/java.lang.Thread.getStackTrace(Thread.java:1619) at edu.wpi.first.math.geometry.Rotation2d.<init>(Rotation2d.java:126) at frc.robot.subsystems.ElevatorSubsystem.periodic(ElevatorSubsystem.java:388) at edu.wpi.first.wpilibj2.command.CommandScheduler.run(CommandScheduler.java:269) at frc.robot.Robot.robotPeriodic(Robot.java:137) at edu.wpi.first.wpilibj.IterativeRobotBase.loopFunc(IterativeRobotBase.java:400) at edu.wpi.first.wpilibj.TimedRobot.startCompetition(TimedRobot.java:148) at edu.wpi.first.wpilibj.RobotBase.runRobot(RobotBase.java:419) at edu.wpi.first.wpilibj.RobotBase.lambda$startRobot$1(RobotBase.java:490) at java.base/java.lang.Thread.run(Thread.java:840)

…into GGCOMPB

src/main/java/frc/robot/subsystems/auto/AutoAlign.java

… into Mechanisms

presets?

markpete · 2025-11-21T05:18:12Z

/gemini review

gemini-code-assist

Code Review

This pull request introduces a Mechanism2d visualization for the robot's arm and elevator, and refactors the AdvantageScope/Elastic layout for better organization. The changes also include updates to the auto-alignment and scoring logic, particularly for the solo controller.

My review focuses on improving code maintainability and fixing a few issues in the new simulation code. I've suggested refactoring some duplicated methods in Controls.java and replacing magic numbers with constants. There are also several minor cleanup items in ElevatorSubsystem.java, along with a critical bug in the Rotation3d usage for the simulation visualization that should be addressed.

gemini-code-assist · 2025-11-21T05:21:02Z

src/main/java/frc/robot/subsystems/ElevatorSubsystem.java

+        TESTpose.set(new Pose3d(
+          0.2, 0.0, targetZ,
+          new Rotation3d(0.0, 0.0, -135)));


The Rotation3d constructor takes radians, but is being passed -135. This is likely intended to be degrees and should be converted using Units.degreesToRadians(-135). Using -135 directly results in a very large and incorrect rotation in radians.

Suggested change

TESTpose.set(new Pose3d(

0.2, 0.0, targetZ,

new Rotation3d(0.0, 0.0, -135)));

TESTpose.set(new Pose3d(

0.2, 0.0, targetZ,

new Rotation3d(0.0, 0.0, edu.wpi.first.math.util.Units.degreesToRadians(-135))));

gemini-code-assist · 2025-11-21T05:21:02Z

src/main/java/frc/robot/Controls.java

+  private Command getSoloCoralBranchHeightCommandL() {
+    return switch (branchHeight) {
+      case CORAL_LEVEL_FOUR -> superStructure
+          .coralLevelFour(soloController.rightBumper())
+          .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
+      case CORAL_LEVEL_THREE -> superStructure
+          .coralLevelThree(soloController.rightBumper(), () -> AutoAlign.poseInPlace())
+          .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
+      case CORAL_LEVEL_TWO -> superStructure
+          .coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace())
+          .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
+      case CORAL_LEVEL_ONE -> superStructure
+          .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlaceL1L())
+          .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
+    };
+  }
+
+  private Command getSoloCoralBranchHeightCommandR() {
    return switch (branchHeight) {
      case CORAL_LEVEL_FOUR -> superStructure
          .coralLevelFour(soloController.rightBumper())
          .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
      case CORAL_LEVEL_THREE -> superStructure
-          .coralLevelThree(soloController.rightBumper())
+          .coralLevelThree(soloController.rightBumper(), () -> AutoAlign.poseInPlace())
          .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
      case CORAL_LEVEL_TWO -> superStructure
-          .coralLevelTwo(soloController.rightBumper())
+          .coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace())
          .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
      case CORAL_LEVEL_ONE -> superStructure
-          .coralLevelOne(soloController.rightBumper())
+          .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlaceL1R())
          .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
    };
  }


The methods getSoloCoralBranchHeightCommandL and getSoloCoralBranchHeightCommandR are nearly identical, with the only difference being the BooleanSupplier passed to superStructure.coralLevelOne. You can refactor them into a single method that accepts the BooleanSupplier as a parameter. This reduces code duplication and improves maintainability.

You can then call this new method from configureSoloControllerBindings like getSoloCoralBranchHeightCommand(() -> AutoAlign.poseInPlaceL1L()) and getSoloCoralBranchHeightCommand(() -> AutoAlign.poseInPlaceL1R()).

Suggested change

private Command getSoloCoralBranchHeightCommandL() {

return switch (branchHeight) {

case CORAL_LEVEL_FOUR -> superStructure

.coralLevelFour(soloController.rightBumper())

.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);

case CORAL_LEVEL_THREE -> superStructure

.coralLevelThree(soloController.rightBumper(), () -> AutoAlign.poseInPlace())

.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);

case CORAL_LEVEL_TWO -> superStructure

.coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace())

.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);

case CORAL_LEVEL_ONE -> superStructure

.coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlaceL1L())

.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);

};

}

private Command getSoloCoralBranchHeightCommandR() {

return switch (branchHeight) {

case CORAL_LEVEL_FOUR -> superStructure

.coralLevelFour(soloController.rightBumper())

.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);

case CORAL_LEVEL_THREE -> superStructure

.coralLevelThree(soloController.rightBumper())

.coralLevelThree(soloController.rightBumper(), () -> AutoAlign.poseInPlace())

.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);

case CORAL_LEVEL_TWO -> superStructure

.coralLevelTwo(soloController.rightBumper())

.coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace())

.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);

case CORAL_LEVEL_ONE -> superStructure

.coralLevelOne(soloController.rightBumper())

.coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlaceL1R())

.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);

};

}

private Command getSoloCoralBranchHeightCommand(BooleanSupplier l1PoseSupplier) {

return switch (branchHeight) {

case CORAL_LEVEL_FOUR -> superStructure

.coralLevelFour(soloController.rightBumper())

.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);

case CORAL_LEVEL_THREE -> superStructure

.coralLevelThree(soloController.rightBumper(), () -> AutoAlign.poseInPlace())

.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);

case CORAL_LEVEL_TWO -> superStructure

.coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace())

.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);

case CORAL_LEVEL_ONE -> superStructure

.coralLevelOne(soloController.rightBumper(), l1PoseSupplier)

.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);

};

}

gemini-code-assist · 2025-11-21T05:21:03Z

src/main/java/frc/robot/Controls.java

+    /* Trigger readyToScore = new Trigger(() -> AutoAlign.poseInPlace());
    readyToScore.onTrue(
        Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 1.0)));
    readyToScore.onFalse(
-        Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 0.0)));
+        Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 0.0))); */


This block of code has been commented out with the note (Removal for autoscoring). If it's no longer needed, it's better to remove it completely to improve code clarity. Version control can be used to retrieve it if needed in the future.

gemini-code-assist · 2025-11-21T05:21:03Z

src/main/java/frc/robot/Robot.java

+    root = mech.getRoot("climber", 0.5 + Units.inchesToMeters(5.5), Units.inchesToMeters(19.5));
+    SmartDashboard.putData("Mechanism", mech);
+    m_elevator =
+        root.append(new MechanismLigament2d("elevator", 1, 90, 2, new Color8Bit(Color.kRed)));
+    var pivot =
+        m_elevator.append(
+            new MechanismLigament2d(
+                "pivot offset", Units.inchesToMeters(4), -90, 2, new Color8Bit(Color.kDarkRed)));
+    m_wrist =
+        pivot.append(
+            new MechanismLigament2d(
+                "wrist", Units.inchesToMeters(14.5), 270, 6, new Color8Bit(Color.kFirstRed)));


The initialization of the Mechanism2d uses several magic numbers for dimensions (e.g., 5.5, 19.5, 4, 14.5) and visualization properties (e.g., 1, 2, 90, -90, 270, 6). It would be better to define these as named constants in a relevant constants class (e.g., SubsystemConstants or a new RobotDimensions class). This improves readability and makes it easier to update these values.

gemini-code-assist · 2025-11-21T05:21:03Z

src/main/java/frc/robot/subsystems/ElevatorSubsystem.java

+import frc.robot.Robot;
+import frc.robot.sensors.ArmSensor;
+import frc.robot.subsystems.auto.AutoLogic;
+import jdk.jfr.Timestamp;


The import jdk.jfr.Timestamp is unused in this file and should be removed.

gemini-code-assist · 2025-11-21T05:21:03Z

src/main/java/frc/robot/subsystems/ElevatorSubsystem.java

+  private  StructPublisher<Pose3d> elevatorPose3d = NetworkTableInstance.getDefault().getStructTopic("elevator/heightPose", Pose3d.struct).publish();
+  public  StructPublisher<Pose3d> TESTpose = NetworkTableInstance.getDefault().getStructTopic("debug/TEST", Pose3d.struct).publish();
+  //public  StructPublisher<Pose3d> TESTpose2 = NetworkTableInstance.getDefault().getStructTopic("debug/TEST2", Pose3d.struct).publish();


This section contains commented-out code and inconsistent formatting. The commented-out publisher for TESTpose2 should be removed if it's not needed. Also, the extra spaces before StructPublisher can be removed for consistent formatting.

Suggested change

private StructPublisher<Pose3d> elevatorPose3d = NetworkTableInstance.getDefault().getStructTopic("elevator/heightPose", Pose3d.struct).publish();

public StructPublisher<Pose3d> TESTpose = NetworkTableInstance.getDefault().getStructTopic("debug/TEST", Pose3d.struct).publish();

//public StructPublisher<Pose3d> TESTpose2 = NetworkTableInstance.getDefault().getStructTopic("debug/TEST2", Pose3d.struct).publish();

private StructPublisher<Pose3d> elevatorPose3d = NetworkTableInstance.getDefault().getStructTopic("elevator/heightPose", Pose3d.struct).publish();

public StructPublisher<Pose3d> TESTpose = NetworkTableInstance.getDefault().getStructTopic("debug/TEST", Pose3d.struct).publish();

gemini-code-assist · 2025-11-21T05:21:03Z

src/main/java/frc/robot/subsystems/ElevatorSubsystem.java

    return runOnce(
            () -> {
              if (hasBeenZeroed) {
+                System.out.println("Setting elevator level to: " + pos);


Using System.out.println for logging is generally discouraged in robot code as it can be slow and doesn't integrate with the standard FRC logging framework. Consider using DataLogManager or publishing to SmartDashboard for debugging information, or remove this line if it's no longer needed.

gemini-code-assist · 2025-11-21T05:21:03Z

src/main/java/frc/robot/subsystems/ElevatorSubsystem.java

+  double smoothedAngleZ = 0.4;
+  double smoothingFactor = 0.1;


These member variables smoothedAngleZ and smoothingFactor are declared but never used. They should be removed to clean up the code.

gemini-code-assist · 2025-11-21T05:21:03Z

src/main/java/frc/robot/subsystems/ElevatorSubsystem.java

+        if (!Robot.getInstance().sensors.armSensor.booleanInClaw()) {
+        }


This if statement has an empty body and should be removed.

gemini-code-assist · 2025-11-21T05:21:03Z

src/main/java/frc/robot/subsystems/ElevatorSubsystem.java

+        double smoothingFactor = 0.5;// Percentage Scaler
+        double bottomZ = 0.2;
+        double topZ = 1.55;
+        double minPos = 0.0;
+        double maxPos = 37.5;
+        double targetZ = (bottomZ + ((curPos - minPos) / (maxPos - minPos)) * (topZ - bottomZ));


These lines use several magic numbers for an interpolation calculation (e.g., 0.5, 0.2, 1.55, 0.0, 37.5). These should be defined as named constants with descriptive names to improve readability and maintainability.

RobototesProgrammers and others added 3 commits May 29, 2025 20:07

Added Mechanism2D display of claw, arm pivot, and elevator

0d38f0a

Spotless

517f0d6

Merge branch 'main' into Mechanisms

ed09bbe

Jetblackdragon added the enhancement New feature or request label Aug 26, 2025

iamawesomecat assigned jamesdooley4 and SnappleRamen Sep 23, 2025

jamesdooley4 requested changes Sep 30, 2025

View reviewed changes

RobototesProgrammers added 2 commits October 2, 2025 18:46

Added mechanism config file to robot code for easy access WIP

83839ad

Added networktables entry for elevator pose3d WIP

5c5fb1f

jamesdooley4 reviewed Oct 6, 2025

View reviewed changes

RobototesProgrammers and others added 14 commits October 18, 2025 08:56

elastic camera fix

7a86d58

autoScore stuff

c8fcedc

Merge branch 'GGCOMPB' of https://github.com/robototes/Reefscape2025 …

3cf57f5

…into GGCOMPB

l1 auto align

1014c22

ee

0d2405e

Merge branch 'GGCOMPB' of https://github.com/robototes/Reefscape2025 …

f0f2c2f

…into GGCOMPB

c

e00f827

super jank code to get auto l1 auto extake

088c917

c

e1b832a

ds changes

c183519

quick hand off fix

0bcbd20

Merge branch 'GGCOMPB' of https://github.com/robototes/Reefscape2025 …

3212b7e

…into GGCOMPB

log + spot

b2d6687

Girls gen stuff added

061e0ee

markpete requested changes Oct 20, 2025

View reviewed changes

src/main/java/frc/robot/subsystems/auto/AutoAlign.java Show resolved Hide resolved

SnappleRamen added 2 commits October 21, 2025 23:07

Added smoothing factor to claw position(DEBUG AND TESTING WIP)

1ce5960

Merge branch 'Mechanisms' of https://github.com/robototes/Reefscape2025…

c9eb0b1

… into Mechanisms

markpete previously approved these changes Oct 24, 2025

View reviewed changes

Claw position fixed, elevator height offset kinda working WIP

6aab30c

SnappleRamen dismissed markpete’s stale review via 6aab30c October 29, 2025 03:47

SnappleRamen added 3 commits November 6, 2025 19:41

Added positional smoothing

5fa4bb0

Sort of decent claw

db5ca1e

presets?

Better debug elevator presets

c59dc10

gemini-code-assist bot reviewed Nov 21, 2025

View reviewed changes

		if (!Robot.getInstance().sensors.armSensor.booleanInClaw()) {
		}

Mechanism2D in AdvantageScope testing #151

Are you sure you want to change the base?

Mechanism2D in AdvantageScope testing #151

Uh oh!

Conversation

RobototesProgrammers commented Jun 4, 2025

Uh oh!

jamesdooley4 Sep 30, 2025

Choose a reason for hiding this comment

Uh oh!

iamawesomecat Sep 30, 2025

Choose a reason for hiding this comment

Uh oh!

jamesdooley4 Sep 30, 2025

Choose a reason for hiding this comment

Uh oh!

jamesdooley4 Sep 30, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

jamesdooley4 Sep 30, 2025

Choose a reason for hiding this comment

Uh oh!

jamesdooley4 Oct 6, 2025

Choose a reason for hiding this comment

Uh oh!

Uh oh!

markpete commented Nov 21, 2025

Uh oh!

gemini-code-assist bot left a comment

Choose a reason for hiding this comment

Code Review

Uh oh!

gemini-code-assist bot Nov 21, 2025

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Nov 21, 2025

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Nov 21, 2025

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Nov 21, 2025

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Nov 21, 2025

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Nov 21, 2025

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Nov 21, 2025

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Nov 21, 2025

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Nov 21, 2025

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Nov 21, 2025

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

7 participants

jamesdooley4 Sep 30, 2025 •

edited

Loading