more testing

team4099 · Dec 13, 2023 · 089c4e8 · 089c4e8
1 parent 5dbe480
commit 089c4e8
Show file tree

Hide file tree

Showing 3 changed files with 33 additions and 5 deletions.
diff --git 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
@@ -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
@@ -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)
         )
       )
   }