Use field frame estimator in drivetrain

Topic: field-frame-drivetrain-2 Relative: refactor-vision-update Reviewers: saraansh, rithvik, pranav
team4099 · Feb 18, 2024 · c1db09f · c1db09f
1 parent 39a8984
commit c1db09f
Show file tree

Hide file tree

Showing 5 changed files with 98 additions and 123 deletions.
diff --git a/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt b/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt
@@ -39,3 +39,11 @@ fun Pose2d.purelyTranslateBy(translation2d: Translation2d): Pose2d {
 fun Pose2d.asTransform2d(): Transform2d {
   return Transform2d(Pose2d(0.meters, 0.meters, 0.radians), this)
 }
+
+/**
+ * Returns pose of whatever the transform represents, in the original frame of the transform. For
+ * example, odomTRobot.asPose2d() would give the pose of the robot in the odometry frame.
+ */
+fun Transform2d.asPose2d(): Pose2d {
+  return Pose2d(this.translation, this.rotation)
+}
diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -89,8 +89,8 @@ object RobotContainer {
     }
 
     superstructure = Superstructure(intake, feeder, elevator, wrist, flywheel)
-    vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) })
-    limelight.poseSupplier = { drivetrain.odometryPose }
+    vision.setDataInterfaces({ drivetrain.fieldTRobot }, { drivetrain.addVisionData(it) })
+    limelight.poseSupplier = { drivetrain.odomTRobot }
   }
 
   fun mapDefaultCommands() {

diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
@@ -1,7 +1,6 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.lib.logging.LoggedTunableValue
-import com.team4099.lib.pathfollow.Trajectory
 import com.team4099.lib.trajectory.CustomHolonomicDriveController
 import com.team4099.lib.trajectory.CustomTrajectoryGenerator
 import com.team4099.lib.trajectory.Waypoint
@@ -227,9 +226,7 @@ class DrivePathCommand(
         desiredState.curvatureRadPerMeter.radians.sin
 
     var nextDriveState =
-      swerveDriveController.calculate(
-        drivetrain.odometryPose.pose2d, desiredState, desiredRotation
-      )
+      swerveDriveController.calculate(drivetrain.odomTRobot.pose2d, desiredState, desiredRotation)
 
     if (leaveOutYAdjustment) {
       nextDriveState =

diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt
@@ -13,7 +13,7 @@ class ResetPoseCommand(val drivetrain: Drivetrain, val pose: Pose2d) : Command()
   }
 
   override fun initialize() {
-    drivetrain.odometryPose = AllianceFlipUtil.apply(pose)
+    drivetrain.resetFieldFrameEstimator(AllianceFlipUtil.apply(pose))
     Logger.recordOutput(
       "Drivetrain/lastResetPose", Pose2dWPILIB.struct, AllianceFlipUtil.apply(pose).pose2d
     )