(12/28/25) Elevator Visualization

Added working visualization for the elevator. Not sure if I integrated it with IO layer correctly. Turns out Mechanism2d is not the way?

Author: rafaelbaird

src/main/java/frc/robot/RobotContainer.java

@@ -17,9 +17,8 @@
 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.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.generated.TunerConstants;
@@ -34,6 +33,7 @@
 import frc.robot.subsystems.rollers.RollersIOSim;
 import frc.robot.subsystems.rollers.RollersIOSparkMax;
 import frc.robot.util.PoseUtils;
+import frc.robot.util.RobotState;
 import frc.robot.util.simulation.MapleSimSwerveDrivetrain;
 import org.ironmaple.simulation.SimulatedArena;
 import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
@@ -107,12 +107,6 @@ public RobotContainer() {
     configureButtonBindings();
   }
 
-  /**
-   * Use this method to define your button->command mappings. Buttons can be created by
-   * instantiating a {@link GenericHID} or one of its subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
-   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
-   */
   private void configureButtonBindings() {
     // Default command, normal field-relative drive
     drivetrain.setDefaultCommand(drivetrain.driveJoystickInputCommand());
@@ -144,6 +138,54 @@ private void configureButtonBindings() {
     // zeros gyro
     driverController.touchpad().onTrue(drivetrain.zeroGyroCommand());
 
+    // L1 Manual
+    operatorController
+        .cross()
+        .and(operatorController.L2().negate())
+        .whileTrue(arm.holdState(RobotState.L1))
+        .onFalse(
+            elevator
+                .setState(RobotState.L1)
+                .alongWith(elevator.setTargetState(RobotState.L1))
+                .alongWith(arm.holdState(RobotState.L1)));
+
+    // L2 Manual
+    operatorController
+        .square()
+        .and(operatorController.L2().negate())
+        .whileTrue(arm.holdState(RobotState.L2))
+        .onFalse(
+            elevator
+                .setState(RobotState.L2)
+                .alongWith(elevator.setTargetState(RobotState.L2))
+                .alongWith(arm.holdState(RobotState.L2)));
+
+    // L3 Manual
+    operatorController
+        .triangle()
+        .and(operatorController.L2().negate())
+        .whileTrue((arm.holdState(RobotState.L3)))
+        .onFalse(
+            elevator
+                .setState(RobotState.L3)
+                .alongWith(elevator.setTargetState(RobotState.L3))
+                .alongWith(arm.holdState(RobotState.L3)));
+
+    operatorController
+        .touchpad()
+        .onTrue(
+            Commands.runOnce(
+                () -> {
+                  arm.setDefaultCommand(arm.joystickControlCommand());
+                }));
+
+    // L4 Manual
+    operatorController
+        .circle()
+        .and(operatorController.L2().negate())
+        .whileTrue(arm.holdState(RobotState.L4).alongWith(elevator.setState(RobotState.L3)))
+        .onFalse(elevator.setState(RobotState.L4).alongWith(arm.holdState(RobotState.L4)));
+
     drivetrain.registerTelemetry(logger::telemeterize);
   }
 
@@ -188,7 +230,5 @@ public void displaySimFieldToAdvantageScope() {
         "FieldSimulation/Coral", SimulatedArena.getInstance().getGamePiecesArrayByType("Coral"));
     Logger.recordOutput(
         "FieldSimulation/Algae", SimulatedArena.getInstance().getGamePiecesArrayByType("Algae"));
-    // Logger.recordOutput(
-    //         "FieldSimulation/Staged Algae", AlgaeHandler.getInstance().periodic());
   }
 }

src/main/java/frc/robot/subsystems/elevator/Elevator.java

@@ -21,6 +21,7 @@ public class Elevator extends SubsystemBase {
   // are simulating or not
   private final ElevatorIOInputsAutoLogged inputs =
       new ElevatorIOInputsAutoLogged(); // inputs, auto logged
+  private ElevatorVisualization visualizer;
 
   private double setpoint = 0; // keeps track of setpoint
 
@@ -29,13 +30,16 @@ public class Elevator extends SubsystemBase {
 
   public Elevator(ElevatorIO io) {
     this.io = io;
+    this.visualizer = new ElevatorVisualization();
     setDefaultCommand(Commands.run(this::joystickControl, this));
   }
 
   @Override
   public void periodic() {
     io.updateInputs(inputs);
     Logger.processInputs("Elevator", inputs);
+    visualizer.update(inputs);
+    visualizer.log();
   }
 
   /**

src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java

@@ -8,9 +8,6 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.simulation.ElevatorSim;
-import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import frc.robot.Constants.ElevatorConstants;
 
 public class ElevatorIOSim implements ElevatorIO {
@@ -31,11 +28,6 @@ public class ElevatorIOSim implements ElevatorIO {
   private ProfiledPIDController pidController;
   public ElevatorFeedforward elevatorFeedforward;
 
-  // Visualization
-  private Mechanism2d mechanism;
-  private MechanismRoot2d rootMechanism;
-  private MechanismLigament2d elevatorMechanism;
-
   // for modeling elevator movement
   private ElevatorSim sim =
       new ElevatorSim(
@@ -66,13 +58,6 @@ public ElevatorIOSim() {
             0,
             0.068398,
             new Constraints(ElevatorConstants.MAX_VELOCITY_RPS, ElevatorConstants.MAX_ACCEL_RPS));
-
-    // Visualization
-    mechanism = new Mechanism2d(4, 4);
-    rootMechanism = mechanism.getRoot("ElevatorBottom", 2, 0);
-    elevatorMechanism =
-        rootMechanism.append(
-            new MechanismLigament2d("Elevator", ElevatorConstants.STARTING_HEIGHT, 90));
   }
 
   @Override
@@ -99,20 +84,16 @@ public void setPosition(double rotations) {
         pidController.calculate(
             sim.getPositionMeters() * ElevatorConstants.ROTATIONS_PER_METER, goalRotations);
 
-    // for calculating FF
     double accel =
         (pidController.getSetpoint().velocity * ElevatorConstants.METERS_PER_ROTATION
                 - lastVelocity)
             / (Timer.getFPGATimestamp() - lastTime);
 
-    // Updates values for calculating accel
     lastTime = Timer.getFPGATimestamp();
     lastVelocity = sim.getVelocityMetersPerSecond();
 
-    // FF
     double ffVoltage = elevatorFeedforward.calculate(pidController.getSetpoint().velocity, accel);
 
-    // Adds FF + PID voltages
     double outputVoltage = MathUtil.clamp(pidOutputVoltage + ffVoltage, -12, 12);
 
     appliedVoltage = outputVoltage; // logs voltage

src/main/java/frc/robot/subsystems/elevator/ElevatorVisualization.java

@@ -0,0 +1,33 @@
+package frc.robot.subsystems.elevator;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import frc.robot.Constants.ElevatorConstants;
+import frc.robot.subsystems.elevator.ElevatorIO.ElevatorIOInputs;
+import org.littletonrobotics.junction.Logger;
+
+public class ElevatorVisualization {
+
+  private Pose3d stage2Pose;
+  private Pose3d stage3Pose;
+
+  public void update(ElevatorIOInputs inputs) {
+    stage2Pose =
+        new Pose3d(
+            -0.103,
+            0,
+            0.14
+                + Math.max(
+                    0,
+                    inputs.extensionMeters
+                        + ElevatorConstants.STARTING_HEIGHT
+                        - ElevatorConstants.STAGE2_MAX_HEIGHT),
+            Rotation3d.kZero);
+    stage3Pose = new Pose3d(-0.103, 0, 0.165 + inputs.extensionMeters, Rotation3d.kZero);
+  }
+
+  public void log() {
+    Logger.recordOutput("Elevator/Stage2Pose", stage2Pose);
+    Logger.recordOutput("Elevator/Stage3Pose", stage3Pose);
+  }
+}