Make Custom Trajectory return pose and drive request

team4099 · Jul 17, 2024 · 304cbf7 · 304cbf7
1 parent 4e524c0
commit 304cbf7
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 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
@@ -263,21 +263,23 @@ private constructor(
     var desiredRotation =
       trajectoryGenerator.holonomicRotationSequence.sample(trajCurTime.inSeconds)
 
-    val targetHolonomicPose =
+    val targetHolonomicPose = generatedTrajectory.sample(trajCurTime).pose2d
+      /*
       AllianceFlipUtil.apply(
         Pose2d(
           desiredState.poseMeters.x.meters,
           desiredState.poseMeters.y.meters,
           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
@@ -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,19 +78,21 @@ 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)
 
                 // 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)))
             }
         }
     }