From 304cbf785ab1805b6b43e8b9d2bd6ada294d8a2b Mon Sep 17 00:00:00 2001
From: SirBeans <ryanchung0084@gmail.com>
Date: Wed, 17 Jul 2024 12:00:20 -0400
Subject: [PATCH] Make Custom Trajectory return pose and drive request

---
 .../commands/drivetrain/DrivePathCommand.kt        |  6 ++++--
 .../team4099/robot2023/util/CustomTrajectory.kt    | 14 ++++++++------
 2 files changed, 12 insertions(+), 8 deletions(-)

diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
index 352865f3..1d8e99b2 100644
--- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
+++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
@@ -263,7 +263,8 @@ private constructor(
     var desiredRotation =
       trajectoryGenerator.holonomicRotationSequence.sample(trajCurTime.inSeconds)
 
-    val targetHolonomicPose =
+    val targetHolonomicPose = generatedTrajectory.sample(trajCurTime).pose2d
+      /*
       AllianceFlipUtil.apply(
         Pose2d(
           desiredState.poseMeters.x.meters,
@@ -271,13 +272,14 @@ private constructor(
           desiredRotation.position.radians.radians
         )
       )
+       */
 
     var robotPoseInSelectedFrame: Pose2d = drivePoseSupplier()
     if (pathFrame == stateFrame) {
       lastSampledPose = targetHolonomicPose
       when (stateFrame) {
         FrameType.FIELD -> {
-          Logger.recordOutput("Pathfollow/fieldTRobotTargetVisualized", targetHolonomicPose.pose2d)
+          Logger.recordOutput("Pathfollow/fieldTRobotTargetVisualized",targetHolonomicPose.pose2d)
 
           Logger.recordOutput("Pathfollow/fieldTRobot", robotPoseInSelectedFrame.pose2d)
         }
diff --git a/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectory.kt b/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectory.kt
index c47a9857..05e91258 100644
--- a/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectory.kt
+++ b/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectory.kt
@@ -47,7 +47,7 @@ class CustomTrajectory(
             }
         }
 
-    fun sample(time: Time) : Request.DrivetrainRequest.ClosedLoop {
+    fun sample(time: Time) : CustomTrajectoryState {
         return when (trajectory) {
             is Trajectory -> {
                 val desiredState = trajectory.sample(time.inSeconds)
@@ -78,7 +78,8 @@ class CustomTrajectory(
                         )
                     )
 
-                Request.DrivetrainRequest.ClosedLoop(chassisSpeeds, chassisAccels)
+                val drivetrainRequest = Request.DrivetrainRequest.ClosedLoop(chassisSpeeds, chassisAccels)
+                CustomTrajectoryState(targetPose, drivetrainRequest)
             }
             is ChoreoTrajectory -> {
                 val trajectoryState = trajectory.sample(time.inSeconds)
@@ -86,11 +87,12 @@ class CustomTrajectory(
                 // Retrieve the target pose
                 val targetPose = Pose2d(trajectoryState.pose)
 
-                Request.DrivetrainRequest.ClosedLoop(trajectoryState.chassisSpeeds)
-            }
-            else -> {
+                val drivetrainRequest = Request.DrivetrainRequest.ClosedLoop(trajectoryState.chassisSpeeds)
+
+                CustomTrajectoryState(targetPose, drivetrainRequest)
+            } else -> {
                 println("Unexpected trajectory type ${trajectory::javaClass}")
-                Request.DrivetrainRequest.ClosedLoop(ChassisSpeeds(0.0, 0.0, 0.0))
+                CustomTrajectoryState(Pose2d(0.meters, 0.meters, 0.radians), Request.DrivetrainRequest.ClosedLoop(ChassisSpeeds(0.0, 0.0, 0.0)))
             }
         }
     }