Update for new actions

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Action.java

@@ -1,261 +0,0 @@
-package org.firstinspires.ftc.teamcode;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.canvas.Canvas;
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.Collections;
-import java.util.List;
-
-public interface Action {
-    void init();
-    default boolean loop(TelemetryPacket p) {
-        return false;
-    }
-
-    default void draw(Canvas c) {
-
-    }
-
-    default void runBlocking() {
-        if (Thread.currentThread().isInterrupted()) return;
-
-        init();
-
-        boolean b = true;
-        while (!Thread.currentThread().isInterrupted() && b) {
-            TelemetryPacket p = new TelemetryPacket();
-
-            draw(p.fieldOverlay());
-            b = loop(p);
-
-            FtcDashboard.getInstance().sendTelemetryPacket(p);
-        }
-    }
-
-    default double clock() {
-        return 1e-9 * System.nanoTime();
-    }
-
-    // not persistent!
-    interface IBuilder<T extends IBuilder<T>> {
-        T add(Action a);
-    }
-
-    // parent-less, generic-less SeqBuilder
-    class Builder implements IBuilder<Builder> {
-        private final List<Action> as = new ArrayList<>();
-
-        @Override
-        public Builder add(Action a) {
-            as.add(a);
-            return this;
-        }
-
-        public SeqBuilder<Builder> seq() {
-            return new SeqBuilder<>(this);
-        }
-
-        public ParBuilder<Builder> par() {
-            return new ParBuilder<>(this);
-        }
-
-        public Builder sleep(double duration) {
-            return add(new SleepAction(duration));
-        }
-
-        public Builder after(double duration, Action a) {
-            return seq().sleep(duration).add(a).end();
-        }
-
-        public Action build() {
-            return new SequentialAction(as);
-        }
-    }
-
-    class SeqBuilder<T extends IBuilder<T>> implements IBuilder<SeqBuilder<T>> {
-        private final T parent;
-        private final List<Action> as = new ArrayList<>();
-
-        private SeqBuilder(T parent) {
-            this.parent = parent;
-        }
-
-        @Override
-        public SeqBuilder<T> add(Action a) {
-            as.add(a);
-            return this;
-        }
-
-        public T end() {
-            parent.add(new SequentialAction(as));
-            return parent;
-        }
-
-        public SeqBuilder<SeqBuilder<T>> seq() {
-            return new SeqBuilder<>(this);
-        }
-
-        public ParBuilder<SeqBuilder<T>> par() {
-            return new ParBuilder<>(this);
-        }
-
-        public SeqBuilder<T> sleep(double duration) {
-            return add(new SleepAction(duration));
-        }
-
-        public SeqBuilder<T> after(double duration, Action a) {
-            return seq().sleep(duration).add(a).end();
-        }
-    }
-
-    final class ParBuilder<T extends IBuilder<T>> implements IBuilder<ParBuilder<T>> {
-        private final T parent;
-        private final List<Action> as = new ArrayList<>();
-
-        private ParBuilder(T parent) {
-            this.parent = parent;
-        }
-
-        @Override
-        public ParBuilder<T> add(Action a) {
-            as.add(a);
-            return this;
-        }
-
-        public T end() {
-            parent.add(new ParallelAction(as));
-            return parent;
-        }
-
-        public SeqBuilder<ParBuilder<T>> seq() {
-            return new SeqBuilder<>(this);
-        }
-
-        public ParBuilder<ParBuilder<T>> par() {
-            return new ParBuilder<>(this);
-        }
-
-        public ParBuilder<T> sleep(double duration) {
-            return add(new SleepAction(duration));
-        }
-
-        public ParBuilder<T> after(double duration, Action a) {
-            return seq().sleep(duration).add(a).end();
-        }
-    }
-}
-
-final class SleepAction implements Action {
-    public final double duration;
-    private double endTs;
-
-    public SleepAction(double duration) {
-        this.duration = duration;
-    }
-
-    @Override
-    public void init() {
-        endTs = clock() + duration;
-    }
-
-    @Override
-    public boolean loop(TelemetryPacket p) {
-        return clock() <= endTs;
-    }
-}
-
-final class ParallelAction implements Action {
-    public final List<Action> actions;
-    private List<Action> remaining;
-
-    public ParallelAction(List<Action> actions) {
-        this.actions = Collections.unmodifiableList(actions);
-    }
-
-    public ParallelAction(Action... actions) {
-        this(Arrays.asList(actions));
-    }
-
-    @Override
-    public void init() {
-        remaining = new ArrayList<>(actions);
-        for (Action a : remaining) {
-            a.init();
-        }
-    }
-
-    @Override
-    public boolean loop(TelemetryPacket p) {
-        List<Action> newRem = new ArrayList<>();
-        for (Action a : remaining) {
-            if (a.loop(p)) {
-                newRem.add(a);
-            }
-        }
-
-        remaining = newRem;
-
-        return remaining.size() > 0;
-    }
-
-    @Override
-    public void draw(Canvas c) {
-        for (Action a : actions) {
-            a.draw(c);
-        }
-    }
-}
-
-final class SequentialAction implements Action {
-    public final List<Action> actions;
-    private int index;
-    private boolean needsInit;
-
-    public SequentialAction(List<Action> actions) {
-        this.actions = Collections.unmodifiableList(actions);
-    }
-
-    public SequentialAction(Action... actions) {
-        this(Arrays.asList(actions));
-    }
-
-    @Override
-    public void init() {
-        if (actions.isEmpty()) {
-            return;
-        }
-
-        actions.get(0).init();
-    }
-
-    @Override
-    public boolean loop(TelemetryPacket p) {
-        if (index >= actions.size()) {
-            return false;
-        }
-
-        Action a = actions.get(index);
-
-        if (needsInit) {
-            a.init();
-            needsInit = false;
-        }
-
-        if (!a.loop(p)) {
-            index++;
-            needsInit = true;
-        }
-
-        return index < actions.size();
-    }
-
-    @Override
-    public void draw(Canvas c) {
-        for (Action a : actions) {
-            a.draw(c);
-        }
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ActionOpMode.java

@@ -0,0 +1,26 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.Action;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+public abstract class ActionOpMode extends LinearOpMode {
+    private final FtcDashboard dash = FtcDashboard.getInstance();
+
+    protected void runBlocking(Action a) {
+        Canvas c = new Canvas();
+        a.preview(c);
+
+        boolean b = true;
+        while (b && !isStopRequested()) {
+            TelemetryPacket p = new TelemetryPacket();
+            p.fieldOverlay().getOperations().addAll(c.getOperations());
+
+            b = a.run(p);
+
+            dash.sendTelemetryPacket(p);
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java

@@ -1,9 +1,13 @@
 package org.firstinspires.ftc.teamcode;
 
+import androidx.annotation.NonNull;
+
 import com.acmerobotics.dashboard.canvas.Canvas;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
 import com.acmerobotics.roadrunner.AccelConstraint;
+import com.acmerobotics.roadrunner.Action;
+import com.acmerobotics.roadrunner.Actions;
 import com.acmerobotics.roadrunner.AngularVelConstraint;
 import com.acmerobotics.roadrunner.DualNum;
 import com.acmerobotics.roadrunner.HolonomicController;
@@ -13,15 +17,11 @@ import com.acmerobotics.roadrunner.MotorFeedforward;
 import com.acmerobotics.roadrunner.Pose2d;
 import com.acmerobotics.roadrunner.Pose2dDual;
 import com.acmerobotics.roadrunner.ProfileAccelConstraint;
-import com.acmerobotics.roadrunner.Profiles;
-import com.acmerobotics.roadrunner.Rotation2d;
-import com.acmerobotics.roadrunner.Rotation2dDual;
-import com.acmerobotics.roadrunner.SafeTrajectoryBuilder;
 import com.acmerobotics.roadrunner.Time;
-import com.acmerobotics.roadrunner.TimeProfile;
 import com.acmerobotics.roadrunner.TimeTrajectory;
-import com.acmerobotics.roadrunner.Trajectory;
+import com.acmerobotics.roadrunner.TimeTurn;
-import com.acmerobotics.roadrunner.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
+import com.acmerobotics.roadrunner.TurnConstraints;
 import com.acmerobotics.roadrunner.Twist2d;
 import com.acmerobotics.roadrunner.Twist2dDual;
 import com.acmerobotics.roadrunner.Twist2dIncrDual;
@@ -81,6 +81,8 @@ public final class MecanumDrive {
 
     public final MotorFeedforward feedforward = new MotorFeedforward(kS, kV, kA);
 
+    public final TurnConstraints defaultTurnConstraints = new TurnConstraints(
+            MAX_ANG_VEL, -MAX_ANG_ACCEL, MAX_ANG_ACCEL);
     public final VelConstraint defaultVelConstraint =
             new MinVelConstraint(Arrays.asList(
                     kinematics.new WheelVelConstraint(MAX_WHEEL_VEL),
@@ -194,16 +196,13 @@ public final class MecanumDrive {
     }
 
     public final class FollowTrajectoryAction implements Action {
-        public final Trajectory trajectory;
+        public final TimeTrajectory timeTrajectory;
-        private TimeTrajectory timeTrajectory;
+        private double beginTs = -1;
-        private double beginTs;
 
-        private boolean active;
         private final double[] xPoints, yPoints;
 
-        public FollowTrajectoryAction(Trajectory t) {
+        public FollowTrajectoryAction(TimeTrajectory t) {
-            this.trajectory = t;
+            timeTrajectory = t;
-            timeTrajectory = new TimeTrajectory(t);
 
             List<Double> disps = com.acmerobotics.roadrunner.Math.range(
                     0, t.path.length(),
@@ -218,23 +217,21 @@ public final class MecanumDrive {
         }
 
         @Override
-        public void init() {
+        public boolean run(@NonNull TelemetryPacket p) {
-            beginTs = clock();
+            double t;
+            if (beginTs < 0) {
+                beginTs = Actions.now();
+                t = 0;
+            } else {
+                t = Actions.now() - beginTs;
+            }
 
-            active = true;
-        }
-
-        @Override
-        public boolean loop(TelemetryPacket p) {
-            double t = clock() - beginTs;
             if (t >= timeTrajectory.duration) {
                 leftFront.setPower(0);
                 leftBack.setPower(0);
                 rightBack.setPower(0);
                 rightFront.setPower(0);
 
-                active = false;
-
                 return false;
             }
 
@@ -274,66 +271,50 @@ public final class MecanumDrive {
             c.setStroke("#3F51B5");
             drawRobot(c, pose);
 
+            c.setStroke("#4CAF50FF");
+            c.setStrokeWidth(1);
+            c.strokePolyline(xPoints, yPoints);
+
             return true;
         }
 
-        public void cancel() {
-            double t = clock() - beginTs;
-            double s = timeTrajectory.profile.get(t).value();
-            beginTs += t;
-            timeTrajectory = new TimeTrajectory(trajectory.cancel(s));
-        }
-
         @Override
-        public void draw(Canvas c) {
+        public void preview(Canvas c) {
+            c.setStroke("#4CAF507A");
             c.setStrokeWidth(1);
-            c.setStroke(active ? "#4CAF50FF" : "#4CAF507A");
             c.strokePolyline(xPoints, yPoints);
         }
     }
 
     public final class TurnAction implements Action {
-        private final Pose2d beginPose;
+        private final TimeTurn turn;
 
-        private final TimeProfile profile;
+        private double beginTs = -1;
-        private double beginTs;
-        private final boolean reversed;
 
-        private boolean active;
+        public TurnAction(TimeTurn turn) {
-
+            this.turn = turn;
-        public TurnAction(Pose2d beginPose, double angle) {
-            this.beginPose = beginPose;
-            profile = new TimeProfile(Profiles.constantProfile(Math.abs(angle), 0, MAX_ANG_VEL, -MAX_ANG_ACCEL, MAX_ANG_ACCEL).baseProfile);
-            reversed = angle < 0;
         }
 
         @Override
-        public void init() {
+        public boolean run(@NonNull TelemetryPacket p) {
-            beginTs = clock();
+            double t;
+            if (beginTs < 0) {
+                beginTs = Actions.now();
+                t = 0;
+            } else {
+                t = Actions.now() - beginTs;
+            }
 
-            active = true;
+            if (t >= turn.duration) {
-        }
-
-        @Override
-        public boolean loop(TelemetryPacket p) {
-            double t = clock() - beginTs;
-            if (t >= profile.duration) {
                 leftFront.setPower(0);
                 leftBack.setPower(0);
                 rightBack.setPower(0);
                 rightFront.setPower(0);
 
-                active = false;
-
                 return false;
             }
 
-            DualNum<Time> x = profile.get(t);
+            Pose2dDual<Time> txWorldTarget = turn.get(t);
-            if (reversed) {
-                x = x.unaryMinus();
-            }
-
-            Pose2dDual<Time> txWorldTarget = Rotation2dDual.exp(x).times(beginPose);
 
             Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel();
 
@@ -359,13 +340,16 @@ public final class MecanumDrive {
             c.setStroke("#3F51B5");
             drawRobot(c, pose);
 
+            c.setStroke("#7C4DFFFF");
+            c.fillCircle(turn.beginPose.trans.x, turn.beginPose.trans.y, 2);
+
             return true;
         }
 
         @Override
-        public void draw(Canvas c) {
+        public void preview(Canvas c) {
-            c.setFill(active ? "#7C4DFFFF" : "#7C4DFF7A");
+            c.setStroke("#7C4DFF7A");
-            c.fillCircle(beginPose.trans.x, beginPose.trans.y, 2);
+            c.fillCircle(turn.beginPose.trans.x, turn.beginPose.trans.y, 2);
         }
     }
 
@@ -410,73 +394,14 @@ public final class MecanumDrive {
         c.strokeLine(p1.x, p1.y, p2.x, p2.y);
     }
 
-    public TurnAction turn(Pose2d beginPose, double angle) {
+    public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
-        return new TurnAction(beginPose, angle);
+        return new TrajectoryActionBuilder(
-    }
+                TurnAction::new,
-    public TurnAction turnTo(Pose2d beginPose, Rotation2d rot) {
+                FollowTrajectoryAction::new,
-        return new TurnAction(beginPose, rot.minus(beginPose.rot));
+                beginPose, 1e-6,
-    }
+                defaultTurnConstraints,
-    public TurnAction turnTo(Pose2d beginPose, double rot) {
+                defaultVelConstraint, defaultAccelConstraint,
-        return turnTo(beginPose, Rotation2d.exp(rot));
+                0.25
-    }
+        );
-
-    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, Rotation2d beginTangent, TrajectoryBuilder.PoseMap poseMap) {
-        return new SafeTrajectoryBuilder(
-                beginPose, beginTangent, 1e-6,
-                0.0, defaultVelConstraint, defaultAccelConstraint, 0.25, poseMap);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, double beginTangent, TrajectoryBuilder.PoseMap poseMap) {
-        return trajectoryBuilder(beginPose, Rotation2d.exp(beginTangent), poseMap);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, boolean reversed, TrajectoryBuilder.PoseMap poseMap) {
-        return trajectoryBuilder(beginPose, beginPose.rot.plus(reversed ? Math.PI : 0), poseMap);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, TrajectoryBuilder.PoseMap poseMap) {
-        return trajectoryBuilder(beginPose, false, poseMap);
-    }
-
-    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, Rotation2d beginTangent) {
-        return new SafeTrajectoryBuilder(
-                beginPose, beginTangent, 1e-6,
-                0.0, defaultVelConstraint, defaultAccelConstraint, 0.25);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, double beginTangent) {
-        return trajectoryBuilder(beginPose, Rotation2d.exp(beginTangent));
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, boolean reversed) {
-        return trajectoryBuilder(beginPose, beginPose.rot.plus(reversed ? Math.PI : 0));
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose) {
-        return trajectoryBuilder(beginPose, false);
-    }
-
-    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, Rotation2d beginTangent, TrajectoryBuilder.PoseMap poseMap) {
-        return trajectoryBuilder(traj.path.end(1).value(), beginTangent, poseMap);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, double beginTangent, TrajectoryBuilder.PoseMap poseMap) {
-        return trajectoryBuilder(traj.path.end(1).value(), beginTangent, poseMap);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, boolean reversed, TrajectoryBuilder.PoseMap poseMap) {
-        return trajectoryBuilder(traj.path.end(1).value(), reversed, poseMap);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, TrajectoryBuilder.PoseMap poseMap) {
-        return trajectoryBuilder(traj.path.end(1).value(), poseMap);
-    }
-
-    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, Rotation2d beginTangent) {
-        return trajectoryBuilder(traj.path.end(1).value(), beginTangent);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, double beginTangent) {
-        return trajectoryBuilder(traj.path.end(1).value(), beginTangent);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, boolean reversed) {
-        return trajectoryBuilder(traj.path.end(1).value(), reversed);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj) {
-        return trajectoryBuilder(traj.path.end(1).value());
-    }
-
-    public FollowTrajectoryAction followTrajectory(Trajectory trajectory) {
-        return new FollowTrajectoryAction(trajectory);
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java

@@ -1,9 +1,13 @@
 package org.firstinspires.ftc.teamcode;
 
+import androidx.annotation.NonNull;
+
 import com.acmerobotics.dashboard.canvas.Canvas;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
 import com.acmerobotics.roadrunner.AccelConstraint;
+import com.acmerobotics.roadrunner.Action;
+import com.acmerobotics.roadrunner.Actions;
 import com.acmerobotics.roadrunner.AngularVelConstraint;
 import com.acmerobotics.roadrunner.Arclength;
 import com.acmerobotics.roadrunner.DualNum;
@@ -12,17 +16,13 @@ import com.acmerobotics.roadrunner.MotorFeedforward;
 import com.acmerobotics.roadrunner.Pose2d;
 import com.acmerobotics.roadrunner.Pose2dDual;
 import com.acmerobotics.roadrunner.ProfileAccelConstraint;
-import com.acmerobotics.roadrunner.Profiles;
 import com.acmerobotics.roadrunner.RamseteController;
-import com.acmerobotics.roadrunner.Rotation2d;
-import com.acmerobotics.roadrunner.Rotation2dDual;
-import com.acmerobotics.roadrunner.SafeTrajectoryBuilder;
 import com.acmerobotics.roadrunner.TankKinematics;
 import com.acmerobotics.roadrunner.Time;
-import com.acmerobotics.roadrunner.TimeProfile;
 import com.acmerobotics.roadrunner.TimeTrajectory;
-import com.acmerobotics.roadrunner.Trajectory;
+import com.acmerobotics.roadrunner.TimeTurn;
-import com.acmerobotics.roadrunner.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
+import com.acmerobotics.roadrunner.TurnConstraints;
 import com.acmerobotics.roadrunner.Twist2d;
 import com.acmerobotics.roadrunner.Twist2dDual;
 import com.acmerobotics.roadrunner.Twist2dIncrDual;
@@ -81,6 +81,8 @@ public final class TankDrive {
 
     public final MotorFeedforward feedforward = new MotorFeedforward(kS, kV, kA);
 
+    public final TurnConstraints defaultTurnConstraints = new TurnConstraints(
+            MAX_ANG_VEL, -MAX_ANG_ACCEL, MAX_ANG_ACCEL);
     public final VelConstraint defaultVelConstraint =
             new MinVelConstraint(Arrays.asList(
                     kinematics.new WheelVelConstraint(MAX_WHEEL_VEL),
@@ -211,16 +213,13 @@ public final class TankDrive {
     }
 
     public final class FollowTrajectoryAction implements Action {
-        public final Trajectory trajectory;
+        public final TimeTrajectory timeTrajectory;
-        private TimeTrajectory timeTrajectory;
+        private double beginTs = -1;
-        private double beginTs;
 
-        private boolean active;
         private final double[] xPoints, yPoints;
 
-        public FollowTrajectoryAction(Trajectory t) {
+        public FollowTrajectoryAction(TimeTrajectory t) {
-            this.trajectory = t;
+            timeTrajectory = t;
-            timeTrajectory = new TimeTrajectory(t);
 
             List<Double> disps = com.acmerobotics.roadrunner.Math.range(
                     0, t.path.length(),
@@ -235,15 +234,15 @@ public final class TankDrive {
         }
 
         @Override
-        public void init() {
+        public boolean run(@NonNull TelemetryPacket p) {
-            beginTs = clock();
+            double t;
+            if (beginTs < 0) {
+                beginTs = Actions.now();
+                t = 0;
+            } else {
+                t = Actions.now() - beginTs;
+            }
 
-            active = true;
-        }
-
-        @Override
-        public boolean loop(TelemetryPacket p) {
-            double t = clock() - beginTs;
             if (t >= timeTrajectory.duration) {
                 for (DcMotorEx m : leftMotors) {
                     m.setPower(0);
@@ -252,8 +251,6 @@ public final class TankDrive {
                     m.setPower(0);
                 }
 
-                active = false;
-
                 return false;
             }
 
@@ -294,50 +291,41 @@ public final class TankDrive {
             c.setStroke("#3F51B5");
             drawRobot(c, pose);
 
+            c.setStroke("#4CAF50FF");
+            c.setStrokeWidth(1);
+            c.strokePolyline(xPoints, yPoints);
+
             return true;
         }
 
-        public void cancel() {
-            double t = clock() - beginTs;
-            double s = timeTrajectory.profile.get(t).value();
-            beginTs += t;
-            timeTrajectory = new TimeTrajectory(trajectory.cancel(s));
-        }
-
         @Override
-        public void draw(Canvas c) {
+        public void preview(Canvas c) {
+            c.setStroke("#4CAF507A");
             c.setStrokeWidth(1);
-            c.setStroke(active ? "#4CAF50FF" : "#4CAF507A");
             c.strokePolyline(xPoints, yPoints);
         }
     }
 
     public final class TurnAction implements Action {
-        private final Pose2d beginPose;
+        private final TimeTurn turn;
 
-        private final TimeProfile profile;
+        private double beginTs = -1;
-        private double beginTs;
-        private final boolean reversed;
 
-        private boolean active;
+        public TurnAction(TimeTurn turn) {
-
+            this.turn = turn;
-        public TurnAction(Pose2d beginPose, double angle) {
-            this.beginPose = beginPose;
-            profile = new TimeProfile(Profiles.constantProfile(Math.abs(angle), 0, MAX_ANG_VEL, -MAX_ANG_ACCEL, MAX_ANG_ACCEL).baseProfile);
-            reversed = angle < 0;
         }
 
         @Override
-        public void init() {
+        public boolean run(@NonNull TelemetryPacket p) {
-            beginTs = clock();
+            double t;
+            if (beginTs < 0) {
+                beginTs = Actions.now();
+                t = 0;
+            } else {
+                t = Actions.now() - beginTs;
+            }
 
-            active = true;
+            if (t >= turn.duration) {
-        }
-
-        @Override
-        public boolean loop(TelemetryPacket p) {
-            double t = clock() - beginTs;
-            if (t >= profile.duration) {
                 for (DcMotorEx m : leftMotors) {
                     m.setPower(0);
                 }
@@ -345,24 +333,18 @@ public final class TankDrive {
                     m.setPower(0);
                 }
 
-                active = false;
-
                 return false;
             }
 
-            DualNum<Time> x = profile.get(t);
+            Pose2dDual<Time> txWorldTarget = turn.get(t);
-            if (reversed) {
-                x = x.unaryMinus();
-            }
 
             Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel();
 
-            Rotation2dDual<Time> target = Rotation2dDual.<Time>constant(beginPose.rot, 3).plus(x);
             Twist2dDual<Time> command = new Twist2dDual<>(
                     Vector2dDual.constant(new Vector2d(0, 0), 3),
-                    target.velocity().plus(
+                    txWorldTarget.rot.velocity().plus(
-                            TURN_GAIN * pose.rot.minus(target.value()) +
+                            TURN_GAIN * pose.rot.minus(txWorldTarget.rot.value()) +
-                            TURN_VEL_GAIN * (robotVelRobot.rotVel - target.velocity().value())
+                            TURN_VEL_GAIN * (robotVelRobot.rotVel - txWorldTarget.rot.velocity().value())
                     )
             );
 
@@ -379,18 +361,21 @@ public final class TankDrive {
             drawPoseHistory(c);
 
             c.setStroke("#4CAF50");
-            drawRobot(c, new Pose2d(beginPose.trans, beginPose.rot.plus(x.value())));
+            drawRobot(c, txWorldTarget.value());
 
             c.setStroke("#3F51B5");
             drawRobot(c, pose);
 
+            c.setStroke("#7C4DFFFF");
+            c.fillCircle(turn.beginPose.trans.x, turn.beginPose.trans.y, 2);
+
             return true;
         }
 
         @Override
-        public void draw(Canvas c) {
+        public void preview(Canvas c) {
-            c.setFill(active ? "#7C4DFFFF" : "#7C4DFF7A");
+            c.setStroke("#7C4DFF7A");
-            c.fillCircle(beginPose.trans.x, beginPose.trans.y, 2);
+            c.fillCircle(turn.beginPose.trans.x, turn.beginPose.trans.y, 2);
         }
     }
 
@@ -435,73 +420,14 @@ public final class TankDrive {
         c.strokeLine(p1.x, p1.y, p2.x, p2.y);
     }
 
-    public TurnAction turn(Pose2d beginPose, double angle) {
+    public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
-        return new TurnAction(beginPose, angle);
+        return new TrajectoryActionBuilder(
-    }
+                TurnAction::new,
-    public TurnAction turnTo(Pose2d beginPose, Rotation2d rot) {
+                FollowTrajectoryAction::new,
-        return new TurnAction(beginPose, rot.minus(beginPose.rot));
+                beginPose, 1e-6,
-    }
+                defaultTurnConstraints,
-    public TurnAction turnTo(Pose2d beginPose, double rot) {
+                defaultVelConstraint, defaultAccelConstraint,
-        return turnTo(beginPose, Rotation2d.exp(rot));
+                0.25
-    }
+        );
-
-    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, Rotation2d beginTangent, TrajectoryBuilder.PoseMap poseMap) {
-        return new SafeTrajectoryBuilder(
-                beginPose, beginTangent, 1e-6,
-                0.0, defaultVelConstraint, defaultAccelConstraint, 0.25, poseMap);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, double beginTangent, TrajectoryBuilder.PoseMap poseMap) {
-        return trajectoryBuilder(beginPose, Rotation2d.exp(beginTangent), poseMap);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, boolean reversed, TrajectoryBuilder.PoseMap poseMap) {
-        return trajectoryBuilder(beginPose, beginPose.rot.plus(reversed ? Math.PI : 0), poseMap);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, TrajectoryBuilder.PoseMap poseMap) {
-        return trajectoryBuilder(beginPose, false, poseMap);
-    }
-
-    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, Rotation2d beginTangent) {
-        return new SafeTrajectoryBuilder(
-                beginPose, beginTangent, 1e-6,
-                0.0, defaultVelConstraint, defaultAccelConstraint, 0.25);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, double beginTangent) {
-        return trajectoryBuilder(beginPose, Rotation2d.exp(beginTangent));
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, boolean reversed) {
-        return trajectoryBuilder(beginPose, beginPose.rot.plus(reversed ? Math.PI : 0));
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose) {
-        return trajectoryBuilder(beginPose, false);
-    }
-
-    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, Rotation2d beginTangent, TrajectoryBuilder.PoseMap poseMap) {
-        return trajectoryBuilder(traj.path.end(1).value(), beginTangent, poseMap);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, double beginTangent, TrajectoryBuilder.PoseMap poseMap) {
-        return trajectoryBuilder(traj.path.end(1).value(), beginTangent, poseMap);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, boolean reversed, TrajectoryBuilder.PoseMap poseMap) {
-        return trajectoryBuilder(traj.path.end(1).value(), reversed, poseMap);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, TrajectoryBuilder.PoseMap poseMap) {
-        return trajectoryBuilder(traj.path.end(1).value(), poseMap);
-    }
-
-    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, Rotation2d beginTangent) {
-        return trajectoryBuilder(traj.path.end(1).value(), beginTangent);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, double beginTangent) {
-        return trajectoryBuilder(traj.path.end(1).value(), beginTangent);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, boolean reversed) {
-        return trajectoryBuilder(traj.path.end(1).value(), reversed);
-    }
-    public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj) {
-        return trajectoryBuilder(traj.path.end(1).value());
-    }
-
-    public TankDrive.FollowTrajectoryAction followTrajectory(Trajectory trajectory) {
-        return new TankDrive.FollowTrajectoryAction(trajectory);
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java

@@ -1,13 +1,12 @@
 package org.firstinspires.ftc.teamcode.tuning;
 
-import com.acmerobotics.roadrunner.Trajectory;
 import com.acmerobotics.roadrunner.Pose2d;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
+import org.firstinspires.ftc.teamcode.ActionOpMode;
 import org.firstinspires.ftc.teamcode.MecanumDrive;
 import org.firstinspires.ftc.teamcode.TankDrive;
 
-public final class ManualFeedbackTuner extends LinearOpMode {
+public final class ManualFeedbackTuner extends ActionOpMode {
     public static double DISTANCE = 64;
 
     @Override
@@ -15,36 +14,26 @@ public final class ManualFeedbackTuner extends LinearOpMode {
         if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
             MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
 
-            Trajectory forward = drive.trajectoryBuilder(drive.pose)
-                    .forward(DISTANCE)
-                    .build();
-
-            Trajectory backward = drive.trajectoryBuilder(forward, true)
-                    .forward(-DISTANCE)
-                    .build();
-
             waitForStart();
 
             while (opModeIsActive()) {
-                drive.followTrajectory(forward).runBlocking();
+                runBlocking(
-                drive.followTrajectory(backward).runBlocking();
+                    drive.actionBuilder(drive.pose)
+                            .forward(DISTANCE)
+                            .forward(-DISTANCE)
+                            .build());
             }
         } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
             TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
 
-            Trajectory forward = drive.trajectoryBuilder(drive.pose)
-                    .forward(DISTANCE)
-                    .build();
-
-            Trajectory backward = drive.trajectoryBuilder(forward, true)
-                    .forward(-DISTANCE)
-                    .build();
-
             waitForStart();
 
             while (opModeIsActive()) {
-                drive.followTrajectory(forward).runBlocking();
+                runBlocking(
-                drive.followTrajectory(backward).runBlocking();
+                    drive.actionBuilder(drive.pose)
+                            .forward(DISTANCE)
+                            .forward(-DISTANCE)
+                            .build());
             }
         } else {
             throw new AssertionError();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java

@@ -1,13 +1,13 @@
 package org.firstinspires.ftc.teamcode.tuning;
 
-import com.acmerobotics.roadrunner.Vector2d;
 import com.acmerobotics.roadrunner.Pose2d;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.acmerobotics.roadrunner.Vector2d;
 
+import org.firstinspires.ftc.teamcode.ActionOpMode;
 import org.firstinspires.ftc.teamcode.MecanumDrive;
 import org.firstinspires.ftc.teamcode.TankDrive;
 
-public final class SplineTest extends LinearOpMode {
+public final class SplineTest extends ActionOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
         if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
@@ -15,23 +15,21 @@ public final class SplineTest extends LinearOpMode {
 
             waitForStart();
 
-            drive.followTrajectory(
+            runBlocking(
-                    drive.trajectoryBuilder(drive.pose)
+                drive.actionBuilder(drive.pose)
-                            .splineTo(new Vector2d(30, 30), Math.PI / 2)
+                        .splineTo(new Vector2d(30, 30), Math.PI / 2)
-                            .splineTo(new Vector2d(60, 0), Math.PI)
+                        .splineTo(new Vector2d(60, 0), Math.PI)
-                            .build()
+                        .build());
-            ).runBlocking();
         } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
             TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
 
             waitForStart();
 
-            drive.followTrajectory(
+            runBlocking(
-                    drive.trajectoryBuilder(drive.pose)
+                    drive.actionBuilder(drive.pose)
                             .splineTo(new Vector2d(30, 30), Math.PI / 2)
                             .splineTo(new Vector2d(60, 0), Math.PI)
-                            .build()
+                            .build());
-            ).runBlocking();
         } else {
             throw new AssertionError();
         }