Added Mechanism2d stuff

src/main/java/frc/robot/Robot.java

@@ -57,6 +57,8 @@ public void robotPeriodic() {
     // This must be called from the robot's periodic block in order for anything in
     // the Command-based framework to work.
     CommandScheduler.getInstance().run();
+
+    robotContainer.updateRobotState();
   }
 
   /** This function is called once when the robot is disabled. */

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

@@ -38,6 +38,8 @@
  * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
+  private RobotState m_robotState;
+
   // Subsystems
   private Drive m_drive;
   private Shooter m_shooter;
@@ -86,6 +88,10 @@ private void configureSubsystems() {
               ShooterConstants.kPivotController,
               ShooterConstants.kFlywheelController);
     }
+
+    m_shooter.setPivotAngle(ShooterConstants.kHomeAngle);
+
+    m_robotState = RobotState.startInstance(m_drive, m_shooter);
   }
 
   private void configureControllers() {
@@ -106,7 +112,7 @@ private void configureButtonBindings() {
             Commands.runOnce(
                 () -> {
                   m_shooter.setFlywheelVelocity(2, 4);
-                  m_shooter.setPivotAngle(Rotation2d.fromDegrees(50));
+                  m_shooter.setPivotAngle(Rotation2d.fromDegrees(60));
                 }))
         .onFalse(
             Commands.runOnce(
@@ -115,4 +121,8 @@ private void configureButtonBindings() {
                   m_shooter.setPivotAngle(ShooterConstants.kHomeAngle);
                 }));
   }
+
+  public void updateRobotState() {
+    m_robotState.updateRobotState();
+  }
 }

src/main/java/frc/robot/RobotState.java

@@ -0,0 +1,69 @@
+package frc.robot;
+
+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 frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.shooter.Shooter;
+
+public class RobotState {
+  private static RobotState instance = null;
+
+  // subsystems
+  private Drive m_drive;
+  private Shooter m_shooter;
+
+  // mechanism
+  private final boolean kMechanismEnabled = true;
+  MechanismLigament2d m_shooterPivotMech;
+
+  private RobotState(Drive drive, Shooter shooter) {
+    // subsystems
+    m_drive = drive;
+    m_shooter = shooter;
+
+    // mechanism set up
+    Mechanism2d m_mechanism = new Mechanism2d(3, 3);
+    MechanismRoot2d m_root = m_mechanism.getRoot("shooter", 3, 1);
+    m_shooterPivotMech =
+        m_root.append(
+            new MechanismLigament2d(
+                "pivot",
+                1,
+                m_shooter.getPivotAngle().getDegrees(),
+                6,
+                new Color8Bit(Color.kPurple)));
+
+    // post to dashboard
+    SmartDashboard.putData("Mech2d", m_mechanism);
+  }
+
+  public static RobotState startInstance(Drive drive, Shooter shooter) {
+    if (instance == null) {
+      instance = new RobotState(drive, shooter);
+    } else {
+      throw new IllegalStateException("RobotState instance already started");
+    }
+    return instance;
+  }
+
+  public static RobotState getInstance() {
+    if (instance == null) {
+      throw new IllegalStateException("RobotState instance not started");
+    }
+    return instance;
+  }
+
+  public void updateMechanism() {
+    m_shooterPivotMech.setAngle(m_shooter.getPivotAngle().getDegrees());
+  }
+
+  public void updateRobotState() {
+    if (kMechanismEnabled) {
+      updateMechanism();
+    }
+  }
+}

src/main/java/frc/robot/oi/DriverControlsXbox.java

@@ -27,6 +27,6 @@ public double getDriveRotation() {
 
   @Override
   public Trigger testShooter() {
-    return m_controller.leftTrigger();
+    return m_controller.button(7);
   }
 }

src/main/java/frc/robot/subsystems/shooter/Shooter.java

@@ -17,8 +17,8 @@ public class Shooter extends SubsystemBase {
   private ShooterPivotIO m_pivotIO;
   private FlywheelIO m_flywheelIO;
 
-  private ShooterPivotInputsAutoLogged m_pivotInputs;
-  private FlywheelInputsAutoLogged m_flywheelInputs;
+  public final ShooterPivotInputsAutoLogged m_pivotInputs;
+  public final FlywheelInputsAutoLogged m_flywheelInputs;
 
   private ProfiledPIDController m_pivotController;
   private ProfiledPIDController m_leftFlywheelController;
@@ -114,4 +114,8 @@ public Command setPivotAngleCommand(Rotation2d angle) {
   public Command setFlywheelVelocityCommand(double leftRPM, double rightRPM) {
     return Commands.runOnce(() -> setFlywheelVelocity(leftRPM, rightRPM));
   }
+
+  public Rotation2d getPivotAngle() {
+    return Rotation2d.fromRadians(m_pivotInputs.curAngle);
+  }
 }