From c221d62815931b01db4b4ccba638d7c6d89b531d Mon Sep 17 00:00:00 2001
From: Keegan Jamison Welch <114922650+Keegan-Welch@users.noreply.github.com>
Date: Sat, 27 Jan 2024 15:23:59 -0500
Subject: [PATCH] add glass Mechanism2d integration

We added the mechanism2d, and made the simulation function so that on the display, the arm revolves around the center.

Co-Authored-By: lydiahays <52471248+lydiahays@users.noreply.github.com>
---
 src/main/java/org/team340/robot/Robot.java    |  4 +++-
 .../org/team340/robot/RobotContainer.java     |  4 ++++
 .../org/team340/robot/subsystems/Shooter.java | 24 +++++++++++++++++++
 3 files changed, 31 insertions(+), 1 deletion(-)

diff --git a/src/main/java/org/team340/robot/Robot.java b/src/main/java/org/team340/robot/Robot.java
index c82d193..643e14d 100644
--- a/src/main/java/org/team340/robot/Robot.java
+++ b/src/main/java/org/team340/robot/Robot.java
@@ -68,5 +68,7 @@ public void testPeriodic() {}
     public void simulationInit() {}
 
     @Override
-    public void simulationPeriodic() {}
+    public void simulationPeriodic() {
+        RobotContainer.simulationPeriodic();
+    }
 }
diff --git a/src/main/java/org/team340/robot/RobotContainer.java b/src/main/java/org/team340/robot/RobotContainer.java
index 1ab1e0f..c4983b4 100644
--- a/src/main/java/org/team340/robot/RobotContainer.java
+++ b/src/main/java/org/team340/robot/RobotContainer.java
@@ -150,4 +150,8 @@ private static double getDriveY() {
     private static double getDriveRotate() {
         return driver.getTriggerDifference(ControllerConstants.DRIVE_ROT_MULTIPLIER, ControllerConstants.DRIVE_ROT_EXP);
     }
+
+    public static void simulationPeriodic() {
+        shooter.simulationPeriodic();
+    }
 }
diff --git a/src/main/java/org/team340/robot/subsystems/Shooter.java b/src/main/java/org/team340/robot/subsystems/Shooter.java
index cb7db78..d5167b8 100644
--- a/src/main/java/org/team340/robot/subsystems/Shooter.java
+++ b/src/main/java/org/team340/robot/subsystems/Shooter.java
@@ -11,6 +11,12 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.DriverStation;
+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 edu.wpi.first.wpilibj2.command.Command;
 import java.util.function.Supplier;
 import org.team340.lib.GRRSubsystem;
@@ -49,6 +55,15 @@ public class Shooter extends GRRSubsystem {
     private final RelativeEncoder pivotRelativeEncoder = pivotMotor.getEncoder();
 
     private final DigitalInput noteDetector = createDigitalInput("Note Detector", RobotMap.SHOOTER_NOTE_DETECTOR);
+    
+    // Create a Mechanism2d display of an Arm with a fixed ArmTower and moving Arm.
+    int simulationAngle = 0;
+    private static final Mechanism2d m_mech2d = new Mechanism2d(60, 60);
+    private static final MechanismRoot2d m_armPivot = m_mech2d.getRoot("ArmPivot", 30, 30);
+    private final MechanismLigament2d m_armTower = m_armPivot.append(new MechanismLigament2d("ArmTower", 30, -90));
+    private static final MechanismLigament2d m_arm = m_armPivot.append(
+        new MechanismLigament2d("Arm", 30, 30, 6, new Color8Bit(Color.kYellow))
+    );
 
     public Shooter() {
         super("Shooter");
@@ -88,6 +103,9 @@ public Shooter() {
             )
             .setIZone(ShooterConstants.SHOOTER_RIGHT_SHOOT_PID.iZone())
             .apply(rightShootMotor, rightShootPID);
+
+        SmartDashboard.putData("Mech2d", m_mech2d);
+        m_armTower.setColor(new Color8Bit(Color.kBlue));
     }
 
     /**
@@ -257,4 +275,10 @@ public Command spit() {
         //TODO: write this
         return null;
     }
+
+    public void simulationPeriodic() {
+        // Update the Mechanism Arm angle based on the simulated arm angle
+        simulationAngle += 1;
+        m_arm.setAngle(simulationAngle);
+    }
 }