e

Signed-off-by: Dasun Abeykoon <[email protected]>

Author: dabeycorn

src/main/java/org/frc6423/robot/subsystems/superstructure/Superstructure.java

@@ -0,0 +1,30 @@
+// Copyright (c) 2025 FRC 6423 - Ward Melville Iron Patriots
+// https://github.com/wmironpatriots
+// 
+// Open Source Software; you can modify and/or share it under the terms of
+// MIT license file in the root directory of this project
+
+package org.frc6423.robot.subsystems.superstructure;
+
+import static edu.wpi.first.units.Units.Centimeters;
+
+import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.frc6423.robot.subsystems.superstructure.arm.Arm;
+import org.frc6423.robot.subsystems.superstructure.elevator.Elevator;
+
+public class Superstructure extends SubsystemBase {
+  @Logged private final Elevator elevator = Elevator.create();
+  @Logged private final Arm arm = Arm.create();
+
+  private final Visualizer visualizer = Visualizer.getInstance();
+
+  public Superstructure() {}
+
+  public void periodic() {
+    /** Set visualizer poses */
+    visualizer.setCarriageHeight(elevator.getCarriageHeight().in(Centimeters));
+    visualizer.setStageHeight(elevator.getStageHeight().in(Centimeters));
+    visualizer.setArmAngle(arm.getRotation2d());
+  }
+}

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

@@ -11,6 +11,7 @@
 import static org.frc6423.robot.subsystems.superstructure.elevator.Elevator.MAX_EXTENSION_HEIGHT;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
@@ -65,6 +66,15 @@ private Visualizer() {
     SmartDashboard.putData("GlobalVisualizer", mech2d);
   }
 
+  public void setStageHeight(Distance height) {
+    stageRoot.setPosition((MAX_EXTENSION_HEIGHT.in(Centimeters) / 2) - 2, height.in(Centimeters));
+  }
+
+  public void setCarriageHeight(Distance height) {
+    carriageRoot.setPosition(
+        (MAX_EXTENSION_HEIGHT.in(Centimeters) / 2) - 4, height.in(Centimeters));
+  }
+
   public void setStageHeight(double heightCentimeters) {
     stageRoot.setPosition((MAX_EXTENSION_HEIGHT.in(Centimeters) / 2) - 2, heightCentimeters);
   }
@@ -73,6 +83,10 @@ public void setCarriageHeight(double heightCentimeters) {
     carriageRoot.setPosition((MAX_EXTENSION_HEIGHT.in(Centimeters) / 2) - 4, heightCentimeters);
   }
 
+  public void setArmAngle(Rotation2d angle) {
+    arm.setAngle(angle.unaryMinus().plus(Rotation2d.kCCW_90deg));
+  }
+
   public void setArmAngle(double angleRads) {
     arm.setAngle(Rotation2d.fromRadians(angleRads).unaryMinus().plus(Rotation2d.kCCW_90deg));
   }