implement limelight simulation

team4099 · Dec 12, 2023 · 8080bfa · 8080bfa
1 parent 3fa807c
commit 8080bfa
Show file tree

Hide file tree

Showing 3 changed files with 55 additions and 28 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -35,14 +35,9 @@ import com.team4099.robot2023.subsystems.superstructure.Superstructure
 import com.team4099.robot2023.subsystems.vision.Vision
 import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar
 import com.team4099.robot2023.util.driver.Ryan
-import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.wpilibj.RobotBase
 import org.littletonrobotics.junction.Logger
-import org.team4099.lib.geometry.Pose2d
-import org.team4099.lib.geometry.Transform2d
-import org.team4099.lib.geometry.Translation2d
 import org.team4099.lib.smoothDeadband
-import org.team4099.lib.units.base.inches
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
 import java.util.function.Supplier

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt
@@ -7,7 +7,6 @@ import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.config.constants.FieldConstants
 import com.team4099.robot2023.config.constants.VisionConstants
 import com.team4099.robot2023.subsystems.gameboy.objective.Objective
-import com.team4099.robot2023.util.AllianceFlipUtil
 import com.team4099.robot2023.util.FMSData
 import com.team4099.robot2023.util.LimelightReading
 import com.team4099.robot2023.util.PoseEstimator
@@ -29,7 +28,6 @@ import org.team4099.lib.geometry.Translation3dWPILIB
 import org.team4099.lib.units.base.Length
 import org.team4099.lib.units.base.inMeters
 import org.team4099.lib.units.base.inMilliseconds
-import org.team4099.lib.units.base.inches
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
@@ -192,15 +190,28 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
 
       Logger.getInstance()
         .recordOutput(
-          "LimelightVision/distanceToGamePieceX", robotPoses.getOrNull(0)?.minus(conePoses.getOrNull(0)?.toPose2d() ?: Pose2d())?.translation?.translation2d?.x ?: 0.0
+          "LimelightVision/distanceToGamePieceX",
+          robotPoses
+            .getOrNull(0)
+            ?.minus(conePoses.getOrNull(0)?.toPose2d() ?: Pose2d())
+            ?.translation
+            ?.translation2d
+            ?.x
+            ?: 0.0
         )
 
       Logger.getInstance()
         .recordOutput(
-          "LimelightVision/distanceToGamePieceY", robotPoses.getOrNull(0)?.minus(conePoses.getOrNull(0)?.toPose2d() ?: Pose2d())?.translation?.translation2d?.y ?: 0.0
+          "LimelightVision/distanceToGamePieceY",
+          robotPoses
+            .getOrNull(0)
+            ?.minus(conePoses.getOrNull(0)?.toPose2d() ?: Pose2d())
+            ?.translation
+            ?.translation2d
+            ?.y
+            ?: 0.0
         )
 
-
       visionConsumer.accept(timestampedVisionUpdates)
     } else if (limelightState == LimelightStates.TELEOP_GAME_PIECE_DETECTION) {
       if (inputs.validReading) {
@@ -316,7 +327,11 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
     return currentPose
       .toPose3d()
       .transformBy(VisionConstants.Limelight.LL_TRANSFORM)
-      .transformBy(Transform3d(Translation3d(targetTranslation), Rotation3d(0.degrees, 0.degrees, 180.0.degrees)))
+      .transformBy(
+        Transform3d(
+          Translation3d(targetTranslation), Rotation3d(0.degrees, 0.degrees, 180.0.degrees)
+        )
+      )
   }
 
   fun xyDistanceFromTarget(target: LimelightReading, targetHeight: Length): Length {

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOSim.kt
@@ -4,17 +4,21 @@ import com.team4099.lib.hal.Clock
 import com.team4099.robot2023.config.constants.FieldConstants
 import com.team4099.robot2023.config.constants.VisionConstants
 import com.team4099.robot2023.util.LimelightReading
+import com.team4099.robot2023.util.rotateBy
 import com.team4099.robot2023.util.toPose3d
+import org.littletonrobotics.junction.Logger
 import org.team4099.lib.geometry.Pose2d
 import org.team4099.lib.geometry.Pose3d
 import org.team4099.lib.geometry.Rotation3d
+import org.team4099.lib.geometry.Translation3d
 import org.team4099.lib.units.base.inMeters
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.base.percent
-import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.derived.radians
-import java.lang.Math.atan
+import kotlin.math.acos
+import kotlin.math.pow
+import kotlin.math.sqrt
 
 object LimelightVisionIOSim : LimelightVisionIO {
 
@@ -26,31 +30,44 @@ object LimelightVisionIOSim : LimelightVisionIO {
         FieldConstants.StagingLocations.positionX,
         FieldConstants.StagingLocations.translations[0]?.y ?: 0.meters,
         VisionConstants.Limelight.CONE_HEIGHT / 2,
-        Rotation3d(0.0.degrees, 0.0.degrees, 0.0.degrees)
+        Rotation3d(0.0.degrees, 0.0.degrees, 180.0.degrees)
       )
-        .relativeTo(
-          poseSupplier.invoke().toPose3d().transformBy(VisionConstants.Limelight.LL_TRANSFORM)
-        )
-    }
-
-  private val tunableTx: Angle
-    get() {
-      return atan(poseToGamePiece.y.inMeters / poseToGamePiece.x.inMeters).radians
-    }
-
-  private val tunableTy: Angle
-    get() {
-      return atan(poseToGamePiece.x.inMeters / poseToGamePiece.z.inMeters).radians
+        .relativeTo(poseSupplier().toPose3d().transformBy(VisionConstants.Limelight.LL_TRANSFORM))
     }
 
   override fun updateInputs(inputs: LimelightVisionIO.LimelightVisionIOInputs) {
+    Logger.getInstance().recordOutput("LimelightVision/relativePose", poseToGamePiece.pose3d)
     inputs.timestamp = Clock.realTimestamp
     inputs.xAngle = 0.0.radians
     inputs.yAngle = 0.0.radians
     inputs.targetSize = 0.0.percent
     inputs.fps = 90.0
     inputs.validReading = true
+
+    val sphericalCoordinatesInCameraSpace = cartesianToSpherical(poseToGamePiece.translation)
     inputs.gamePieceTargets =
-      listOf(LimelightReading("cone", 0.0.percent, tunableTx, tunableTy, 0.0, 0.0, 0.0.percent))
+      listOf(
+        LimelightReading(
+          "cone",
+          0.0.percent,
+          sphericalCoordinatesInCameraSpace.x,
+          sphericalCoordinatesInCameraSpace.y,
+          0.0,
+          0.0,
+          0.0.percent
+        )
+      )
+  }
+
+  private fun cartesianToSpherical(translationInCameraSpace: Translation3d): Rotation3d {
+    val x = translationInCameraSpace.x
+    val y = translationInCameraSpace.y
+    val z = translationInCameraSpace.z
+
+    val r = sqrt(x.inMeters.pow(2) + y.inMeters.pow(2) + z.inMeters.pow(2)).meters
+    val theta = acos(z / r).radians.rotateBy(-VisionConstants.Limelight.LL_TRANSFORM.rotation.y)
+    val phi = (y.sign * acos(x.inMeters / sqrt(x.inMeters.pow(2) + y.inMeters.pow(2)))).radians
+
+    return Rotation3d(phi, theta, 0.0.degrees)
   }
 }