Add Roadrunner

TeamCode/trajectorysequence/sequencesegment/SequenceSegment.java

@@ -0,0 +1,40 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+
+import java.util.List;
+
+public abstract class SequenceSegment {
+    private final double duration;
+    private final Pose2d startPose;
+    private final Pose2d endPose;
+    private final List<TrajectoryMarker> markers;
+
+    protected SequenceSegment(
+            double duration,
+            Pose2d startPose, Pose2d endPose,
+            List<TrajectoryMarker> markers
+    ) {
+        this.duration = duration;
+        this.startPose = startPose;
+        this.endPose = endPose;
+        this.markers = markers;
+    }
+
+    public double getDuration() {
+        return this.duration;
+    }
+
+    public Pose2d getStartPose() {
+        return startPose;
+    }
+
+    public Pose2d getEndPose() {
+        return endPose;
+    }
+
+    public List<TrajectoryMarker> getMarkers() {
+        return markers;
+    }
+}

TeamCode/trajectorysequence/sequencesegment/TrajectorySegment.java

@@ -0,0 +1,20 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+
+import java.util.Collections;
+
+public final class TrajectorySegment extends SequenceSegment {
+    private final Trajectory trajectory;
+
+    public TrajectorySegment(Trajectory trajectory) {
+        // Note: Markers are already stored in the `Trajectory` itself.
+        // This class should not hold any markers
+        super(trajectory.duration(), trajectory.start(), trajectory.end(), Collections.emptyList());
+        this.trajectory = trajectory;
+    }
+
+    public Trajectory getTrajectory() {
+        return this.trajectory;
+    }
+}

TeamCode/trajectorysequence/sequencesegment/TurnSegment.java

@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.profile.MotionProfile;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+import com.acmerobotics.roadrunner.util.Angle;
+
+import java.util.List;
+
+public final class TurnSegment extends SequenceSegment {
+    private final double totalRotation;
+    private final MotionProfile motionProfile;
+
+    public TurnSegment(Pose2d startPose, double totalRotation, MotionProfile motionProfile, List<TrajectoryMarker> markers) {
+        super(
+                motionProfile.duration(),
+                startPose,
+                new Pose2d(
+                        startPose.getX(), startPose.getY(),
+                        Angle.norm(startPose.getHeading() + totalRotation)
+                ),
+                markers
+        );
+
+        this.totalRotation = totalRotation;
+        this.motionProfile = motionProfile;
+    }
+
+    public final double getTotalRotation() {
+        return this.totalRotation;
+    }
+
+    public final MotionProfile getMotionProfile() {
+        return this.motionProfile;
+    }
+}

TeamCode/trajectorysequence/sequencesegment/WaitSegment.java

@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+
+import java.util.List;
+
+public final class WaitSegment extends SequenceSegment {
+    public WaitSegment(Pose2d pose, double seconds, List<TrajectoryMarker> markers) {
+        super(seconds, pose, pose, markers);
+    }
+}