working choreo integration

team4099 · Aug 5, 2024 · 8484c9d · 8484c9d
1 parent e0fedf4
commit 8484c9d
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 5 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt
@@ -139,10 +139,9 @@ object AutonomousSelector {
       AutonomousMode.TEST_AUTO_PATH ->
         return WaitCommand(waitTime.inSeconds)
           .andThen({
-            drivetrain.tempZeroGyroYaw(TestAutoPath.startingPose.rotation)
-            drivetrain.resetFieldFrameEstimator(
-              AllianceFlipUtil.apply(TestAutoPath.startingPose)
-            )
+            val flippedPose = AllianceFlipUtil.apply(TestAutoPath.startingPose)
+            drivetrain.tempZeroGyroYaw(flippedPose.rotation)
+            drivetrain.resetFieldFrameEstimator(flippedPose)
           })
           .andThen(TestAutoPath(drivetrain, superstructure))
       else -> println("ERROR: unexpected auto mode: $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
@@ -270,7 +270,7 @@ private constructor(
     drivetrain.targetPose =
       targetPose.relativeTo(drivetrain.odomTField.asPose2d())
 
-    CustomLogger.recordOutput(
+    CustomLogger.recordDebugOutput(
       "Pathfollow/targetPose",
       targetPose.pose2d
     )