Limelight Stuff

Author: falOn-Dev

src/main/java/frc/robot/commands/vision/UpdateOdometryCommand.kt

@@ -21,9 +21,10 @@ class UpdateOdometryCommand : Command() {
     }
 
     override fun isFinished(): Boolean {
-        val position: Pose3d = LimelightSubsystem.botpose
-
-        SwerveSubsystem.addVisionMeasurement(VisionMeasurement(position))
+//        val position: Pose3d = LimelightSubsystem.botpose
+//
+//        SwerveSubsystem.addVisionMeasurement(VisionMeasurement(position))
+        SwerveSubsystem.addVisionMeasurement(LimelightSubsystem.poseMeasurement)
 
         return true
     }

src/main/java/frc/robot/subsystems/LimelightSubsystem.kt

@@ -6,6 +6,7 @@ import edu.wpi.first.units.Angle
 import edu.wpi.first.units.Measure
 import edu.wpi.first.wpilibj2.command.SubsystemBase
 import lib.vision.Limelight
+import lib.vision.VisionMeasurement
 
 /**
  * The subsystem that controls the limelight.
@@ -14,6 +15,8 @@ object LimelightSubsystem : SubsystemBase() {
     // TODO: get the actual position of the limelight
     private val limelight: Limelight = Limelight("limelight-odom", Pose3d())
 
+    val poseMeasurement: VisionMeasurement
+        get() = limelight.poseMeasurement
 
     /**
      * Get the position of the bot as estimated by the limelight

src/main/java/lib/vision/Limelight.kt

@@ -55,18 +55,18 @@ class Limelight(hostname: String, mountPosition: Pose3d) : AutoCloseable {
         ts = table.getDoubleTopic("ts").subscribe(0.0)
         tv = table.getDoubleTopic("tv").subscribe(0.0)
 
-        botpose = table.getDoubleArrayTopic("botpose").subscribe(DoubleArray(6))
+        botpose = table.getDoubleArrayTopic("botpose_wpiblue").subscribe(DoubleArray(6))
 
         visionTab = Shuffleboard.getTab("Vision")
-
-        visionTab.addDouble("X Offset") { yawRaw }
-        visionTab.addDouble("Y Offset") { pitchRaw }
-        visionTab.add("Yaw Offset") { yawOffset }
-        visionTab.add("Pitch Offset") { pitchOffset }
-        visionTab.add("Roll") { roll }
-        visionTab.add("Position") { position }
-        visionTab.addDouble("Area") { area }
-        visionTab.addBoolean("Target Visible") { targetVisible }
+//
+//        visionTab.addDouble("X Offset") { yawRaw }
+//        visionTab.addDouble("Y Offset") { pitchRaw }
+//        visionTab.add("Yaw Offset") { yawOffset }
+//        visionTab.add("Pitch Offset") { pitchOffset }
+//        visionTab.add("Roll") { roll }
+//        visionTab.add("Position") { position }
+//        visionTab.addDouble("Area") { area }
+//        visionTab.addBoolean("Target Visible") { targetVisible }
 
         _mountPosition = mountPosition
         // FIXME - remove if possible
@@ -190,5 +190,6 @@ class Limelight(hostname: String, mountPosition: Pose3d) : AutoCloseable {
         ta.close()
         ts.close()
         tv.close()
+        botpose.close()
     }
 }