diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OneTimeTask.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OneTimeTask.java deleted file mode 100644 index d29080e9ea01..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OneTimeTask.java +++ /dev/null @@ -1,11 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -public abstract class OneTimeTask implements Task { - @Override - public void iterate() {} - - @Override - public boolean hasNext() { - return false; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Task.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Task.java deleted file mode 100644 index a0bf6558a250..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Task.java +++ /dev/null @@ -1,18 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -public interface Task { - int TICK_MS = 50; - - default void init() { - } - - void iterate(); - - default void finish() { - } - - boolean hasNext(); - - default void cancel() { - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java index 35387ecee3a8..b154f72bb809 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java @@ -3,21 +3,21 @@ import android.util.ArrayMap; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.Gamepad; +import org.firstinspires.ftc.teamcode.task.Task; + +import java.util.List; import java.util.Map; +import java.util.stream.Collectors; public abstract class TaskOpMode extends LinearOpMode { - Map tasks; - Map taskStatuses; - int minimumFreeTaskId = 0; - Gamepad gamepad1Snapshot; - Gamepad gamepad2Snapshot; + protected Map tasks; + protected Map taskLinks; public TaskOpMode() { tasks = new ArrayMap<>(); - taskStatuses = new ArrayMap<>(); + taskLinks = new ArrayMap<>(); } public abstract void linearInit(); @@ -36,20 +36,17 @@ public void runOpMode() { waitForStart(); linearStart(); while (opModeIsActive()) { - for (Map.Entry taskEntry: tasks.entrySet()) { + for (Map.Entry taskEntry : tasks.entrySet()) { int taskId = taskEntry.getKey(); Task task = taskEntry.getValue(); if (!task.hasNext()) { task.finish(); tasks.remove(taskId, task); - taskStatuses.remove(taskId); + taskLinks.remove(taskId); continue; } - TaskStatus taskStatus = taskStatuses.get(taskId); - assert taskStatus != null; - if (tasks.containsKey(taskStatus.afterTaskId)) continue; - boolean shouldRunTask = taskStatus.tickDown(); - if (!shouldRunTask) continue; + Integer linkedTaskId = taskLinks.get(taskId); + if (tasks.containsKey(linkedTaskId)) continue; task.iterate(); } linearLoop(); @@ -58,38 +55,50 @@ public void runOpMode() { linearStop(); } - public int scheduleTask(Task task) { - return scheduleTask(task, 0); - } - - public int scheduleTask(Task task, int delayTicks) { - return scheduleTask(task, delayTicks, 1); + public int registerTask(Task task) { + return registerTask(task, -1); } - public int scheduleTask(Task task, int delayTicks, int loopTicks) { - return scheduleTask(task, delayTicks, loopTicks, -1); - } - - public int scheduleTask(Task task, int delayTicks, int loopTicks, int afterTaskId) { + /** + * Register a task in the task map. All the tasks in the map will be iterated in + * the main loop when it's the right time to do. + * + * @param task the task that need registering. + * @param linkedTaskId the taskId of the linked task. The task being registered will + * be run after the linked task is finished. If nothing provided, + * the registered task will start to run immediately. + * @return the taskId of the registered task. + */ + public int registerTask(Task task, int linkedTaskId) { task.init(); - int taskId = minimumFreeTaskId; - TaskStatus taskStatus = new TaskStatus(delayTicks, loopTicks, afterTaskId); + int taskId = findMinFreeTaskId(); tasks.put(taskId, task); - taskStatuses.put(taskId, taskStatus); - minimumFreeTaskId++; + taskLinks.put(taskId, linkedTaskId); return taskId; } - public void cancelTask(int taskId, boolean chained) { + /** + * Cancel the task and all the linked-after tasks if necessary. + * + * @param taskId the taskId of the task needs canceling. + */ + public void cancelTask(int taskId) { Task task = tasks.get(taskId); if (task == null) return; task.cancel(); tasks.remove(taskId, task); - taskStatuses.remove(taskId); - if (chained) { - taskStatuses.entrySet().stream() - .filter(e -> e.getValue().afterTaskId == taskId) - .forEach(e -> cancelTask(e.getKey(), true)); + taskLinks.remove(taskId); + taskLinks.entrySet().stream() + .filter(e -> e.getValue() == taskId) + .forEach(e -> cancelTask(e.getKey())); + } + + protected int findMinFreeTaskId() { + List taskIdList = tasks.keySet().stream().sorted().collect(Collectors.toList()); + int maxTaskId = taskIdList.get(taskIdList.size() - 1); + for (int i = 0; i <= maxTaskId; i++) { + if (taskIdList.contains(i)) return 1; } + return maxTaskId + 1; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskStatus.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskStatus.java deleted file mode 100644 index 62c65d834b19..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskStatus.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -public class TaskStatus { - int delayTicks; - int loopTicks; - int remainingLoopTicks; - int afterTaskId; - - boolean idling = true; - - public TaskStatus(int delayTicks, int loopTicks, int afterTaskId) { - this.delayTicks = delayTicks; - this.loopTicks = loopTicks; - this.afterTaskId = afterTaskId; - this.remainingLoopTicks = loopTicks; - } - - public boolean tickDown() { - if (idling) { - delayTicks--; - if (delayTicks == 0) { - idling = false; - return true; - } - } else { - remainingLoopTicks--; - if (remainingLoopTicks == 0) { - remainingLoopTicks = loopTicks; - return true; - } - } - return false; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TimedServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/TimedServo.java similarity index 87% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TimedServo.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/TimedServo.java index 9ba3f3c488ae..f5f6a641ecc7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TimedServo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/TimedServo.java @@ -1,10 +1,12 @@ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.hardware; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.ServoController; +import org.firstinspires.ftc.teamcode.task.Task; + public class TimedServo { - Servo servo; + protected Servo servo; public TimedServo(Servo servo) { this.servo = servo; @@ -87,6 +89,15 @@ public void scaleRange(double min, double max) { servo.scaleRange(min, max); } + /** + * Sets the current position of the servo in a fixed time, expressed as a fraction of + * its available range. If PWM power is enabled for the servo, the servo will attempt + * to move to the indicated position. + * + * @param position the position to which the servo should move, a value in the range [0.0, 1.0] + * @param timeMs the time that the servo will take to move. + * @return a Task object + */ public Task setPosition(double position, long timeMs) { final double delta = position - servo.getPosition(); final long iterationCount = timeMs / Task.TICK_MS; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/OneTimeTask.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/OneTimeTask.java new file mode 100644 index 000000000000..8a1fd39bb8da --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/OneTimeTask.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.task; + +public interface OneTimeTask extends Task { + + void init(); + + @Override + default void iterate() {} + + @Override + default boolean hasNext() { + return false; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SleepTask.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/SleepTask.java similarity index 78% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SleepTask.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/SleepTask.java index 1c92fccf8a5f..87ed2726201d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SleepTask.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/SleepTask.java @@ -1,7 +1,7 @@ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.task; public class SleepTask implements Task { - int remainingSleepTicks; + protected int remainingSleepTicks; public SleepTask(int sleepTicks) { remainingSleepTicks = sleepTicks; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/Task.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/Task.java new file mode 100644 index 000000000000..2ced8b70b134 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/Task.java @@ -0,0 +1,35 @@ +package org.firstinspires.ftc.teamcode.task; + +public interface Task { + int TICK_MS = 50; + + /** + * This method is called in the main loop of OpMode + * It should contain the code of EACH TICK, instead of a loop in it! + */ + void iterate(); + + /** + * To judge whether the task needs to continue. + * @return Return true when it need to continue, otherwise false. + */ + boolean hasNext(); + + /** + * This method is called when the task is registered in the TaskOpMode. + */ + default void init() { + } + + /** + * The method is called when the task is finished. + */ + default void finish() { + } + + /** + * The method is called when the task is canceled. + */ + default void cancel() { + } +}