Skip to content

Mechanism2D in AdvantageScope testing #151

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 5 commits into
base: main
Choose a base branch
from
Open

Mechanism2D in AdvantageScope testing #151

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
0d38f0a
Added Mechanism2D display of claw, arm pivot, and elevator
RobototesProgrammers May 30, 2025
517f0d6
Spotless
RobototesProgrammers May 30, 2025
ed09bbe
Merge branch 'main' into Mechanisms
Jetblackdragon Aug 26, 2025
83839ad
Added mechanism config file to robot code for easy access WIP
RobototesProgrammers Oct 3, 2025
5c5fb1f
Added networktables entry for elevator pose3d WIP
RobototesProgrammers Oct 3, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 26 additions & 1 deletion src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

import au.grapplerobotics.CanBridge;
import com.pathplanner.lib.commands.FollowPathCommand;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DataLogManager;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.PowerDistribution;
Expand All @@ -14,7 +15,12 @@
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj.util.Color8Bit;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.robot.Subsystems.SubsystemConstants;
import frc.robot.subsystems.SuperStructure;
Expand All @@ -37,11 +43,15 @@ public static Robot getInstance() {
private final RobotType robotType;
public final Controls controls;
public final Subsystems subsystems;

public final Sensors sensors;
public final SuperStructure superStructure;
private final PowerDistribution PDH;

Mechanism2d mech;
MechanismRoot2d root;
MechanismLigament2d m_elevator;
MechanismLigament2d m_wrist;
Comment on lines +50 to +53
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.


protected Robot() {
// non public for singleton. Protected so test class can subclass

Expand All @@ -51,6 +61,19 @@ protected Robot() {
PDH = new PowerDistribution(Hardware.PDH_ID, ModuleType.kRev);
LiveWindow.disableAllTelemetry();
LiveWindow.enableTelemetry(PDH);
mech = new Mechanism2d(1, 2);
root = mech.getRoot("climber", 0.5 + Units.inchesToMeters(5.5), Units.inchesToMeters(19.5));
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Funnel pivot?

SmartDashboard.putData("Mechanism", mech);
m_elevator =
root.append(new MechanismLigament2d("elevator", 1, 90, 2, new Color8Bit(Color.kRed)));
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

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)));

sensors = new Sensors();
subsystems = new Subsystems(sensors);
Expand Down Expand Up @@ -115,6 +138,8 @@ public void robotPeriodic() {
if (subsystems.visionSubsystem != null) {
subsystems.visionSubsystem.update();
}
m_elevator.setLength(subsystems.elevatorSubsystem.getHeightMeters());
m_wrist.setAngle(subsystems.armPivotSubsystem.getAngle());
}

@Override
Expand Down
5 changes: 5 additions & 0 deletions src/main/java/frc/robot/subsystems/ArmPivot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.filter.Debouncer;
import edu.wpi.first.math.filter.Debouncer.DebounceType;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.measure.Voltage;
import edu.wpi.first.wpilibj.Alert;
Expand Down Expand Up @@ -146,6 +147,10 @@ private double getCurrentPosition() {
return curPos.getValueAsDouble();
}

public Rotation2d getAngle() { // 0 is forward, + is up
return Rotation2d.fromRotations(getCurrentPosition());
}
// preset command placeholder
/* moves arm to the input position
- sets the target position to the inputted position
- shrinks the difference between the current position and the target position until they are close enough to work
Expand Down
18 changes: 17 additions & 1 deletion src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,11 @@
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.filter.Debouncer;
import edu.wpi.first.math.filter.Debouncer.DebounceType;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StructPublisher;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.MutVoltage;
import edu.wpi.first.units.measure.Voltage;
Expand Down Expand Up @@ -52,6 +57,7 @@ public class ElevatorSubsystem extends SubsystemBase {
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
Copy link
Member

@jamesdooley4 jamesdooley4 Sep 30, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

public static final double MANUAL = 0.1;
private static final double POS_TOLERANCE = 0.1;
private final double ELEVATOR_KP = 7.804;
Expand Down Expand Up @@ -90,7 +96,8 @@ public class ElevatorSubsystem extends SubsystemBase {
new Alert("Elevator", "Motor 2 not connected", AlertType.kError);
private final Debouncer notConnectedDebouncerOne = new Debouncer(.1, DebounceType.kBoth);
private final Debouncer notConnectedDebouncerTwo = new Debouncer(.1, DebounceType.kBoth);

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

// Creates a SysIdRoutine
SysIdRoutine routine =
new SysIdRoutine(
Expand All @@ -107,6 +114,8 @@ public ElevatorSubsystem() {
motorConfigs();

Shuffleboard.getTab("Elevator").addDouble("Motor Current Position", () -> getCurrentPosition());
//Elevator pose test

Shuffleboard.getTab("Elevator").addDouble("Target Position", () -> getTargetPosition());
Shuffleboard.getTab("Elevator")
.addDouble("M1 supply current", () -> m_motor.getSupplyCurrent().getValueAsDouble());
Expand All @@ -133,6 +142,7 @@ public ElevatorSubsystem() {
"M2 at reverse softstop", () -> m_motor2.getFault_ReverseSoftLimit().getValue());
Shuffleboard.getTab("Elevator")
.addDouble("Elevator Speed", () -> m_motor.getVelocity().getValueAsDouble());

}

public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
Expand Down Expand Up @@ -249,6 +259,10 @@ private double getCurrentPosition() {
return curPos;
}

public double getHeightMeters() { // Elevator height converted to Meters
return getCurrentPosition() / MOTOR_ROTATIONS_PER_METER;
}

private void setCurrentPosition(double pos) {
m_motor.setPosition(pos);
}
Expand Down Expand Up @@ -370,6 +384,8 @@ public void periodic() {
if (RobotBase.isSimulation()) {
m_motorOneSimState.setRawRotorPosition(targetPos);
m_motorTwoSimState.setRawRotorPosition(targetPos);

elevatorPose3d.set(new Pose3d(new Pose2d(0.0, getHeightMeters(), new Rotation2d(0.0,0.0))));
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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)

}
}
}
Loading