format

blair-robot-project · Jan 25, 2025 · 46f3304 · 46f3304
1 parent 2db4cac
commit 46f3304
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 13 deletions.
diff --git a/src/main/kotlin/frc/team449/subsystems/vision/ApriltagCamera.kt b/src/main/kotlin/frc/team449/subsystems/vision/ApriltagCamera.kt
@@ -9,12 +9,10 @@ import edu.wpi.first.math.numbers.N1
 import edu.wpi.first.math.numbers.N3
 import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj.smartdashboard.Field2d
-import frc.team449.subsystems.vision.PoseSubsystem
 import frc.team449.subsystems.vision.ReefOnlyEstimator
 import frc.team449.subsystems.vision.VisionConstants
 import org.photonvision.EstimatedRobotPose
 import org.photonvision.PhotonCamera
-import org.photonvision.PhotonPoseEstimator
 import org.photonvision.simulation.PhotonCameraSim
 import org.photonvision.simulation.SimCameraProperties
 import org.photonvision.simulation.VisionSystemSim

diff --git a/src/main/kotlin/frc/team449/subsystems/vision/ReefOnlyEstimator.kt b/src/main/kotlin/frc/team449/subsystems/vision/ReefOnlyEstimator.kt
@@ -1,11 +1,8 @@
 package frc.team449.subsystems.vision
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout
-import edu.wpi.first.math.MathUtil
 import edu.wpi.first.math.geometry.*
-import edu.wpi.first.math.util.Units
 import edu.wpi.first.wpilibj.DriverStation
-import edu.wpi.first.wpilibj.RobotBase
 import org.photonvision.EstimatedRobotPose
 import org.photonvision.PhotonCamera
 import org.photonvision.PhotonPoseEstimator
@@ -14,8 +11,6 @@ import org.photonvision.targeting.PhotonTrackedTarget
 import java.util.Optional
 import kotlin.jvm.optionals.getOrNull
 import kotlin.math.abs
-import kotlin.math.cos
-import kotlin.math.tan
 
 /**
  * This class uses normal multi-tag PNP and lowest ambiguity using the gyro rotation
@@ -117,8 +112,11 @@ class ReefOnlyEstimator(
       val targetPoseAmbiguity = target.poseAmbiguity
       // Make sure the target is a Fiducial target.
       if (targetPoseAmbiguity != -1.0 && targetPoseAmbiguity < lowestAmbiguityScore &&
-        ((listOf(6,7,8,9,10,11).contains(target.fiducialId) && DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Red) ||
-        (listOf(17,18,19,20,21,22).contains(target.fiducialId) && DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Blue))) {
+        (
+          (listOf(6, 7, 8, 9, 10, 11).contains(target.fiducialId) && DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Red) ||
+            (listOf(17, 18, 19, 20, 21, 22).contains(target.fiducialId) && DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Blue)
+          )
+      ) {
         lowestAmbiguityScore = targetPoseAmbiguity
         lowestAmbiguityTarget = target
         print("getting here")

diff --git a/src/main/kotlin/frc/team449/subsystems/vision/VisionConstants.kt b/src/main/kotlin/frc/team449/subsystems/vision/VisionConstants.kt
@@ -2,7 +2,6 @@ package frc.team449.subsystems.vision
 
 import edu.wpi.first.apriltag.AprilTag
 import edu.wpi.first.apriltag.AprilTagFieldLayout
-import edu.wpi.first.apriltag.AprilTagFields
 import edu.wpi.first.math.MatBuilder
 import edu.wpi.first.math.Matrix
 import edu.wpi.first.math.Nat
@@ -64,9 +63,9 @@ object VisionConstants {
 
   /** Vision Sim Setup Constants */
   const val SIM_FPS = 25.0
-  const val SIM_CAMERA_HEIGHT_PX = 800// 1200 // 800
-  const val SIM_CAMERA_WIDTH_PX = 1280//1600 // 1280
-  const val SIM_FOV_DEG = 79.09//87.6115 // 79.09
+  const val SIM_CAMERA_HEIGHT_PX = 800 // 1200 // 800
+  const val SIM_CAMERA_WIDTH_PX = 1280 // 1600 // 1280
+  const val SIM_FOV_DEG = 79.09 // 87.6115 // 79.09
   const val SIM_CALIB_AVG_ERR_PX = 0.45
   const val SIM_CALIB_ERR_STDDEV_PX = 0.65
   const val SIM_AVG_LATENCY = 60.0