move log target pose to DrivePathCommand

team4099 · Jul 5, 2024 · 7d30d77 · 7d30d77
1 parent f448483
commit 7d30d77
Show file tree

Hide file tree

Showing 4 changed files with 3 additions and 3 deletions.
diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock
diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe
diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
@@ -338,6 +338,8 @@ private constructor(
     drivetrain.targetPose =
       Pose2d(Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position))
 
+    CustomLogger.recordOutput("Odometry/targetPose", drivetrain.targetPose.pose2d)
+
     Logger.recordOutput(
       "Pathfollow/target",
       Pose2dWPILIB.struct,

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/Drivetrain.kt
@@ -254,7 +254,6 @@ class Drivetrain(private val gyroIO: GyroIO, val swerveModules: List<SwerveModul
     CustomLogger.recordOutput("Drivetrain/ModuleStates", *measuredStates.toTypedArray())
     CustomLogger.recordOutput("Drivetrain/SetpointStates", *setPointStates.toTypedArray())
 
-    CustomLogger.recordOutput("Odometry/pose", odomTRobot.pose2d)
     CustomLogger.recordOutput(
       "Odometry/pose3d",
       Pose3d(
@@ -268,8 +267,6 @@ class Drivetrain(private val gyroIO: GyroIO, val swerveModules: List<SwerveModul
 
     CustomLogger.recordOutput("FieldFrameEstimator/odomTField", odomTField.transform2d)
 
-    CustomLogger.recordOutput("Odometry/targetPose", targetPose.pose2d)
-
     // Log the current state
     CustomLogger.recordOutput("Drivetrain/currentState", currentState.toString())
     // Log the current state
@@ -558,6 +555,7 @@ class Drivetrain(private val gyroIO: GyroIO, val swerveModules: List<SwerveModul
     fieldFrameEstimator.addVisionData(visionData)
   }
 
+
   fun addSpeakerVisionData(visionData: TimestampedTrigVisionUpdate) {
     fieldFrameEstimator.addSpeakerVisionData(visionData)
   }