finished choreo integration

team4099 · Aug 12, 2024 · eee8bca · eee8bca
1 parent 8484c9d
commit eee8bca
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 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
@@ -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
@@ -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
@@ -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
@@ -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)