diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 0e75eaf9..0d8dcafc 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -25,7 +25,8 @@ import com.team4099.robot2023.subsystems.led.Led import com.team4099.robot2023.subsystems.led.LedIO import com.team4099.robot2023.subsystems.led.LedIOSim import com.team4099.robot2023.subsystems.limelight.LimelightVision -import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO +import com.team4099.robot2023.subsystems.limelight.LimelightVisionIOReal +import com.team4099.robot2023.subsystems.limelight.LimelightVisionIOSim import com.team4099.robot2023.subsystems.manipulator.Manipulator import com.team4099.robot2023.subsystems.manipulator.ManipulatorIONeo import com.team4099.robot2023.subsystems.manipulator.ManipulatorIOSim @@ -73,7 +74,7 @@ object RobotContainer { Led(object : LedIO {}), GameBoy(GameboyIOServer) ) - limelight = LimelightVision(object : LimelightVisionIO {}) + limelight = LimelightVision(LimelightVisionIOReal) } else { // Simulation implementations drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim) @@ -91,11 +92,12 @@ object RobotContainer { Led(LedIOSim), GameBoy(GameboyIOServer) ) - limelight = LimelightVision(object : LimelightVisionIO {}) + limelight = LimelightVision(LimelightVisionIOSim) } vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) }) - limelight.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) }) + limelight.setDataInterfaces({ drivetrain.odometryPose }, { it }) + LimelightVisionIOSim.poseSupplier = { drivetrain.odometryPose } drivetrain.elevatorHeightSupplier = Supplier { superstructure.elevatorInputs.elevatorPosition } drivetrain.objectiveSupplier = Supplier { superstructure.objective } limelight.gamePieceToLookFor = { superstructure.objective } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt index 441fee36..fd7e0470 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt @@ -69,8 +69,8 @@ object VisionConstants { val LL_TRANSFORM = Transform3d( - Translation3d(1.1438.inches, 10.3966.inches, 12.9284.inches), - Rotation3d(8.159.degrees, -90.degrees + 61.610.degrees, -14.1254.degrees) + Translation3d(11.6569.inches, -2.999.inches, 37.0768.inches), + Rotation3d(0.degrees, 90.0.degrees - 49.678.degrees, 0.degrees) ) const val RES_WIDTH = 320 const val RES_HEIGHT = 240 // no clue what these numbers should be but usnig these for now diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt index 15ef5093..927f8b88 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt @@ -31,6 +31,7 @@ import org.team4099.lib.units.base.inMilliseconds import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees import org.team4099.lib.units.derived.inRadians import org.team4099.lib.units.derived.radians import org.team4099.lib.units.derived.tan @@ -111,7 +112,7 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { val gamePieceLocationIndex = if (FMSData.isBlue) gamePieceToLookFor.invoke().autoStagingLocation - else 3 - gamePieceToLookFor.invoke().autoStagingLocation + else 3 - gamePieceToLookFor().autoStagingLocation val gamePieceY = FieldConstants.StagingLocations.firstY + @@ -153,7 +154,7 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { for ((index, gamePiecePose) in visibleGamePieces.withIndex()) { val searchList = if (inputs.gamePieceTargets[index].className == "cone") conePoses else cubePoses - val closestPose = gamePiecePose.findClosestPose(*searchList.toTypedArray()) + val closestPose = searchList[0] trueGamePieces.add(closestPose) @@ -188,6 +189,17 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { "LimelightVision/estimatedRobotPoses", *robotPoses.map { it.pose2d }.toTypedArray() ) + Logger.getInstance() + .recordOutput( + "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 + ) + + visionConsumer.accept(timestampedVisionUpdates) } else if (limelightState == LimelightStates.TELEOP_GAME_PIECE_DETECTION) { if (inputs.validReading) { @@ -213,6 +225,18 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { } } + Logger.getInstance() + .recordOutput( + "LimelightVision/RawLimelightReadingsTx", + inputs.gamePieceTargets.map { it.tx.inDegrees }.toDoubleArray() + ) + + Logger.getInstance() + .recordOutput( + "LimelightVision/RawLimelightReadingsTy", + inputs.gamePieceTargets.map { it.ty.inDegrees }.toDoubleArray() + ) + Logger.getInstance() .recordOutput( "LimelightVision/robotVisiblePieces", @@ -230,6 +254,24 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { "LoggedRobot/Subsystems/LimelightLoopTimeMS", (Clock.realTimestamp - startTime).inMilliseconds ) + Logger.getInstance().recordOutput("LimelightVision/LimeLightState", limelightState.name) + Logger.getInstance() + .recordOutput( + "LimelightVision/test", + Pose3d( + FieldConstants.StagingLocations.positionX, + FieldConstants.StagingLocations.translations[0]?.y ?: 0.meters, + VisionConstants.Limelight.CONE_HEIGHT / 2, + Rotation3d() + ) + .relativeTo( + LimelightVisionIOSim.poseSupplier + .invoke() + .toPose3d() + .transformBy(VisionConstants.Limelight.LL_TRANSFORM) + ) + .pose3d + ) } fun solveTargetPoseFromAngle( @@ -262,18 +304,18 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { 0.degrees..180.degrees ) { // we are looking at a red node which is facing towards 180 degrees - 180.0.degrees + 0.degrees } else { // we are looking at a blue node which is facing towards 0 degrees - 0.degrees + 180.degrees } ) return currentPose .toPose3d() .transformBy(VisionConstants.Limelight.LL_TRANSFORM) - .transformBy(Transform3d(Translation3d(targetTranslation), targetRotation)) + .transformBy(Transform3d(Translation3d(targetTranslation), Rotation3d(0.degrees, 0.degrees, 180.0.degrees))) } fun xyDistanceFromTarget(target: LimelightReading, targetHeight: Length): Length { @@ -320,6 +362,7 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { val xDistanceFromTargetToCamera = (targetHeight - cameraTransform.z) / verticalAngleFromCamera.tan + val yDistanceFromTargetToCamera = xDistanceFromTargetToCamera * horizontalAngleFromCamera.tan val translationFromTargetToCamera = diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOSim.kt index a7dc7117..464e3344 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOSim.kt @@ -1,25 +1,47 @@ package com.team4099.robot2023.subsystems.limelight import com.team4099.lib.hal.Clock -import com.team4099.lib.logging.LoggedTunableValue +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.toPose3d +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Pose3d +import org.team4099.lib.geometry.Rotation3d +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.inDegrees import org.team4099.lib.units.derived.radians +import java.lang.Math.atan object LimelightVisionIOSim : LimelightVisionIO { - private val tunableTx = - LoggedTunableValue( - "LimelightSim/txDegrees", (8.612154).degrees, Pair({ it.inDegrees }, { it.degrees }) - ) - private val tunableTy = - LoggedTunableValue( - "LimelightSim/tyDegrees", - (-1.6576093255536648).degrees, - Pair({ it.inDegrees }, { it.degrees }) - ) + var poseSupplier: () -> Pose2d = { Pose2d() } + + val poseToGamePiece: Pose3d + get() { + return Pose3d( + 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) + ) + .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 + } override fun updateInputs(inputs: LimelightVisionIO.LimelightVisionIOInputs) { inputs.timestamp = Clock.realTimestamp @@ -29,10 +51,6 @@ object LimelightVisionIOSim : LimelightVisionIO { inputs.fps = 90.0 inputs.validReading = true inputs.gamePieceTargets = - listOf( - LimelightReading( - "", 0.0.percent, tunableTx.get(), tunableTy.get(), 0.0, 0.0, 0.0.percent - ) - ) + listOf(LimelightReading("cone", 0.0.percent, tunableTx, tunableTy, 0.0, 0.0, 0.0.percent)) } }