retrieve target pose in sample

team4099 · Jul 16, 2024 · 4e524c0 · 4e524c0
1 parent 9f2ebb5
commit 4e524c0
Showing 1 changed file with 14 additions and 0 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectory.kt b/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectory.kt
@@ -68,10 +68,24 @@ class CustomTrajectory(
                             desiredState.curvatureRadPerMeter.radians.sin
                 val chassisAccels = ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB
 
+                // Retrieve the target pose
+                val targetPose =
+                    AllianceFlipUtil.apply(
+                        Pose2d(
+                            desiredState.poseMeters.x.meters,
+                            desiredState.poseMeters.y.meters,
+                            desiredRotation.position.radians.radians
+                        )
+                    )
+
                 Request.DrivetrainRequest.ClosedLoop(chassisSpeeds, chassisAccels)
             }
             is ChoreoTrajectory -> {
                 val trajectoryState = trajectory.sample(time.inSeconds)
+
+                // Retrieve the target pose
+                val targetPose = Pose2d(trajectoryState.pose)
+
                 Request.DrivetrainRequest.ClosedLoop(trajectoryState.chassisSpeeds)
             }
             else -> {