custom traj madae

team4099 · Jul 9, 2024 · 5ef342a · 5ef342a
1 parent 1b02cae
commit 5ef342a
Show file tree

Hide file tree

Showing 3 changed files with 69 additions and 0 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
@@ -1,5 +1,7 @@
 package com.team4099.robot2023.commands.drivetrain
 
+import com.choreo.lib.Choreo
+import com.choreo.lib.ChoreoTrajectory
 import com.team4099.lib.logging.LoggedTunableValue
 import com.team4099.lib.math.asPose2d
 import com.team4099.lib.math.asTransform2d

diff --git a/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectory.kt b/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectory.kt
@@ -0,0 +1,58 @@
+package com.team4099.robot2023.util
+
+import com.choreo.lib.ChoreoTrajectory
+import com.team4099.lib.trajectory.CustomHolonomicDriveController
+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.team4099.lib.geometry.Pose2d
+import org.team4099.lib.kinematics.ChassisAccels
+import org.team4099.lib.units.base.Time
+import org.team4099.lib.units.base.inSeconds
+import org.team4099.lib.units.base.meters
+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.perSecond
+
+class CustomTrajectory(
+    val poseSupplier: () -> Pose2d,
+    val trajectory: TrajectoryTypes,
+    val trajectoryGenerator: CustomTrajectoryGenerator,
+    val swerveDriveController: CustomHolonomicDriveController
+) {
+    fun sample(time: Time) : Request.DrivetrainRequest.ClosedLoop {
+        return when (trajectory) {
+            is Trajectory -> {
+                val desiredState = trajectory.sample(time.inSeconds)
+                val desiredRotation = trajectoryGenerator.holonomicRotationSequence.sample(time.inSeconds)
+                val nextDriveState = swerveDriveController.calculate(
+                    poseSupplier().pose2d,
+                    desiredState,
+                    desiredRotation
+                )
+
+                val chassisSpeeds = ChassisSpeeds(nextDriveState.vxMetersPerSecond, nextDriveState.vyMetersPerSecond, nextDriveState.omegaRadiansPerSecond)
+
+                val xAccel =
+                    desiredState.accelerationMetersPerSecondSq.meters.perSecond.perSecond *
+                            desiredState.curvatureRadPerMeter.radians.cos
+                val yAccel =
+                    desiredState.accelerationMetersPerSecondSq.meters.perSecond.perSecond *
+                            desiredState.curvatureRadPerMeter.radians.sin
+                val chassisAccels = ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB
+
+                Request.DrivetrainRequest.ClosedLoop(chassisSpeeds, chassisAccels)
+            }
+            is ChoreoTrajectory -> {
+                val trajectoryState = trajectory.sample(time.inSeconds)
+                Request.DrivetrainRequest.ClosedLoop(trajectoryState.chassisSpeeds)
+            }
+            else -> {
+                println("Unexpected trajectory type ${trajectory::javaClass}")
+                Request.DrivetrainRequest.ClosedLoop(ChassisSpeeds(0.0, 0.0, 0.0))
+            }
+        }
+    }
+}
diff --git a/src/main/kotlin/com/team4099/robot2023/util/TrajectoryTypes.kt b/src/main/kotlin/com/team4099/robot2023/util/TrajectoryTypes.kt
@@ -0,0 +1,9 @@
+package com.team4099.robot2023.util
+
+import com.choreo.lib.ChoreoTrajectory
+import edu.wpi.first.math.trajectory.Trajectory
+
+sealed interface TrajectoryTypes {
+    class WPILIBTrajectoryType : Trajectory()
+    class ChoreoTrajectoryType : ChoreoTrajectory()
+}