migrate: smartdashboard to shuffleboard (#105)

Author: KawiMinn

.Glass/glass.json

@@ -0,0 +1,26 @@
+{
+  "NetworkTables Info": {
+    "visible": true
+  },
+  "NetworkTables Log": {
+    "visible": true
+  },
+  "Plots": {
+    "Plot <0>": {
+      "plots": [
+        {
+          "backgroundColor": [
+            0.0,
+            0.0,
+            0.0,
+            0.8500000238418579
+          ],
+          "height": 332
+        }
+      ],
+      "window": {
+        "visible": false
+      }
+    }
+  }
+}

pybot/telemetry.py

@@ -1,8 +1,9 @@
 from ntcore import NetworkTableInstance
 from phoenix6 import SignalLogger, swerve, units
-from wpilib import Color, Color8Bit, Mechanism2d, MechanismLigament2d, SmartDashboard
+from wpilib import Color, Color8Bit, Mechanism2d, MechanismLigament2d, SmartDashboard, Field2d
 from wpimath.geometry import Pose2d
 from wpimath.kinematics import ChassisSpeeds, SwerveModulePosition, SwerveModuleState
+import time
 
 class Telemetry:
     def __init__(self, max_speed: units.meters_per_second):
@@ -17,6 +18,11 @@ def __init__(self, max_speed: units.meters_per_second):
 
         # What to publish over networktables for telemetry
         self._inst = NetworkTableInstance.getDefault()
+        self._inst.startServer()  # Start NetworkTables server for simulation
+
+        # Initialize Field2D object
+        self._field = Field2d()
+        SmartDashboard.putData("Field", self._field)
 
         # Robot swerve drive state
         self._drive_state_table = self._inst.getTable("DriveState")
@@ -107,10 +113,13 @@ def telemeterize(self, state: swerve.SwerveDrivetrain.SwerveDriveState):
         self._field_type_pub.set("Field2d")
         self._field_pub.set(pose_array)
 
+        # Update the Field2d object with the robot's pose
+        self._field.setRobotPose(state.pose)
+
         # Telemeterize the module states to a Mechanism2d
         for i, module_state in enumerate(state.module_states):
             self._module_speeds[i].setAngle(module_state.angle.degrees())
             self._module_directions[i].setAngle(module_state.angle.degrees())
             self._module_speeds[i].setLength(module_state.speed / (2 * self._max_speed))
 
-            SmartDashboard.putData(f"Module {i}", self._module_mechanisms[i])
+            SmartDashboard.putData(f"Module {i}", self._module_mechanisms[i])