Use field frame estimator in drivetrain

Topic: field-frame-drivetrain-2 Relative: refactor-vision-update Reviewers: saraansh, rithvik, pranav
team4099 · Feb 18, 2024 · 5093d38 · 5093d38
1 parent 63f2950
commit 5093d38
Show file tree

Hide file tree

Showing 6 changed files with 103 additions and 126 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/FollowPathPlannerPathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt
@@ -53,9 +53,9 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla
   private val atReference: Boolean
     get() =
       (
-        (lastSampledPose.translation - drivetrain.odometryPose.translation).magnitude.meters <=
+        (lastSampledPose.translation - drivetrain.odomTRobot.translation).magnitude.meters <=
           translationToleranceAtEnd &&
-          (lastSampledPose.rotation - drivetrain.odometryPose.rotation).absoluteValue <=
+          (lastSampledPose.rotation - drivetrain.odomTRobot.rotation).absoluteValue <=
           thetaToleranceAtEnd
         )
 
@@ -176,7 +176,7 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla
     }
 
     val currentSpeeds = drivetrain.targetedChassisSpeeds
-    val poseRotation = drivetrain.odometryPose.rotation.inRotation2ds
+    val poseRotation = drivetrain.odomTRobot.rotation.inRotation2ds
     val generatedTrajectory = path.getTrajectory(currentSpeeds, poseRotation)
 
     // Sampling the trajectory for a state that we're trying to target
@@ -194,13 +194,13 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla
     Logger.recordOutput(
       "Pathfollow/thetaSetpoint", stateFromTrajectory.targetHolonomicPose.rotation.degrees
     )
-    Logger.recordOutput("Pathfollow/currentTheta", drivetrain.odometryPose.rotation.inDegrees)
+    Logger.recordOutput("Pathfollow/currentTheta", drivetrain.odomTRobot.rotation.inDegrees)
 
     lastSampledPose = Pose2d(stateFromTrajectory.targetHolonomicPose)
 
     val targetedChassisSpeeds =
       swerveDriveController.calculateRobotRelativeSpeeds(
-        drivetrain.odometryPose, stateFromTrajectory
+        drivetrain.odomTRobot, stateFromTrajectory
       )
 
     // Set closed loop request

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
     )