Refactor/subsystems

src/main/java/org/frc6423/lib/subsystems/RollerIONeo.java

@@ -0,0 +1,37 @@
+// 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.lib.subsystems;
+
+// TODO FINISH CLASS
+public class RollerIONeo implements RollerIO {
+  @Override
+  public void periodic() {}
+
+  @Override
+  public double getAppliedVolts() {
+    return 0.0;
+  }
+
+  @Override
+  public double getSpeedRpm() {
+    return 0.0;
+  }
+
+  @Override
+  public double getStatorCurrentAmps() {
+    return 0.0;
+  }
+
+  @Override
+  public void setVolts(double volts) {}
+
+  @Override
+  public void setSpeed(double speedRpm) {}
+
+  @Override
+  public void close() throws Exception {}
+}

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

@@ -0,0 +1,93 @@
+// 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 static org.frc6423.robot.subsystems.superstructure.arm.ArmPivot.*;
+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;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+public class Visualizer {
+  private static Visualizer instance;
+
+  /**
+   * @return {@link Visualizer} singleton
+   */
+  public static Visualizer getInstance() {
+    if (instance == null) {
+      instance = new Visualizer();
+    }
+
+    return instance;
+  }
+
+  private final Mechanism2d mech2d =
+      new Mechanism2d(
+          MAX_EXTENSION_HEIGHT.in(Centimeters), MAX_EXTENSION_HEIGHT.in(Centimeters) * 2 + 10);
+
+  // Elevator Roots
+  private final MechanismRoot2d baseRoot =
+      mech2d.getRoot("base", MAX_EXTENSION_HEIGHT.in(Centimeters) / 2, 0.0);
+  private final MechanismRoot2d stageRoot =
+      mech2d.getRoot("stageRoot", (MAX_EXTENSION_HEIGHT.in(Centimeters) / 2) - 1, 0.0);
+  private final MechanismRoot2d carriageRoot =
+      mech2d.getRoot("carriageRoot", (MAX_EXTENSION_HEIGHT.in(Centimeters) / 2) - 2, 0.0);
+
+  // Arm Ligament
+  private final MechanismLigament2d arm;
+
+  private Visualizer() {
+    // Setup elevator ligaments
+    baseRoot.append(
+        new MechanismLigament2d("base", 81.43936976, 90.0, 10.0, new Color8Bit(Color.kRed)));
+    stageRoot.append(
+        new MechanismLigament2d("stage", 83.82, 90.0, 7.0, new Color8Bit(Color.kYellow)));
+    var carriageLig =
+        carriageRoot.append(
+            new MechanismLigament2d("carriage", 14.083, 90.0, 4.5, new Color8Bit(Color.kGreen)));
+
+    arm =
+        carriageLig.append(
+            new MechanismLigament2d(
+                "arm", LENGTH.in(Centimeters), 0.0, 10, new Color8Bit(Color.kAliceBlue)));
+
+    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);
+  }
+
+  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));
+  }
+}

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

@@ -6,14 +6,55 @@
 
 package org.frc6423.robot.subsystems.superstructure.arm;
 
+import static edu.wpi.first.units.Units.RPM;
+import static edu.wpi.first.units.Units.Radians;
+
 import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.function.DoubleSupplier;
 import org.frc6423.lib.subsystems.RollerIO;
+import org.frc6423.lib.subsystems.RollerIONeo;
+import org.frc6423.lib.subsystems.RollerIONone;
+import org.frc6423.robot.Robot;
 
 /** Arm Subsystem */
 public class Arm extends SubsystemBase {
+  /** Represents a state the {@link Arm} subsystem can be in */
+  // TODO CHECK ROLLER SPEEDS
+  public static enum ArmState {
+    /** Resting state */
+    STOWED(Rotation2d.fromDegrees(90), 0.0),
+    /** Avoidance state for preventing collisions */
+    AVOIDING(Rotation2d.fromDegrees(65), 0.0),
+    /** Flipped state for intaking */
+    INTAKING(Rotation2d.fromDegrees(-90), 20.0),
+    /** L2 pose but not scoring */
+    L2_PRIMED(Rotation2d.fromDegrees(21.975), 0.0),
+    /** L3 pose but not scoring */
+    L3_PRIMED(Rotation2d.fromDegrees(21.975), 0.0),
+    /** L4 pose but not scoring */
+    L4_PRIMED(Rotation2d.fromDegrees(74.5), 0.0),
+    /** L2 scoring */
+    L2_SCORING(Rotation2d.fromDegrees(21.975), -40.0),
+    /** L3 scoring */
+    L3_SCORING(Rotation2d.fromDegrees(21.975), -40.0),
+    /** L4 scoring */
+    L4_SCORING(Rotation2d.fromDegrees(74.5), -40.0);
+
+    public final Rotation2d angle;
+    public final double rollerSpeedRpm;
+
+    private ArmState(Rotation2d angle, double rollerSpeedRpm) {
+      this.angle = angle;
+      this.rollerSpeedRpm = rollerSpeedRpm;
+    }
+  }
+
   @Logged(name = "Pivot Subsystem-Component")
   private final ArmPivot pivot;
 
@@ -23,7 +64,27 @@ public class Arm extends SubsystemBase {
   @Logged(name = "Arm Setpoint State")
   private ArmState setpointState = ArmState.STOWED;
 
-  public Arm(ArmPivotIO pivotHardware, RollerIO rollerHardware) {
+  /**
+   * @return fake {@link Arm} subsystem
+   */
+  public static Arm none() {
+    return new Arm(new ArmPivotIONone(), new RollerIONone());
+  }
+
+  /**
+   * Factory for creating a {@link Arm} subsystem
+   *
+   * @return {@link Arm} Subsystem
+   */
+  public static Arm create() {
+    if (Robot.isReal()) {
+      return new Arm(new ArmPivotIOReal(), new RollerIONeo());
+    } else {
+      return new Arm(new ArmPivotIOSim(), new RollerIONone());
+    }
+  }
+
+  private Arm(ArmPivotIO pivotHardware, RollerIO rollerHardware) {
     this.pivot = new ArmPivot(pivotHardware);
     this.roller = new ArmRoller(rollerHardware);
   }
@@ -38,6 +99,13 @@ public boolean isNearSetpointAngle() {
     return pivot.isNearSetpointAngle();
   }
 
+  /**
+   * @return {@link Rotation2d} representing the arm angle
+   */
+  public Rotation2d getRotation2d() {
+    return pivot.getRotation2d();
+  }
+
   /**
    * @return true if arm has vectored coral piece
    */
@@ -46,25 +114,51 @@ public boolean hasCoralVectored() {
   }
 
   /**
-   * Run arm to specified {@link ArmState}
+   * Run arm to specified state
    *
-   * @param state desired {@link ArmState}
+   * @param pivotAngle {@link Angle} representing desired pivot angle
+   * @param rollerSpeed {@link AngularVelocity} representing desired roller speed
    * @return {@link Command}
    */
-  public Command runState(ArmState state) {
+  public Command runState(Angle pivotAngle, AngularVelocity rollerSpeed) {
     return Commands.parallel(
-        this.run(() -> setpointState = state).until(() -> true),
-        pivot.runAngle(state.angle.getMeasure()),
-        roller.runSpeed(state.rollerSpeedRpm));
+        this.run(() -> {}).until(() -> true),
+        pivot.runAngle(pivotAngle.in(Radians)),
+        roller.runSpeed(rollerSpeed.in(RPM)));
   }
 
   /**
-   * Hold arm at current angle and roller speed
+   * Run arm to specified state
    *
+   * @param pivotAngleRads desired pivot angle in radians
+   * @param rollerSpeed desired roller speed in radians
    * @return {@link Command}
    */
-  public Command holdState() {
+  public Command runState(DoubleSupplier pivotAngleRads, DoubleSupplier rollerSpeedRpm) {
     return Commands.parallel(
-        this.run(() -> {}).until(() -> true), pivot.holdAngle(), roller.holdSpeed());
+        this.run(() -> {}).until(() -> true),
+        pivot.runAngle(pivotAngleRads.getAsDouble()),
+        roller.runSpeed(rollerSpeedRpm.getAsDouble()));
+  }
+
+  /**
+   * Run arm to specified state
+   *
+   * @param pivotAngleRads desired pivot angle in radians
+   * @param rollerSpeed desired roller speed in radians
+   * @return {@link Command}
+   */
+  public Command runState(double pivotAngleRads, double rollerSpeedRpm) {
+    return this.runState(() -> pivotAngleRads, () -> rollerSpeedRpm);
+  }
+
+  /**
+   * Run arm to specified {@link ArmState}
+   *
+   * @param state desired {@link ArmState}
+   * @return {@link Command}
+   */
+  public Command runState(ArmState state) {
+    return this.runState(state.angle.getRadians(), state.rollerSpeedRpm);
   }
 }