From eee8bca2a29dc8ff16ef511fb1115d9d1a03e240 Mon Sep 17 00:00:00 2001
From: Shom770 <shayaanwadkar@gmail.com>
Date: Mon, 12 Aug 2024 18:12:01 -0400
Subject: [PATCH] finished choreo integration

---
 .../team4099/robot2023/auto/mode/ExamplePathAuto.kt  |  9 +++++++--
 .../commands/drivetrain/DrivePathCommand.kt          |  8 --------
 .../subsystems/drivetrain/drive/Drivetrain.kt        |  7 -------
 .../com/team4099/robot2023/util/CustomTrajectory.kt  | 12 +++++++++---
 4 files changed, 16 insertions(+), 20 deletions(-)

diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt
index cefa3b1c..b3482617 100644
--- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt
+++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt
@@ -29,9 +29,14 @@ class ExamplePathAuto(val drivetrain: Drivetrain) : SequentialCommandGroup() {
               ),
               FieldWaypoint(
                 Translation2d(2.meters, 5.535.meters).translation2d,
-                null,
+                0.degrees.inRotation2ds,
                 startingPose.rotation.inRotation2ds
-              )
+              ),
+              FieldWaypoint(
+                Translation2d(5.meters, 7.535.meters).translation2d,
+                null,
+                90.degrees.inRotation2ds
+              ),
             )
           }
         )
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 d820c340..d17d96e5 100644
--- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
+++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
@@ -267,14 +267,6 @@ private constructor(
       }
     }
 
-    drivetrain.targetPose =
-      targetPose.relativeTo(drivetrain.odomTField.asPose2d())
-
-    CustomLogger.recordDebugOutput(
-      "Pathfollow/targetPose",
-      targetPose.pose2d
-    )
-
     /*
     drivetrain.setOpenLoop(
         nextDriveState.omegaRadiansPerSecond.radians.perSecond,
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt
index 22abd3f4..765f8612 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt
@@ -84,8 +84,6 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
   var isInAutonomous = false
     private set
 
-  var targetPose: Pose2d = Pose2d(0.0.meters, 0.0.meters, 0.0.radians)
-
   private var drift: Transform2d = Transform2d(Translation2d(), 0.0.radians)
 
   var fieldVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond)
@@ -326,11 +324,6 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
 
     Logger.recordOutput("FieldFrameEstimator/odomTField", odomTField.transform2d)
 
-    Logger.recordOutput(
-      "Odometry/targetPose",
-      doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians)
-    )
-
     Logger.recordOutput(
       "LoggedRobot/Subsystems/DrivetrainLoopTimeMS",
       (Clock.realTimestamp - startTime).inMilliseconds
diff --git a/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectory.kt b/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectory.kt
index 83754e68..a285838f 100644
--- a/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectory.kt
+++ b/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectory.kt
@@ -65,10 +65,16 @@ class CustomTrajectory(
             is TrajectoryTypes.WPILib -> {
                 val desiredState = trajectory.rawTrajectory.sample(time.inSeconds)
                 val desiredRotation = trajectoryGenerator.holonomicRotationSequence.sample(time.inSeconds)
+                val poseReference = if (stateFrame == FrameType.ODOMETRY) {
+                    drivetrain.odomTField.inverse().asPose2d().transformBy(poseSupplier().asTransform2d())
+                } else {
+                    poseSupplier()
+                }.pose2d
+
                 val nextDriveState = swerveDriveController.calculate(
-                    poseSupplier().pose2d,
-                    desiredState,
-                    desiredRotation
+                    poseReference,
+                    AllianceFlipUtil.apply(desiredState),
+                    AllianceFlipUtil.apply(desiredRotation)
                 )
 
                 val chassisSpeeds = ChassisSpeeds(nextDriveState.vxMetersPerSecond, nextDriveState.vyMetersPerSecond, nextDriveState.omegaRadiansPerSecond)