Auto stash before rebase of "refs/heads/trajectorysequence-v2"

src/main/kotlin/com/noahbres/meepmeep/roadrunner/trajectorysequence/SequenceSegment.kt

@@ -0,0 +1,25 @@
+package com.noahbres.meepmeep.roadrunner.trajectorysequence
+
+import com.acmerobotics.roadrunner.geometry.Pose2d
+import com.acmerobotics.roadrunner.geometry.Vector2d
+import com.acmerobotics.roadrunner.profile.MotionProfile
+import com.acmerobotics.roadrunner.trajectory.Trajectory
+
+sealed class SequenceSegment(open val duration: Double)
+
+data class TrajectorySegment(
+        val trajectory: Trajectory,
+        override val duration: Double
+) : SequenceSegment(duration)
+
+data class TurnSegment(
+        val startPose: Pose2d,
+        val totalRotation: Double,
+        val motionProfile: MotionProfile,
+        override val duration: Double
+) : SequenceSegment(duration)
+
+data class WaitSegment(
+        val pose: Pose2d,
+        val seconds: Double
+) : SequenceSegment(seconds)

src/main/kotlin/com/noahbres/meepmeep/roadrunner/trajectorysequence/TrajectorySequence.kt

@@ -0,0 +1,67 @@
+package com.noahbres.meepmeep.roadrunner.trajectorysequence
+
+import com.acmerobotics.roadrunner.geometry.Pose2d
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker
+
+class TrajectorySequence(
+        val sequenceSegments: List<SequenceSegment>,
+        val duration: Double,
+        val markers: List<TrajectoryMarker> = emptyList()
+) {
+    operator fun get(time: Double): Pose2d {
+        val (currentSegment, segmentTime) = getCurrentState(time)
+
+        return when(currentSegment) {
+            is TrajectorySegment -> currentSegment.trajectory[segmentTime]
+            is TurnSegment -> {
+                val turnAngle = currentSegment.motionProfile[segmentTime].x
+
+                currentSegment.startPose.copy(heading = currentSegment.startPose.heading + turnAngle)
+            }
+            is WaitSegment -> currentSegment.pose
+            null -> Pose2d()
+        }
+    }
+
+    fun velocity(time: Double): Pose2d {
+        val (currentSegment, segmentTime) = getCurrentState(time)
+
+        return when(currentSegment) {
+            is TrajectorySegment -> currentSegment.trajectory.velocity(segmentTime)
+            is TurnSegment -> Pose2d(0.0, 0.0, currentSegment.motionProfile[segmentTime].v)
+            is WaitSegment -> Pose2d()
+            null -> Pose2d()
+        }
+    }
+
+    fun acceleration(time: Double): Pose2d {
+        val (currentSegment, segmentTime) = getCurrentState(time)
+
+        return when(currentSegment) {
+            is TrajectorySegment -> currentSegment.trajectory.acceleration(segmentTime)
+            is TurnSegment -> Pose2d(0.0, 0.0, currentSegment.motionProfile[segmentTime].a)
+            is WaitSegment -> Pose2d()
+            null -> Pose2d()
+        }
+    }
+
+    private fun getCurrentState(time: Double): Pair<SequenceSegment?, Double> {
+        var currentTime = 0.0
+
+        sequenceSegments.forEach {
+            if(currentTime + it.duration > time) {
+                val segmentTime = time - currentTime
+
+                return Pair(it, segmentTime)
+            } else {
+                currentTime += it.duration
+            }
+        }
+
+        return Pair(null, 0.0)
+    }
+
+    fun start() = get(0.0)
+
+    fun end() = get(duration)
+}