Merge pull request #2 from FFFabulousRobotics/task-concept

Task system update

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<Integer, Task> tasks;
-    Map<Integer, TaskStatus> taskStatuses;
-    int minimumFreeTaskId = 0;
-    Gamepad gamepad1Snapshot;
-    Gamepad gamepad2Snapshot;
+    protected Map<Integer, Task> tasks;
+    protected Map<Integer, Integer> 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<Integer, Task> taskEntry: tasks.entrySet()) {
+            for (Map.Entry<Integer, Task> 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<Integer> 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;
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TimedServo.java → 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;

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;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SleepTask.java → 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;

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() {
+    }
+}