add lastStableOdoTField and change when ResetFieldFrameEstimator is c…

…alled
team4099 · Feb 23, 2024 · b8800f2 · b8800f2
1 parent 0af5b02
commit b8800f2
Show file tree

Hide file tree

Showing 5 changed files with 17 additions and 7 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -189,5 +189,9 @@ object RobotContainer {
 
   fun getAutonomousCommand() = AutonomousSelector.getCommand(drivetrain)
 
+  fun drivetrainResetFieldFrameEstimator() {
+    drivetrain.resetFieldFrameEstimator(drivetrain.fieldTRobot)
+  }
+
   fun mapTunableCommands() {}
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt
@@ -1,5 +1,6 @@
 package com.team4099.robot2023.auto
 
+import com.team4099.robot2023.RobotContainer
 import com.team4099.robot2023.auto.mode.TestAutoPath
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import edu.wpi.first.networktables.GenericEntry
@@ -57,6 +58,7 @@ object AutonomousSelector {
 
   fun getCommand(drivetrain: Drivetrain): Command {
     val mode = AutonomousMode.TEST_AUTO_PATH
+    RobotContainer.drivetrainResetFieldFrameEstimator()
     // val mode = autonomousModeChooser.get()
     //    println("${waitTime().inSeconds} wait command")
     when (mode) {

diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
@@ -147,6 +147,7 @@ private constructor(
 
   private var drivePoseSupplier: () -> Pose2d
   private var odoTField: Transform2d = Transform2d(Translation2d(), 0.0.degrees)
+  private var lastStableOdoTField: Transform2d = Transform2d(Translation2d(), 0.0.degrees)
 
   private var errorString = ""
 
@@ -214,6 +215,8 @@ private constructor(
 
   override fun initialize() {
     odoTField = drivetrain.odomTField
+    lastStableOdoTField = drivetrain.lastStableOdomTField
+
     pathTransform =
       Transform2d(
         Translation2d(waypoints.get()[0].translation),

diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt
@@ -65,6 +65,7 @@ class FollowPathPlannerPathCommand(
 
   private var drivePoseSupplier: () -> Pose2d
   private var odoTField: Transform2d = Transform2d(Translation2d(), 0.0.degrees)
+  private var lastStableOdoTField: Transform2d = Transform2d(Translation2d(), 0.0.degrees)
 
   private val atReference: Boolean
     get() =
@@ -174,6 +175,7 @@ class FollowPathPlannerPathCommand(
   override fun initialize() {
     trajStartTime = Clock.fpgaTime
     odoTField = drivetrain.odomTField
+    lastStableOdoTField = drivetrain.lastStableOdomTField
 
     currentSpeeds = drivetrain.targetedChassisSpeeds
     poseRotation = drivePoseSupplier().rotation.inRotation2ds

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
@@ -180,16 +180,15 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
     )
 
   val odomTRobot: Pose2d
-    get() = Pose2d(swerveDriveOdometry.poseMeters)
-
-  // NOTE(parth): This should be expected to be noisy. Avoid using this directly if possible.
+    get () = Pose2d(swerveDriveOdometry.poseMeters)
+  // NOTE (parth): This should be expected to be noisy. Avoid using this directly if possible.
   val fieldTRobot: Pose2d
     get() =
-      (fieldFrameEstimator.getLatestOdometryTField().inverse() + odomTRobot.asTransform2d())
-        .asPose2d()
-
+      (fieldFrameEstimator.getLatestOdometryTField().inverse() + odomTRobot.asTransform2d ()). asPose2d()
   val odomTField: Transform2d
-    get() = fieldFrameEstimator.getLatestOdometryTField()
+    get () = fieldFrameEstimator.getLatestOdometryTField()
+  val lastStableOdomTField: Transform2d
+    get () = fieldFrameEstimator.getLatestOdometryTField ()
 
   private var undriftedPose: Pose2d
     get() = Pose2d(undriftedSwerveDriveOdometry.poseMeters)