From d0b06e2d5924b485043f338136d40547040f9541 Mon Sep 17 00:00:00 2001 From: Ryan Brott Date: Sun, 1 Jan 2023 22:40:18 -0800 Subject: [PATCH] Update for new actions --- .../firstinspires/ftc/teamcode/Action.java | 261 ------------------ .../ftc/teamcode/ActionOpMode.java | 26 ++ .../ftc/teamcode/MecanumDrive.java | 187 ++++--------- .../firstinspires/ftc/teamcode/TankDrive.java | 198 +++++-------- .../teamcode/tuning/ManualFeedbackTuner.java | 35 +-- .../ftc/teamcode/tuning/SplineTest.java | 24 +- 6 files changed, 167 insertions(+), 564 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Action.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ActionOpMode.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Action.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Action.java deleted file mode 100644 index 47fadf5..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Action.java +++ /dev/null @@ -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 add(Action a); - } - - // parent-less, generic-less SeqBuilder - class Builder implements IBuilder { - private final List as = new ArrayList<>(); - - @Override - public Builder add(Action a) { - as.add(a); - return this; - } - - public SeqBuilder seq() { - return new SeqBuilder<>(this); - } - - public ParBuilder 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> implements IBuilder> { - private final T parent; - private final List as = new ArrayList<>(); - - private SeqBuilder(T parent) { - this.parent = parent; - } - - @Override - public SeqBuilder add(Action a) { - as.add(a); - return this; - } - - public T end() { - parent.add(new SequentialAction(as)); - return parent; - } - - public SeqBuilder> seq() { - return new SeqBuilder<>(this); - } - - public ParBuilder> par() { - return new ParBuilder<>(this); - } - - public SeqBuilder sleep(double duration) { - return add(new SleepAction(duration)); - } - - public SeqBuilder after(double duration, Action a) { - return seq().sleep(duration).add(a).end(); - } - } - - final class ParBuilder> implements IBuilder> { - private final T parent; - private final List as = new ArrayList<>(); - - private ParBuilder(T parent) { - this.parent = parent; - } - - @Override - public ParBuilder add(Action a) { - as.add(a); - return this; - } - - public T end() { - parent.add(new ParallelAction(as)); - return parent; - } - - public SeqBuilder> seq() { - return new SeqBuilder<>(this); - } - - public ParBuilder> par() { - return new ParBuilder<>(this); - } - - public ParBuilder sleep(double duration) { - return add(new SleepAction(duration)); - } - - public ParBuilder 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 actions; - private List remaining; - - public ParallelAction(List 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 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 actions; - private int index; - private boolean needsInit; - - public SequentialAction(List 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); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ActionOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ActionOpMode.java new file mode 100644 index 0000000..6142bd9 --- /dev/null +++ b/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); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index 4354cb3..9f31b9e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/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.TrajectoryBuilder; +import com.acmerobotics.roadrunner.TimeTurn; +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; - private TimeTrajectory timeTrajectory; - private double beginTs; + public final TimeTrajectory timeTrajectory; + private double beginTs = -1; - private boolean active; private final double[] xPoints, yPoints; - public FollowTrajectoryAction(Trajectory t) { - this.trajectory = t; - timeTrajectory = new TimeTrajectory(t); + public FollowTrajectoryAction(TimeTrajectory t) { + timeTrajectory = t; List disps = com.acmerobotics.roadrunner.Math.range( 0, t.path.length(), @@ -218,23 +217,21 @@ public final class MecanumDrive { } @Override - public void init() { - beginTs = clock(); + public boolean run(@NonNull TelemetryPacket p) { + 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; - private final boolean reversed; + private double beginTs = -1; - private boolean active; - - 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; + public TurnAction(TimeTurn turn) { + this.turn = turn; } @Override - public void init() { - beginTs = clock(); + public boolean run(@NonNull TelemetryPacket p) { + 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 >= profile.duration) { + if (t >= turn.duration) { leftFront.setPower(0); leftBack.setPower(0); rightBack.setPower(0); rightFront.setPower(0); - active = false; - return false; } - DualNum