feat(vision): use megatag2 for LL and working LL diagnostics

src/main/java/com/frcteam3636/frc2024/Dashboard.kt

@@ -4,10 +4,31 @@ import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain
 import com.frcteam3636.frc2024.subsystems.shooter.Shooter
 import edu.wpi.first.wpilibj.RobotController
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
+import java.net.InetAddress
 import kotlin.concurrent.thread
 
 object Dashboard {
+    private val limelightAddrs = ArrayList<String>()
+    private var limelightsConnected = true
+
     init {
+        limelightAddrs.add("10.36.36.11") // TODO: Think of a better way to do this
+
+        thread(isDaemon = true) {
+            while (true) {
+                var allConnected = true // Prevent the value from flickering
+                for (addr in limelightAddrs) {
+                    try {
+                        InetAddress.getByName(addr).isReachable(1000)
+                    } catch (err: Exception) {
+                        allConnected = false
+                        break
+                    }
+                }
+                limelightsConnected = allConnected
+            }
+        }
+
         thread(isDaemon = true) {
             val canDiagnostics = TalonFXDiagnosticCollector(Drivetrain, Shooter.Pivot)
             while (true) {
@@ -26,6 +47,6 @@ object Dashboard {
     fun update() {
         SmartDashboard.putBoolean("Battery Full", RobotController.getBatteryVoltage() >= 12.3)
         SmartDashboard.putBoolean("NavX OK", Drivetrain.gyroConnected)
-        SmartDashboard.putBoolean("All Cameras OK", Drivetrain.allCamerasConnected)
+        SmartDashboard.putBoolean("All Cameras OK", limelightsConnected && Drivetrain.allCamerasConnected)
     }
 }

src/main/java/com/frcteam3636/frc2024/subsystems/drivetrain/AbsolutePose.kt

@@ -15,7 +15,6 @@ import edu.wpi.first.math.util.Units
 import edu.wpi.first.networktables.NetworkTableInstance
 import edu.wpi.first.util.struct.Struct
 import edu.wpi.first.util.struct.StructSerializable
-import edu.wpi.first.wpilibj.Timer
 import org.littletonrobotics.junction.LogTable
 import org.littletonrobotics.junction.Logger
 import org.littletonrobotics.junction.inputs.LoggableInputs
@@ -51,22 +50,17 @@ class LimelightPoseIOReal(name: String) : AbsolutePoseIO {
         .getTable(name)
     private val stddev = VecBuilder.fill(.7, .7, 9999999.0)
 
-    private val botPose = table.getDoubleArrayTopic("botpose_wpiblue").subscribe(null)
+    private val botPose = table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(null)
     private val cl = table.getDoubleTopic("cl").subscribe(0.0)
     private val tl = table.getDoubleTopic("tl").subscribe(0.0)
-    private val disconnectTimeout = Timer().apply {
-        start()
-    }
 
     override fun updateInputs(inputs: AbsolutePoseIO.Inputs) {
-//        val entries = DoubleArray(6)
-//        entries[0] = Drivetrain.estimatedPose.rotation.degrees
-//        entries[1] = 0.0
-//        entries[2] = 0.0
-//        entries[3] = 0.0
-//        entries[4] = 0.0
-//        entries[5] = 0.0
-//        table.getEntry("robot_orientation_set").setDoubleArray(entries)
+        val entries = DoubleArray(6)
+        for (i in entries.indices) {
+            entries[i] = 0.0
+        }
+        entries[0] = Drivetrain.estimatedPose.rotation.degrees
+        table.getEntry("robot_orientation_set").setDoubleArray(entries)
         inputs.measurement = botPose.readQueue().lastOrNull()?.let { update ->
             val x = update.value[0]
             val y = update.value[1]
@@ -76,8 +70,6 @@ class LimelightPoseIOReal(name: String) : AbsolutePoseIO {
             val yaw = Units.degreesToRadians(update.value[5])
             val tagCount = update.value[7]
 
-            disconnectTimeout.restart()
-
             if (tagCount == 0.0 || Drivetrain.gyroRate > 720) {
                 return
             }
@@ -95,8 +87,7 @@ class LimelightPoseIOReal(name: String) : AbsolutePoseIO {
         }
     }
 
-    override val cameraConnected
-        get() = disconnectTimeout.hasElapsed(1.0)
+    override val cameraConnected = true // we determine this value in a ping thread
 }
 
 @Suppress("unused")