From 089c4e80975c4ca30dbe37813878316490b063e7 Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Tue, 12 Dec 2023 23:06:26 -0500 Subject: [PATCH] more testing --- .../kotlin/com/team4099/robot2023/Robot.kt | 7 +++++++ .../com/team4099/robot2023/RobotContainer.kt | 18 +++++++++++++++++- .../subsystems/limelight/LimelightVision.kt | 13 +++++++++---- 3 files changed, 33 insertions(+), 5 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt index 39adaf01..2efb1d1d 100644 --- a/src/main/kotlin/com/team4099/robot2023/Robot.kt +++ b/src/main/kotlin/com/team4099/robot2023/Robot.kt @@ -30,7 +30,12 @@ import org.littletonrobotics.junction.Logger import org.littletonrobotics.junction.inputs.LoggedPowerDistribution import org.littletonrobotics.junction.wpilog.WPILOGReader import org.littletonrobotics.junction.wpilog.WPILOGWriter +import org.team4099.lib.geometry.Pose2d 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.centi +import org.team4099.lib.units.derived.degrees import java.nio.file.Files import java.nio.file.Paths @@ -209,6 +214,8 @@ object Robot : LoggedRobot() { RobotContainer.mapTunableCommands() } RobotContainer.zeroArm() + + RobotContainer.resetPose() } override fun testInit() { diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 842d7df1..28bd6e52 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -20,6 +20,7 @@ import com.team4099.robot2023.subsystems.gameboy.GameBoy import com.team4099.robot2023.subsystems.gameboy.GameboyIOServer import com.team4099.robot2023.subsystems.gameboy.objective.isConeNode import com.team4099.robot2023.subsystems.groundintake.GroundIntake +import com.team4099.robot2023.subsystems.groundintake.GroundIntakeIO import com.team4099.robot2023.subsystems.groundintake.GroundIntakeIONeo import com.team4099.robot2023.subsystems.groundintake.GroundIntakeIOSim import com.team4099.robot2023.subsystems.led.Led @@ -36,9 +37,16 @@ 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.Pose3d +import org.team4099.lib.geometry.Rotation3d import org.team4099.lib.smoothDeadband +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.centi import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees import java.util.function.Supplier @@ -70,7 +78,7 @@ object RobotContainer { superstructure = Superstructure( Elevator(ElevatorIONeo), - GroundIntake(GroundIntakeIONeo), + GroundIntake(object : GroundIntakeIO {}), Manipulator(ManipulatorIONeo), Led(object : LedIO {}), GameBoy(GameboyIOServer) @@ -106,6 +114,14 @@ object RobotContainer { } + fun resetPose() { + drivetrain.odometryPose = Pose2d( + (68.45).inches, + (108.19).inches + 114.centi.meters, + 0.0.degrees + ) + } + fun mapDefaultCommands() { drivetrain.defaultCommand = 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 756adba1..0ffeff74 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt @@ -1,6 +1,8 @@ package com.team4099.robot2023.subsystems.limelight import com.team4099.lib.hal.Clock +import com.team4099.lib.logging.LoggedTunableNumber +import com.team4099.lib.logging.LoggedTunableValue import com.team4099.lib.logging.TunableNumber import com.team4099.lib.vision.TargetCorner import com.team4099.robot2023.config.constants.Constants @@ -55,8 +57,8 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { val vpw = (2.0 * (VisionConstants.Limelight.HORIZONTAL_FOV / 2).tan) val vph = (2.0 * (VisionConstants.Limelight.VERITCAL_FOV / 2).tan) - private val xyStdDevCoefficient = TunableNumber("LimelightVision/xystdev", 0.05) - private val thetaStdDev = TunableNumber("LimelightVision/thetaStdDev", 0.75) + private val xyStdDevCoefficient = LoggedTunableNumber("LimelightVision/xystdev", 0.0) + private val thetaStdDev = LoggedTunableNumber("LimelightVision/thetaStdDev", 0.0) val limelightState: LimelightStates = LimelightStates.AUTO_POSE_ESTIMATION @@ -155,7 +157,8 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { if (inputs.gamePieceTargets[index].className == "cone") conePoses else cubePoses val closestPose = gamePiecePose.findClosestPose(*searchList.toTypedArray()) - trueGamePieces.add(closestPose) + + trueGamePieces.add(closestPose.findClosestPose(*searchList.toTypedArray())) // find inverse translation from the detected pose to robot val targetToCamera = @@ -212,6 +215,8 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { ?: 0.0 ) + Logger.getInstance().recordOutput("LimelightVision/trueGamePiecePoses", *trueGamePieces.map { it.pose3d }.toTypedArray()) + visionConsumer.accept(timestampedVisionUpdates) } else if (limelightState == LimelightStates.TELEOP_GAME_PIECE_DETECTION) { if (inputs.validReading) { @@ -329,7 +334,7 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { .transformBy(VisionConstants.Limelight.LL_TRANSFORM) .transformBy( Transform3d( - Translation3d(targetTranslation), Rotation3d(0.degrees, 0.degrees, 180.0.degrees) + Translation3d(targetTranslation), Rotation3d(0.degrees, 0.degrees, 0.0.degrees) ) ) }