spotless apply

team4099 · Feb 18, 2024 · 2b5ce62 · 2b5ce62
1 parent b7f086c
commit 2b5ce62
Show file tree

Hide file tree

Showing 6 changed files with 46 additions and 39 deletions.
diff --git a/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt b/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt
@@ -51,9 +51,11 @@ fun Transform2d.asPose2d(): Pose2d {
 }
 
 fun Transform2d.asDoubleArray(): DoubleArray {
-  return doubleArrayOf(this.translation.x.inMeters, this.translation.y.inMeters, this.rotation.inRadians)
+  return doubleArrayOf(
+    this.translation.x.inMeters, this.translation.y.inMeters, this.rotation.inRadians
+  )
 }
 
 fun Pose2d.asDoubleArray(): DoubleArray {
   return doubleArrayOf(this.x.inMeters, this.y.inMeters, this.rotation.inRadians)
-}
+}
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt
@@ -56,10 +56,10 @@ object VisionConstants {
       ) // camera facing leftward
     )
 
-  val SIM_CAMERA_TRANSFORM = Transform3d(
-    Translation3d(0.inches, 0.inches, 0.inches),
-    Rotation3d(0.degrees, 0.degrees, 0.degrees)
-  )
+  val SIM_CAMERA_TRANSFORM =
+    Transform3d(
+      Translation3d(0.inches, 0.inches, 0.inches), Rotation3d(0.degrees, 0.degrees, 0.degrees)
+    )
 
   val CAMERA_NAMES = listOf("northstar_1", "northstar_2", "northstar_3")
 

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt
@@ -299,7 +299,14 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
         .pose3d
     )
 
-    Logger.recordOutput("FieldFrameEstimator/odomTField", doubleArrayOf(odomTField.translation.x.inMeters, odomTField.translation.y.inMeters, odomTField.rotation.inRadians))
+    Logger.recordOutput(
+      "FieldFrameEstimator/odomTField",
+      doubleArrayOf(
+        odomTField.translation.x.inMeters,
+        odomTField.translation.y.inMeters,
+        odomTField.rotation.inRadians
+      )
+    )
 
     Logger.recordOutput(
       "Odometry/targetPose",
@@ -391,7 +398,12 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
       undriftedSwerveDriveOdometry.update((gyroInputs.gyroYaw).inRotation2ds, undriftedModules)
 
       drift = undriftedPose.minus(odomTRobot)
-      Logger.recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, doubleArrayOf(undriftedPose.x.inMeters, undriftedPose.y.inMeters, undriftedPose.rotation.inRadians))
+      Logger.recordOutput(
+        VisionConstants.SIM_POSE_TOPIC_NAME,
+        doubleArrayOf(
+          undriftedPose.x.inMeters, undriftedPose.y.inMeters, undriftedPose.rotation.inRadians
+        )
+      )
     }
   }
 

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt
@@ -5,14 +5,12 @@ import com.team4099.lib.logging.TunableNumber
 import com.team4099.lib.math.asDoubleArray
 import com.team4099.lib.vision.TimestampedVisionUpdate
 import com.team4099.robot2023.config.constants.FieldConstants
-import com.team4099.robot2023.config.constants.VisionConstants
 import com.team4099.robot2023.subsystems.vision.camera.CameraIO
 import edu.wpi.first.math.VecBuilder
 import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj2.command.SubsystemBase
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.geometry.Pose2d
-import org.team4099.lib.geometry.Pose2dWPILIB
 import org.team4099.lib.geometry.Pose3d
 import org.team4099.lib.geometry.Pose3dWPILIB
 import org.team4099.lib.geometry.Transform3d
@@ -22,7 +20,6 @@ import org.team4099.lib.units.base.inMilliseconds
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.degrees
-import org.team4099.lib.units.derived.inRadians
 import java.util.function.Consumer
 import java.util.function.Supplier
 import kotlin.math.pow
@@ -55,7 +52,6 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() {
       lastFrameTimes[i] = 0.0.seconds
       names.add(io[i].id)
       robotTCameras.add(io[i].robotTCamera)
-
     }
   }
 
@@ -99,7 +95,6 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() {
       var cameraPose: Pose3d? = inputs[instance].frame
       var robotPose: Pose2d? = cameraPose?.transformBy(robotTCameras[instance])?.toPose2d()
 
-
       if (cameraPose == null || robotPose == null) {
         continue
       }
@@ -132,32 +127,26 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() {
       robotPoses.add(robotPose)
 
       Logger.recordOutput(
-        "Vision/${names[instance]}/latencyMS",
-        (Clock.fpgaTime - timestamp).inMilliseconds
+        "Vision/${names[instance]}/latencyMS", (Clock.fpgaTime - timestamp).inMilliseconds
       )
 
-      Logger.recordOutput(
-        "Vision/${names[instance]}/estimatedRobotPose", robotPose.asDoubleArray()
-      )
+      Logger.recordOutput("Vision/${names[instance]}/estimatedRobotPose", robotPose.asDoubleArray())
 
       Logger.recordOutput(
-        "Vision/${names[instance]}/tagPoses",
-        *tagPoses.map { it.pose3d }.toTypedArray()
+        "Vision/${names[instance]}/tagPoses", *tagPoses.map { it.pose3d }.toTypedArray()
       )
 
-//      if (inputs[instance].timestamp == 0.0.seconds) { // prolly wrong lol
-//        Logger.recordOutput(
-//          "Vision/${names[instance]}/estimatedRobotPose",
-//          Pose2dWPILIB.struct,
-//          Pose2d().pose2d
-//        )
-//      }
+      //      if (inputs[instance].timestamp == 0.0.seconds) { // prolly wrong lol
+      //        Logger.recordOutput(
+      //          "Vision/${names[instance]}/estimatedRobotPose",
+      //          Pose2dWPILIB.struct,
+      //          Pose2d().pose2d
+      //        )
+      //      }
 
       if (Clock.fpgaTime - lastFrameTimes[instance]!! > targetLogTime) {
         Logger.recordOutput(
-          "Vision/${names[instance]}/tagPoses",
-          Pose3dWPILIB.struct,
-          *arrayOf<Pose3dWPILIB>()
+          "Vision/${names[instance]}/tagPoses", Pose3dWPILIB.struct, *arrayOf<Pose3dWPILIB>()
         )
       }
     }

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt
@@ -14,7 +14,8 @@ import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.micro
 import java.util.Optional
 
-class CameraIOPhotonvision(override val id: String, override val robotTCamera: Transform3d) : CameraIO {
+class CameraIOPhotonvision(override val id: String, override val robotTCamera: Transform3d) :
+  CameraIO {
 
   private val photonEstimator: PhotonPoseEstimator
   private val camera: PhotonCamera

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt
@@ -36,22 +36,25 @@ class CameraIOSim(override val id: String, override val robotTCamera: Transform3
     odoHistory.add(Pair(Clock.fpgaTime, drivePose.toPose2d()))
 
     // clear out old poses history
-    if (odoHistory.size > 1 && odoHistory[0].first < Clock.fpgaTime - 0.3.seconds){
+    if (odoHistory.size > 1 && odoHistory[0].first < Clock.fpgaTime - 0.3.seconds) {
       odoHistory.removeAt(0)
     }
 
     inputs.timestamp = Clock.fpgaTime - (Math.random() * 0.3).seconds
 
     var interpolatedDrivePose = Pose2d()
-    for (poseInd in 1 until odoHistory.size){
-        // pose within these two timestamps
-        if (odoHistory[poseInd].first > inputs.timestamp && odoHistory[poseInd-1].first < inputs.timestamp){
-          val interpolatingFrac = odoHistory[poseInd - 1].first.lerp(odoHistory[poseInd].first, inputs.timestamp)
-          interpolatedDrivePose = odoHistory[poseInd - 1].second.lerp(odoHistory[poseInd].second, interpolatingFrac)
-        }
+    for (poseInd in 1 until odoHistory.size) {
+      // pose within these two timestamps
+      if (odoHistory[poseInd].first > inputs.timestamp &&
+        odoHistory[poseInd - 1].first < inputs.timestamp
+      ) {
+        val interpolatingFrac =
+          odoHistory[poseInd - 1].first.lerp(odoHistory[poseInd].first, inputs.timestamp)
+        interpolatedDrivePose =
+          odoHistory[poseInd - 1].second.lerp(odoHistory[poseInd].second, interpolatingFrac)
+      }
     }
 
-
     val robotTtag = interpolatedDrivePose.toPose3d().relativeTo(FieldConstants.aprilTags[0].pose)
     inputs.frame = interpolatedDrivePose.toPose3d().gaussianShenanigans()
     val estimatedRobotPose =