implement sim pose estimation

FRC3636 · Jan 18, 2024 · 3d22ff4 · 3d22ff4
1 parent f2b79c5
commit 3d22ff4
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 12 deletions.
diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt
@@ -54,6 +54,11 @@ object Drivetrain : Subsystem {
         io.updateInputs(inputs)
         Logger.processInputs("Drivetrain", inputs)
 
+        poseEstimator.update(
+            inputs.gyroRotation.toRotation2d(),
+            inputs.measuredPositions.toTypedArray()
+        )
+
         Logger.recordOutput("Drivetrain/EstimatedPose", estimatedPose)
     }
 
@@ -158,8 +163,8 @@ class DrivetrainIOReal : DrivetrainIO() {
 }
 
 class DrivetrainIOSim : DrivetrainIO() {
-    override val gyro = GyroSim()
     override val modules = PerCorner.generate { SimSwerveModule() }
+    override val gyro = GyroSim(modules.map { it })
 }
 
 // Constants

diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/drivetrain/Gyro.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/drivetrain/Gyro.kt
@@ -1,8 +1,12 @@
 package com.frcteam3636.frc2024.subsystems.drivetrain
 
+import com.frcteam3636.frc2024.Robot
+import com.frcteam3636.frc2024.utils.swerve.PerCorner
 import com.kauailabs.navx.frc.AHRS
 import edu.wpi.first.math.geometry.Quaternion
 import edu.wpi.first.math.geometry.Rotation3d
+import edu.wpi.first.math.geometry.Translation2d
+import kotlin.math.sign
 
 
 interface Gyro {
@@ -16,7 +20,7 @@ class GyroNavX(private var offset: Rotation3d = Rotation3d()) : Gyro {
     private val ahrs = AHRS()
 
     override var rotation: Rotation3d
-        get() = Rotation3d(
+        get() = offset + Rotation3d(
             Quaternion(
                 ahrs.quaternionW.toDouble(),
                 ahrs.quaternionX.toDouble(),
@@ -25,18 +29,20 @@ class GyroNavX(private var offset: Rotation3d = Rotation3d()) : Gyro {
             )
         )
         set(value) {
-            val currentRotation = Rotation3d(
-                Quaternion(
-                    ahrs.quaternionW.toDouble(),
-                    ahrs.quaternionX.toDouble(),
-                    ahrs.quaternionY.toDouble(),
-                    ahrs.quaternionZ.toDouble()
-                )
-            )
-            offset = currentRotation - rotation
+            offset = value - rotation
         }
 }
 
-class GyroSim : Gyro {
+class GyroSim(private val modules: PerCorner<SwerveModule>) : Gyro {
     override var rotation = Rotation3d()
+
+    override fun periodic() {
+        val moduleVelocities = modules.map { Translation2d(it.state.speedMetersPerSecond, it.state.angle) }
+        val translationVelocity = moduleVelocities.reduce(Translation2d::plus) / 4.0;
+        val rotationalVelocities = moduleVelocities.map { it - translationVelocity }
+        val yawVelocity =
+            sign(rotationalVelocities.frontLeft.y) * rotationalVelocities.frontLeft.norm / MODULE_POSITIONS.frontLeft.translation.norm
+
+        rotation += Rotation3d(0.0, 0.0, yawVelocity) * Robot.period
+    }
 }