Clear errors in Drive Path command still have errors with .calculate …

…(its not abstracted yet) and a few other minor errors to fix

src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt

@@ -1,5 +1,8 @@
 package com.team4099.lib.trajectory
 
+import com.team4099.robot2023.util.CustomTrajectoryState
+import com.team4099.robot2023.util.TrajectoryTypes
+import com.team4099.robot2023.util.driver.DriveStateTypes
 import edu.wpi.first.math.controller.PIDController
 import edu.wpi.first.math.geometry.Pose2d
 import edu.wpi.first.math.geometry.Rotation2d
@@ -116,16 +119,31 @@ class CustomHolonomicDriveController(
    */
   fun calculate(
     currentPose: Pose2d,
-    driveState: Trajectory.State,
+    driveState: DriveStateTypes,
     holonomicRotationState: RotationSequence.State
   ): ChassisSpeeds {
-    return calculate(
-      currentPose,
-      driveState.poseMeters,
-      driveState.velocityMetersPerSecond,
-      holonomicRotationState.position,
-      holonomicRotationState.velocityRadiansPerSec
-    )
+
+    return when (driveState) {
+      is DriveStateTypes.WPILIBDriveStateType -> {
+        calculate(
+          currentPose,
+          driveState.state.poseMeters,
+          driveState.state.velocityMetersPerSecond,
+          holonomicRotationState.position,
+          holonomicRotationState.velocityRadiansPerSec
+        )
+      }
+      is DriveStateTypes.CustomDriveState -> {
+        calculate(
+          currentPose,
+          driveState.state.pose2d.pose2d,
+          driveState.state.velocityMetersPerSecond,
+          holonomicRotationState.position,
+          holonomicRotationState.velocityRadiansPerSec
+        )
+      }
+    }
+
   }
 
   /**

src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt

@@ -20,6 +20,7 @@ import com.team4099.robot2023.util.TrajectoryTypes
 import com.team4099.robot2023.util.Velocity2d
 import edu.wpi.first.math.kinematics.ChassisSpeeds
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics
+import edu.wpi.first.math.trajectory.Trajectory
 import edu.wpi.first.math.trajectory.TrajectoryParameterizer.TrajectoryGenerationException
 import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint
 import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint
@@ -41,24 +42,7 @@ import org.team4099.lib.units.base.inSeconds
 import org.team4099.lib.units.base.inches
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.base.seconds
-import org.team4099.lib.units.derived.Radian
-import org.team4099.lib.units.derived.cos
-import org.team4099.lib.units.derived.degrees
-import org.team4099.lib.units.derived.inMetersPerSecondPerMeter
-import org.team4099.lib.units.derived.inMetersPerSecondPerMeterSeconds
-import org.team4099.lib.units.derived.inMetersPerSecondPerMetersPerSecond
-import org.team4099.lib.units.derived.inRadians
-import org.team4099.lib.units.derived.inRadiansPerSecondPerRadian
-import org.team4099.lib.units.derived.inRadiansPerSecondPerRadianPerSecond
-import org.team4099.lib.units.derived.inRadiansPerSecondPerRadianSeconds
-import org.team4099.lib.units.derived.metersPerSecondPerMetersPerSecond
-import org.team4099.lib.units.derived.perMeter
-import org.team4099.lib.units.derived.perMeterSeconds
-import org.team4099.lib.units.derived.perRadian
-import org.team4099.lib.units.derived.perRadianPerSecond
-import org.team4099.lib.units.derived.perRadianSeconds
-import org.team4099.lib.units.derived.radians
-import org.team4099.lib.units.derived.sin
+import org.team4099.lib.units.derived.*
 import org.team4099.lib.units.inMetersPerSecond
 import org.team4099.lib.units.inMetersPerSecondPerSecond
 import org.team4099.lib.units.inRadiansPerSecond
@@ -71,7 +55,7 @@ import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainReques
 class DrivePathCommand<T : Waypoint>
 private constructor(
   val drivetrain: Drivetrain,
-  private val trajectory: TrajectoryTypes,
+  private var trajectory: TrajectoryTypes,
   val resetPose: Boolean = false,
   val useLowerTolerance: Boolean = false,
   val flipForAlliances: Boolean = true,
@@ -248,6 +232,10 @@ private constructor(
   }
 
   override fun execute() {
+
+    val trajectory = trajectoryGenerator.driveTrajectory
+
+
     if (generatedTrajectory.totalStates <= 1) {
       return
     }
@@ -258,6 +246,7 @@ private constructor(
     }
 
     trajCurTime = Clock.fpgaTime - trajStartTime
+
     var desiredState = generatedTrajectory.sample(trajCurTime)
 
     var desiredRotation =
@@ -317,35 +306,30 @@ private constructor(
     Logger.recordOutput("Pathfollow/targetPoseInStateFrame", lastSampledPose.pose2d)
 
     if (flipForAlliances) {
-      desiredState = AllianceFlipUtil.apply(desiredState)
+      desiredState = apply(desiredState)
       desiredRotation = AllianceFlipUtil.apply(desiredRotation)
     }
 
-    val xAccel =
-      desiredState.accelerationMetersPerSecondSq.meters.perSecond.perSecond *
-        desiredState.curvatureRadPerMeter.radians.cos
-    var yAccel =
-      desiredState.accelerationMetersPerSecondSq.meters.perSecond.perSecond *
-        desiredState.curvatureRadPerMeter.radians.sin
-
     var nextDriveState =
       swerveDriveController.calculate(
         robotPoseInSelectedFrame.pose2d, desiredState, desiredRotation
       )
-
+/*
     if (leaveOutYAdjustment) {
       nextDriveState =
         ChassisSpeeds(nextDriveState.vxMetersPerSecond, 0.0, nextDriveState.omegaRadiansPerSecond)
       yAccel = 0.0.meters.perSecond.perSecond
     }
+    */
+    //TODO figure out wtf this is for and if it was needed reimplement it
 
     drivetrain.targetPose =
-      Pose2d(Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position))
+      Pose2d(Pose2dWPILIB(desiredState.pose2d.translation.translation2d, desiredRotation.position))
 
     Logger.recordOutput(
       "Pathfollow/target",
       Pose2dWPILIB.struct,
-      Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position)
+      Pose2dWPILIB(desiredState.pose2d.translation.translation2d, desiredRotation.position)
     )
 
     /*
@@ -358,11 +342,7 @@ private constructor(
 
      */
 
-    drivetrain.currentRequest =
-      DrivetrainRequest.ClosedLoop(
-        nextDriveState,
-        ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB
-      )
+    drivetrain.currentRequest = desiredState.request
 
     Logger.recordOutput("Pathfollow/thetaPIDPositionErrorRadians", thetaPID.error.inRadians)
 
@@ -372,21 +352,15 @@ private constructor(
       "Pathfollow/thetaPIDVelocityErrorRadians", thetaPID.errorDerivative.inRadiansPerSecond
     )
 
-    Logger.recordOutput(
-      "Pathfollow/xAccelMetersPerSecondPerSecond", xAccel.inMetersPerSecondPerSecond
-    )
-    Logger.recordOutput(
-      "Pathfollow/yAccelMetersPerSecondPerSecond", yAccel.inMetersPerSecondPerSecond
-    )
-
     Logger.recordOutput("Pathfollow/Start Time", trajStartTime.inSeconds)
     Logger.recordOutput("Pathfollow/Current Time", trajCurTime.inSeconds)
     Logger.recordOutput(
-      "Pathfollow/Desired Angle in Degrees", desiredState.poseMeters.rotation.degrees
+      "Pathfollow/Desired Angle in Degrees", desiredState.pose2d.rotation.inDegrees
     )
 
     Logger.recordOutput("Pathfollow/isAtReference", swerveDriveController.atReference())
     Logger.recordOutput("Pathfollow/trajectoryTimeSeconds", trajectory.totalTimeSeconds)
+    //totalTimeSEconds
 
     CustomLogger.recordDebugOutput("ActiveCommands/DrivePathCommand", true)
 

src/main/kotlin/com/team4099/robot2023/util/CustomTrajectory.kt

@@ -6,6 +6,7 @@ import com.team4099.lib.trajectory.CustomTrajectoryGenerator
 import com.team4099.robot2023.subsystems.superstructure.Request
 import edu.wpi.first.math.kinematics.ChassisSpeeds
 import edu.wpi.first.math.trajectory.Trajectory
+import org.littletonrobotics.junction.Logger
 import org.team4099.lib.geometry.Pose2d
 import org.team4099.lib.kinematics.ChassisAccels
 import org.team4099.lib.units.base.Time
@@ -15,6 +16,7 @@ import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.cos
 import org.team4099.lib.units.derived.radians
 import org.team4099.lib.units.derived.sin
+import org.team4099.lib.units.inMetersPerSecondPerSecond
 import org.team4099.lib.units.perSecond
 
 class CustomTrajectory(
@@ -66,6 +68,14 @@ class CustomTrajectory(
                 val yAccel =
                     desiredState.accelerationMetersPerSecondSq.meters.perSecond.perSecond *
                             desiredState.curvatureRadPerMeter.radians.sin
+
+                Logger.recordOutput(
+                    "Pathfollow/xAccelMetersPerSecondPerSecond", xAccel.inMetersPerSecondPerSecond
+                )
+                Logger.recordOutput(
+                    "Pathfollow/yAccelMetersPerSecondPerSecond", yAccel.inMetersPerSecondPerSecond
+                )
+
                 val chassisAccels = ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB
 
                 // Retrieve the target pose

src/main/kotlin/com/team4099/robot2023/util/driver/DriveStateTypes.kt

@@ -0,0 +1,9 @@
+package com.team4099.robot2023.util.driver
+
+import com.team4099.robot2023.util.CustomTrajectoryState
+import edu.wpi.first.math.trajectory.Trajectory
+
+sealed interface DriveStateTypes {
+     data class WPILIBDriveStateType(val state: Trajectory.State) : DriveStateTypes
+     data class CustomDriveState(val state: CustomTrajectoryState) : DriveStateTypes
+}