Skip to content

Commit af39331

Browse files
dabeycorndabeycorn
committed
refactor: replace subsystem visualizers with global singleton visualizer
Signed-off-by: Dasun Abeykoon <[email protected]>
1 parent ae87420 commit af39331

File tree

5 files changed

+93
-54
lines changed

5 files changed

+93
-54
lines changed

‎src/main/java/org/frc6423/robot/subsystems/superstructure/Visualizer.java‎

Lines changed: 79 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,79 @@
1+
// Copyright (c) 2025 FRC 6423 - Ward Melville Iron Patriots
2+
// https://github.com/wmironpatriots
3+
//
4+
// Open Source Software; you can modify and/or share it under the terms of
5+
// MIT license file in the root directory of this project
6+
7+
package org.frc6423.robot.subsystems.superstructure;
8+
9+
import static edu.wpi.first.units.Units.Centimeters;
10+
import static org.frc6423.robot.subsystems.superstructure.arm.ArmPivot.*;
11+
import static org.frc6423.robot.subsystems.superstructure.elevator.Elevator.MAX_EXTENSION_HEIGHT;
12+
13+
import edu.wpi.first.math.geometry.Rotation2d;
14+
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
15+
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
16+
import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
17+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
18+
import edu.wpi.first.wpilibj.util.Color;
19+
import edu.wpi.first.wpilibj.util.Color8Bit;
20+
21+
public class Visualizer {
22+
private static Visualizer instance;
23+
24+
/**
25+
* @return {@link Visualizer} singleton
26+
*/
27+
public static Visualizer getInstance() {
28+
if (instance == null) {
29+
instance = new Visualizer();
30+
}
31+
32+
return instance;
33+
}
34+
35+
private final Mechanism2d mech2d =
36+
new Mechanism2d(
37+
MAX_EXTENSION_HEIGHT.in(Centimeters), MAX_EXTENSION_HEIGHT.in(Centimeters) * 2 + 10);
38+
39+
// Elevator Roots
40+
private final MechanismRoot2d baseRoot =
41+
mech2d.getRoot("base", MAX_EXTENSION_HEIGHT.in(Centimeters) / 2, 0.0);
42+
private final MechanismRoot2d stageRoot =
43+
mech2d.getRoot("stageRoot", (MAX_EXTENSION_HEIGHT.in(Centimeters) / 2) - 1, 0.0);
44+
private final MechanismRoot2d carriageRoot =
45+
mech2d.getRoot("carriageRoot", (MAX_EXTENSION_HEIGHT.in(Centimeters) / 2) - 2, 0.0);
46+
47+
// Arm Ligament
48+
private final MechanismLigament2d arm;
49+
50+
private Visualizer() {
51+
// Setup elevator ligaments
52+
baseRoot.append(
53+
new MechanismLigament2d("base", 81.43936976, 90.0, 10.0, new Color8Bit(Color.kRed)));
54+
stageRoot.append(
55+
new MechanismLigament2d("stage", 83.82, 90.0, 7.0, new Color8Bit(Color.kYellow)));
56+
var carriageLig =
57+
carriageRoot.append(
58+
new MechanismLigament2d("carriage", 14.083, 90.0, 4.5, new Color8Bit(Color.kGreen)));
59+
60+
arm =
61+
carriageLig.append(
62+
new MechanismLigament2d(
63+
"arm", LENGTH.in(Centimeters), 0.0, 10, new Color8Bit(Color.kAliceBlue)));
64+
65+
SmartDashboard.putData("GlobalVisualizer", mech2d);
66+
}
67+
68+
public void setStageHeight(double heightCentimeters) {
69+
stageRoot.setPosition((MAX_EXTENSION_HEIGHT.in(Centimeters) / 2) - 2, heightCentimeters);
70+
}
71+
72+
public void setCarriageHeight(double heightCentimeters) {
73+
carriageRoot.setPosition((MAX_EXTENSION_HEIGHT.in(Centimeters) / 2) - 4, heightCentimeters);
74+
}
75+
76+
public void setArmAngle(double angleRads) {
77+
arm.setAngle(Rotation2d.fromRadians(angleRads).unaryMinus().plus(Rotation2d.kCCW_90deg));
78+
}
79+
}

‎src/main/java/org/frc6423/robot/subsystems/superstructure/arm/Arm.java‎

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,7 @@ public class Arm extends SubsystemBase {
3535
/**
3636
* @return fake {@link Arm} subsystem
3737
*/
38-
public Arm none() {
38+
public static Arm none() {
3939
return new Arm(new ArmPivotIONone(), new RollerIONone());
4040
}
4141

@@ -44,7 +44,7 @@ public Arm none() {
4444
*
4545
* @return {@link Arm} Subsystem
4646
*/
47-
public Arm create() {
47+
public static Arm create() {
4848
if (Robot.isReal()) {
4949
return new Arm(new ArmPivotIOReal(), new RollerIONeo());
5050
} else {
@@ -83,7 +83,9 @@ public boolean hasCoralVectored() {
8383
*/
8484
public Command runState(Angle pivotAngle, AngularVelocity rollerSpeed) {
8585
return Commands.parallel(
86-
pivot.runAngle(pivotAngle.in(Radians)), roller.runSpeed(rollerSpeed.in(RPM)));
86+
this.run(() -> {}).until(() -> true),
87+
pivot.runAngle(pivotAngle.in(Radians)),
88+
roller.runSpeed(rollerSpeed.in(RPM)));
8789
}
8890

8991
/**
@@ -95,6 +97,7 @@ public Command runState(Angle pivotAngle, AngularVelocity rollerSpeed) {
9597
*/
9698
public Command runState(DoubleSupplier pivotAngleRads, DoubleSupplier rollerSpeedRpm) {
9799
return Commands.parallel(
100+
this.run(() -> {}).until(() -> true),
98101
pivot.runAngle(pivotAngleRads.getAsDouble()),
99102
roller.runSpeed(rollerSpeedRpm.getAsDouble()));
100103
}

‎src/main/java/org/frc6423/robot/subsystems/superstructure/arm/ArmPivot.java‎

Lines changed: 3 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,6 @@
66

77
package org.frc6423.robot.subsystems.superstructure.arm;
88

9-
import static edu.wpi.first.units.Units.Centimeters;
109
import static edu.wpi.first.units.Units.Degrees;
1110
import static edu.wpi.first.units.Units.Inches;
1211
import static edu.wpi.first.units.Units.KilogramSquareMeters;
@@ -15,19 +14,13 @@
1514
import edu.wpi.first.epilogue.Logged;
1615
import edu.wpi.first.math.MathUtil;
1716
import edu.wpi.first.math.filter.LinearFilter;
18-
import edu.wpi.first.math.geometry.Rotation2d;
1917
import edu.wpi.first.units.measure.Angle;
2018
import edu.wpi.first.units.measure.Distance;
2119
import edu.wpi.first.units.measure.MomentOfInertia;
22-
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
23-
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
24-
import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
25-
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
26-
import edu.wpi.first.wpilibj.util.Color;
27-
import edu.wpi.first.wpilibj.util.Color8Bit;
2820
import edu.wpi.first.wpilibj2.command.Command;
2921
import edu.wpi.first.wpilibj2.command.SubsystemBase;
3022
import java.util.function.DoubleSupplier;
23+
import org.frc6423.robot.subsystems.superstructure.Visualizer;
3124

3225
/** Pivot subsystem-component of the {@link Arm} Subsystem */
3326
public class ArmPivot extends SubsystemBase implements AutoCloseable {
@@ -63,20 +56,10 @@ public class ArmPivot extends SubsystemBase implements AutoCloseable {
6356

6457
private boolean isZeroed = false;
6558

66-
// Visualizer
67-
private final Mechanism2d mech2d =
68-
new Mechanism2d(LENGTH.in(Centimeters), LENGTH.in(Centimeters) * 2);
69-
private final MechanismRoot2d root =
70-
mech2d.getRoot("pivot", LENGTH.in(Centimeters), LENGTH.in(Centimeters));
71-
private final MechanismLigament2d arm =
72-
root.append(
73-
new MechanismLigament2d(
74-
"arm", LENGTH.in(Centimeters), 0.0, 10, new Color8Bit(Color.kAliceBlue)));
59+
private final Visualizer visualizer = Visualizer.getInstance();
7560

7661
public ArmPivot(ArmPivotIO hardware) {
7762
this.hardware = hardware;
78-
79-
SmartDashboard.putData("ArmVisualizer", mech2d);
8063
}
8164

8265
@Override
@@ -85,8 +68,7 @@ public void periodic() {
8568

8669
filteredPivotCurrent = pivotCurrentFilter.calculate(hardware.getStatorCurrentAmps());
8770

88-
arm.setAngle(
89-
Rotation2d.fromRadians(hardware.getAngleRads()).unaryMinus().rotateBy(Rotation2d.k180deg));
71+
visualizer.setArmAngle(hardware.getAngleRads());
9072
}
9173

9274
/**

‎src/main/java/org/frc6423/robot/subsystems/superstructure/arm/ArmPivotIOSim.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,7 @@ public class ArmPivotIOSim implements ArmPivotIO {
3535
private double pivotAppliedVolts;
3636

3737
private final ProfiledPIDController pivotFeedback =
38-
new ProfiledPIDController(10, 0.0, 0.0, new TrapezoidProfile.Constraints(3.5, 3.5));
38+
new ProfiledPIDController(20, 0.0, 0.0, new TrapezoidProfile.Constraints(5.5, 17));
3939

4040
public ArmPivotIOSim() {
4141
SmartDashboard.putData(pivotFeedback);

‎src/main/java/org/frc6423/robot/subsystems/superstructure/elevator/Elevator.java‎

Lines changed: 4 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -22,16 +22,11 @@
2222
import edu.wpi.first.units.measure.LinearAcceleration;
2323
import edu.wpi.first.units.measure.LinearVelocity;
2424
import edu.wpi.first.units.measure.Mass;
25-
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
26-
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
27-
import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
28-
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
29-
import edu.wpi.first.wpilibj.util.Color;
30-
import edu.wpi.first.wpilibj.util.Color8Bit;
3125
import edu.wpi.first.wpilibj2.command.Command;
3226
import edu.wpi.first.wpilibj2.command.SubsystemBase;
3327
import java.util.function.DoubleSupplier;
3428
import org.frc6423.robot.Robot;
29+
import org.frc6423.robot.subsystems.superstructure.Visualizer;
3530

3631
/** Elevator Subsystem */
3732
public class Elevator extends SubsystemBase implements AutoCloseable {
@@ -74,16 +69,7 @@ public class Elevator extends SubsystemBase implements AutoCloseable {
7469

7570
private boolean isZeroed = false;
7671

77-
// Visualizer
78-
private final Mechanism2d mech2d =
79-
new Mechanism2d(
80-
MAX_EXTENSION_HEIGHT.in(Centimeters), MAX_EXTENSION_HEIGHT.in(Centimeters) * 2 + 10);
81-
private final MechanismRoot2d baseRoot =
82-
mech2d.getRoot("base", MAX_EXTENSION_HEIGHT.in(Centimeters) / 2, 0.0);
83-
private final MechanismRoot2d stageRoot =
84-
mech2d.getRoot("stageRoot", (MAX_EXTENSION_HEIGHT.in(Centimeters) / 2) - 1, 0.0);
85-
private final MechanismRoot2d carriageRoot =
86-
mech2d.getRoot("carriageRoot", (MAX_EXTENSION_HEIGHT.in(Centimeters) / 2) - 2, 0.0);
72+
private final Visualizer visualizer = Visualizer.getInstance();
8773

8874
/**
8975
* @return fake {@link Elevator} subsystem
@@ -111,15 +97,6 @@ public static Elevator create() {
11197

11298
private Elevator(ElevatorIO hardware) {
11399
this.hardware = hardware;
114-
115-
baseRoot.append(
116-
new MechanismLigament2d("base", 81.43936976, 90.0, 10.0, new Color8Bit(Color.kRed)));
117-
stageRoot.append(
118-
new MechanismLigament2d("stage", 83.82, 90.0, 7.0, new Color8Bit(Color.kYellow)));
119-
carriageRoot.append(
120-
new MechanismLigament2d("carriage", 17.78, 90.0, 4.5, new Color8Bit(Color.kGreen)));
121-
122-
SmartDashboard.putData("Elevator Visualizer", mech2d);
123100
}
124101

125102
@Override
@@ -129,10 +106,8 @@ public void periodic() {
129106
filteredCurrent = currentFilter.calculate(hardware.getParentStatorCurrentAmps());
130107

131108
/** Set visualizer poses */
132-
stageRoot.setPosition(
133-
(MAX_EXTENSION_HEIGHT.in(Centimeters) / 2) - 2, getStageHeight().in(Centimeters));
134-
carriageRoot.setPosition(
135-
(MAX_EXTENSION_HEIGHT.in(Centimeters) / 2) - 4, getCarriageHeight().in(Centimeters));
109+
visualizer.setCarriageHeight(getCarriageHeight().in(Centimeters));
110+
visualizer.setStageHeight(getStageHeight().in(Centimeters));
136111
}
137112

138113
/**

0 commit comments

Comments
 (0)