Vision sim (but rebased correctly)

simgui-ds.json

@@ -1,4 +1,9 @@
 {
+  "Keyboard 0 Settings": {
+    "window": {
+      "visible": true
+    }
+  },
   "System Joysticks": {
     "window": {
       "visible": false
@@ -20,9 +25,17 @@
           "decayRate": 0.0,
           "incKey": 82,
           "keyRate": 0.009999999776482582
+        },
+        {
+          "decKey": 75,
+          "incKey": 74
+        },
+        {
+          "decKey": 76,
+          "incKey": 73
         }
       ],
-      "axisCount": 3,
+      "axisCount": 5,
       "buttonCount": 4,
       "buttonKeys": [
         90,
@@ -45,16 +58,6 @@
       "povCount": 1
     },
     {
-      "axisConfig": [
-        {
-          "decKey": 74,
-          "incKey": 76
-        },
-        {
-          "decKey": 73,
-          "incKey": 75
-        }
-      ],
       "axisCount": 2,
       "buttonCount": 4,
       "buttonKeys": [

src/main/kotlin/com/team4099/lib/logging/DoubleArrayConversions.kt

@@ -24,9 +24,10 @@ fun Pose3d.toDoubleArray(): Array<Double> {
     this.x.inMeters,
     this.y.inMeters,
     this.z.inMeters,
-    this.rotation.x.inRadians,
-    this.rotation.y.inRadians,
-    this.rotation.z.inRadians
+    this.rotation.quaternion.w.inRadians,
+    this.rotation.quaternion.x.inMeters,
+    this.rotation.quaternion.y.inMeters,
+    this.rotation.quaternion.z.inMeters
   )
 }
 

src/main/kotlin/com/team4099/lib/math/GeomUtil.kt

@@ -4,8 +4,10 @@ import org.team4099.lib.geometry.Pose2d
 import org.team4099.lib.geometry.Transform2d
 import org.team4099.lib.geometry.Translation2d
 import org.team4099.lib.geometry.Twist2d
+import org.team4099.lib.units.base.inMeters
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inRadians
 import org.team4099.lib.units.derived.radians
 
 /**
@@ -47,3 +49,13 @@ fun Pose2d.asTransform2d(): Transform2d {
 fun Transform2d.asPose2d(): Pose2d {
   return Pose2d(this.translation, this.rotation)
 }
+
+fun Transform2d.asDoubleArray(): DoubleArray {
+  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)
+}

src/main/kotlin/com/team4099/robot2023/RobotContainer.kt

@@ -5,6 +5,7 @@ import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand
 import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand
 import com.team4099.robot2023.config.ControlBoard
 import com.team4099.robot2023.config.constants.Constants
+import com.team4099.robot2023.config.constants.VisionConstants
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOReal
 import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim
@@ -27,12 +28,16 @@ import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO
 import com.team4099.robot2023.subsystems.superstructure.Request
 import com.team4099.robot2023.subsystems.superstructure.Superstructure
 import com.team4099.robot2023.subsystems.vision.Vision
+import com.team4099.robot2023.subsystems.vision.camera.CameraIO
+import com.team4099.robot2023.subsystems.vision.camera.CameraIOSim
 import com.team4099.robot2023.subsystems.wrist.Wrist
 import com.team4099.robot2023.subsystems.wrist.WristIO
 import com.team4099.robot2023.subsystems.wrist.WristIOSim
-import com.team4099.robot2023.subsystems.vision.camera.CameraIOPhotonvision
 import com.team4099.robot2023.util.driver.Ryan
 import edu.wpi.first.wpilibj.RobotBase
+import org.team4099.lib.geometry.Rotation3d
+import org.team4099.lib.geometry.Transform3d
+import org.team4099.lib.geometry.Translation3d
 import org.team4099.lib.smoothDeadband
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
@@ -57,11 +62,16 @@ object RobotContainer {
       drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal)
       vision =
         Vision(
-          //          object: CameraIO {}
+          object : CameraIO {
+            override val id: String
+              get() = "skrt"
+            override val robotTCamera: Transform3d
+              get() = Transform3d(Translation3d(), Rotation3d())
+          }
           //          CameraIONorthstar("northstar"),
-          CameraIOPhotonvision("parakeet_1"),
-          CameraIOPhotonvision("parakeet_2"),
-          CameraIOPhotonvision("parakeet_3"),
+          //          CameraIOPhotonvision("parakeet_1", VisionConstants.CAMERA_TRANSFORMS[0]),
+          //          CameraIOPhotonvision("parakeet_2", VisionConstants.CAMERA_TRANSFORMS[1]),
+          //          CameraIOPhotonvision("parakeet_3", VisionConstants.CAMERA_TRANSFORMS[2]),
           //        CameraIONorthstar("right"),
           //        CameraIONorthstar("backward")
         )
@@ -74,12 +84,7 @@ object RobotContainer {
     } else {
       // Simulation implementations
       drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim)
-      vision =
-        Vision(
-          CameraIOPhotonvision("parakeet_1"),
-          CameraIOPhotonvision("parakeet_2"),
-          CameraIOPhotonvision("parakeet_3")
-        )
+      vision = Vision(CameraIOSim("skrt", VisionConstants.SIM_CAMERA_TRANSFORM))
       limelight = LimelightVision(object : LimelightVisionIO {})
       intake = Intake(IntakeIOSim)
       feeder = Feeder(FeederIOSim)
@@ -150,7 +155,7 @@ object RobotContainer {
 
   fun mapTeleopControls() {
 
-    ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 180.degrees))
+    ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 0.0.degrees))
     ControlBoard.runGroundIntake.whileTrue(superstructure.groundIntakeCommand())
     ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand())
     ControlBoard.prepAmpScore.whileTrue(superstructure.prepAmpCommand())

src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt

@@ -182,6 +182,8 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla
     // Sampling the trajectory for a state that we're trying to target
     val stateFromTrajectory = generatedTrajectory.sample(trajCurTime.inSeconds)
 
+    lastSampledPose = Pose2d(stateFromTrajectory.targetHolonomicPose)
+
     // Retrieves the last sampled pose, so we can keep our `atReference` variable updated
     Logger.recordOutput(
       "Odometry/targetPose",
@@ -196,8 +198,6 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla
     )
     Logger.recordOutput("Pathfollow/currentTheta", drivetrain.odomTRobot.rotation.inDegrees)
 
-    lastSampledPose = Pose2d(stateFromTrajectory.targetHolonomicPose)
-
     val targetedChassisSpeeds =
       swerveDriveController.calculateRobotRelativeSpeeds(
         drivetrain.odomTRobot, stateFromTrajectory

src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt

@@ -2,14 +2,28 @@ package com.team4099.robot2023.config.constants
 
 import org.team4099.lib.apriltag.AprilTag
 import org.team4099.lib.geometry.Pose3d
+import org.team4099.lib.geometry.Rotation3d
+import org.team4099.lib.geometry.Translation3d
 import org.team4099.lib.units.base.feet
 import org.team4099.lib.units.base.inMeters
+import org.team4099.lib.units.base.meters
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.radians
 
 object FieldConstants {
   val fieldLength = 54.feet
   val fieldWidth = 26.feet
 
-  val aprilTags: List<AprilTag> = listOf()
+  val aprilTags: List<AprilTag> =
+    listOf(
+      AprilTag(
+        0,
+        Pose3d(
+          Translation3d(1.0.meters, 5.0.meters, 2.0.meters),
+          Rotation3d(0.0.radians, 0.0.radians, 180.degrees)
+        )
+      )
+    )
   val homeAprilTags: List<AprilTag> = listOf()
 
   val wpilibAprilTags =

src/main/kotlin/com/team4099/robot2023/config/constants/GyroConstants.kt

@@ -5,5 +5,5 @@ import org.team4099.lib.units.derived.degrees
 object GyroConstants {
   val mountPitch = 0.0.degrees
   val mountYaw = 0.0.degrees
-  val mountRoll = 0.0.degrees
+  val mountRoll = 180.0.degrees
 }

src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt

@@ -56,6 +56,11 @@ 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 CAMERA_NAMES = listOf("northstar_1", "northstar_2", "northstar_3")
 
   object Limelight {

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", odomTField.transform2d)
+    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, undriftedPose.pose2d)
+      Logger.recordOutput(
+        VisionConstants.SIM_POSE_TOPIC_NAME,
+        doubleArrayOf(
+          undriftedPose.x.inMeters, undriftedPose.y.inMeters, undriftedPose.rotation.inRadians
+        )
+      )
     }
   }
 
@@ -556,12 +568,13 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
    * @param toAngle Zeros the gyro to the value
    */
   fun zeroGyroYaw(toAngle: Angle = 0.degrees) {
+    gyroIO.zeroGyroYaw(toAngle)
     // TODO(parth): This feels incorrect -- I think the first arg should be the gyro angle and the
     // undrifted pose should be updated to toAngle
     if (RobotBase.isSimulation()) {
       // NOTE(parth): The gyro itself should never need to be reset in-match on a real robot, the
       // odometry can be updated directly
-      gyroIO.zeroGyroYaw(toAngle)
+
       undriftedSwerveDriveOdometry.resetPosition(
         toAngle.inRotation2ds,
         swerveModules.map { it.modulePosition }.toTypedArray(),

src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt

@@ -153,14 +153,17 @@ object GyroIOPigeon2 : GyroIO {
   }
 
   override fun zeroGyroYaw(toAngle: Angle) {
-    gyroYawOffset = toAngle - pigeon2.yaw.value.IEEErem(360.0).degrees
+    Logger.recordOutput("Gyro/gyroYaw", gyroYaw.inDegrees)
+    Logger.recordOutput("Gyro/toAngle", toAngle.inDegrees)
+    gyroYawOffset = toAngle - yawSignal.value.IEEErem(360.0).degrees
+    Logger.recordOutput("Gyro/gyroYawOffsetDegrees", gyroYawOffset.inDegrees)
   }
 
   override fun zeroGyroPitch(toAngle: Angle) {
-    gyroPitchOffset = toAngle - pigeon2.pitch.value.IEEErem(360.0).degrees
+    gyroPitchOffset = toAngle - pitchSignal.value.IEEErem(360.0).degrees
   }
 
   override fun zeroGyroRoll(toAngle: Angle) {
-    gyroRollOffset = toAngle - pigeon2.roll.value.IEEErem(360.0).degrees
+    gyroRollOffset = toAngle - rollSignal.value.IEEErem(360.0).degrees
   }
 }