more ll fusion work

Author: AshOnDiscord

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Drive.kt

@@ -52,16 +52,16 @@ open class Drive(val opMode: OpMode, limelight: Limelight? = null) : Subsystem("
         println("end pose ${follower.currentPath?.endPose()}")
     }
 
-    fun drawRobot() {
+    fun drawRobot(pose: Pose2d = this.pose, name: String = "pose", color: String = "white") {
         canvas.moveCursor(pose.x, pose.y)
-        canvas.setStyle(fill = "none", outline = "white", width = 1.5)
+        canvas.setStyle(fill = "none", outline = color, width = 1.5)
         canvas.circle(8.0)
         val lookPos = pose + Vec2d(1.0, 0.0).rotate(pose.radians) * 8.0
         canvas.line(lookPos.x, lookPos.y)
 
-        opMode.tel.addData("pose.x", pose.x)
-        opMode.tel.addData("pose.y", pose.y)
-        opMode.tel.addData("pose.h", pose.heading)
+        opMode.tel.addData("$name.x", pose.x)
+        opMode.tel.addData("$name.y", pose.y)
+        opMode.tel.addData("$name.h", pose.heading)
     }
 
     companion object {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Limelight.kt

@@ -3,9 +3,11 @@ package org.firstinspires.ftc.teamcode.common.subsystem
 import com.bylazar.configurables.annotations.Configurable
 import com.millburnx.cmdx.Command
 import com.millburnx.util.Pose2d
+import com.millburnx.util.vector.Vec2d
 import com.qualcomm.hardware.limelightvision.Limelight3A
 import org.firstinspires.ftc.teamcode.common.GlobalStore
 import org.firstinspires.ftc.teamcode.common.hardware.fromFTC
+import org.firstinspires.ftc.teamcode.common.hardware.normalizeDegrees
 import org.firstinspires.ftc.teamcode.common.util.OpModeLoop
 import org.firstinspires.ftc.teamcode.opmode.OpMode
 
@@ -20,6 +22,12 @@ class Limelight(
 
     var getPose: () -> Pose2d = { Pose2d() }
     var setPose: (Pose2d) -> Unit = {}
+    var drawPose: (Pose2d) -> Unit = {}
+
+    /**
+     * Heading of turret in global space
+     */
+    var turretHeading: () -> Double = { 0.0 }
 
     var localizationState: LocalizationState =
         if (GlobalStore.autonPose == null) LocalizationState.NONE else LocalizationState.READY
@@ -35,22 +43,45 @@ class Limelight(
             if (localizationState == LocalizationState.NONE) {
                 val result = limelight.getLatestResult()
                 if (result != null && result.isValid) {
-                    val pose = Pose2d.fromFTC(result.botpose)
+                    val pose = convertLL(Pose2d.fromFTC(result.botpose), useDriveHeading = false)
                     this@Limelight.pose = pose to System.nanoTime()
                     setPose(pose)
+                    drawPose(pose)
                     localizationState = LocalizationState.READY
                 }
             }
             if (localizationState == LocalizationState.READY) {
-                limelight.updateRobotOrientation(getPose().heading)
+                limelight.updateRobotOrientation(normalizeDegrees(getPose().heading + turretHeading()))
                 val result = limelight.getLatestResult()
                 if (result != null && result.isValid) {
-                    pose = Pose2d.fromFTC(result.botpose_MT2) to System.nanoTime()
+                    val pose = convertLL(Pose2d.fromFTC(result.botpose_MT2))
+                    this@Limelight.pose = pose to System.nanoTime()
+                    drawPose(pose)
                 }
             }
         }
     }
 
+    fun convertLL(pose: Pose2d, useDriveHeading: Boolean = true): Pose2d {
+        // the primary thing we need to handle here
+        // is conversion from the turret frame to the robot frame
+        // so account for the offset of the centers and the heading of the turret
+
+        val newHeading = normalizeDegrees(pose.heading - turretHeading())
+
+        val offset = Vec2d(16.0, -42.0) / 25.4
+
+        val driveHeading = if (useDriveHeading) {
+            getPose().heading
+        } else {
+            newHeading
+        }
+
+        val newPos = pose.position - offset.rotate(driveHeading)
+
+        return Pose2d(newPos, newHeading)
+    }
+
     enum class LocalizationState {
         NONE,
         READY;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/test/localization/FusionTest.kt

@@ -7,6 +7,7 @@ import com.millburnx.util.vector.Vec2d
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp
 import org.firstinspires.ftc.teamcode.common.subsystem.Limelight
 import org.firstinspires.ftc.teamcode.common.subsystem.TeleOpDrive
+import org.firstinspires.ftc.teamcode.common.subsystem.Turret
 import org.firstinspires.ftc.teamcode.common.util.OpModeLoop
 import org.firstinspires.ftc.teamcode.opmode.OpMode
 
@@ -16,6 +17,11 @@ class FusionTest : OpMode() {
     override fun run() {
         val limelight = Limelight(this)
         val drive = TeleOpDrive(this, true, limelight)
+        val turret = Turret(this, { drive.pose.heading }, { drive.velocity.heading }, { voltageSensor.voltage })
+        limelight.turretHeading = { turret.angle }
+        limelight.drawPose = { pose ->
+            drive.drawRobot(pose, "llPose", "yellow")
+        }
 
         scheduler.schedule(Command() {
             val canvas = PanelsField.field

…/ftc/teamcode/pedro/PinpointLocalizer.kt …es/ftc/teamcode/pedro/FusionLocalizer.ktTeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/PinpointLocalizer.kt renamed to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/FusionLocalizer.kt TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/PinpointLocalizer.kt renamed to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/FusionLocalizer.kt

@@ -37,6 +37,8 @@ class FusionLocalizer(hardwareMap: HardwareMap, val deltaTime: () -> Double, val
     }
 
     init {
+        limelight?.getPose = { _pose }
+        limelight?.setPose = { setPose(it) }
         while (pinpoint.deviceStatus != GoBildaPinpointDriver.DeviceStatus.READY) {
             Thread.yield()
         }
@@ -57,12 +59,14 @@ class FusionLocalizer(hardwareMap: HardwareMap, val deltaTime: () -> Double, val
 
     override fun setStartPose(p0: Pose) = setPose(p0) // low-key cannot be bothered to compensate, just don't update?
 
-    override fun setPose(p0: Pose) {
-        pinpoint.position = Pose2d.fromPedro(p0).toFTC()
+    fun setPose(p0: Pose2d) {
+        pinpoint.position = p0.toFTC()
         kfX.reset()
         kfY.reset()
     }
 
+    override fun setPose(p0: Pose) = setPose(Pose2d.fromPedro(p0))
+
     override fun update() {
         pinpoint.update()
         val pose = Pose2d.fromFTC(pinpoint.position)