Big update part 2:

 - Shiny new Angle class to solve all your angular problems
 - Servo speed can now be changed
 - All DriveComponents now have an exposed motor group for easy access to motor properties
 - Differential swerve drive support out-of-the-box!
 - DashboardUtil has been removed and merged into SuperTelemetry
 - SuperTelemetry now graphs items on their own line in FTC Dashboard
 - Motor now has 2 new properties: rotation and maxDistanceVelocity
 - RobotOpMode now has preInit and preStart instead of just init so you can actually schedule commands to run in the run loop
 - Did someone say DRAGGABLE TANGENTS AND HEADING? Cause the GUI has it now
 - CommandScheduler.reset() should now be thread-safe
 - Minor logo changes

README.md

@@ -24,6 +24,6 @@ repositories {
   maven { url 'https://jitpack.io' }
 }
 dependencies {
-  implementation "com.github.amarcolini.joos:$module:0.2.2-alpha"
+  implementation "com.github.amarcolini.joos:$module:0.4.0-alpha"
 }
-```
+```

build.gradle

@@ -1,7 +1,7 @@
 buildscript {
     ext {
         kotlin_version = "1.6.0"
-        lib_version = "v0.2.2-alpha"
+        lib_version = "v0.4.0-alpha"
     }
 
     repositories {

command/src/main/kotlin/com/amarcolini/joos/command/CommandScheduler.kt

@@ -28,9 +28,11 @@ open class CommandScheduler {
 
     private val conditions = LinkedHashMap<BooleanSupplier, MutableSet<Command>>()
 
-    private val scheduleCache = LinkedHashSet<Command>()
-
-    private val cancelCache = HashSet<Command>()
+    /**
+     * A list storing all actions that couldn't be completed at the time they were called because
+     * the scheduler was busy.
+     */
+    private val actionCache = LinkedHashSet<() -> Boolean>()
 
     /**
      * Command scheduling policy. If true, all commands which cannot currently be scheduled will be scheduled as soon as
@@ -57,14 +59,14 @@ open class CommandScheduler {
      *
      * @return `true` if all commands were successfully scheduled immediately, and `false` if they were not. Note that if
      * the scheduler is currently updating, this method will return `false`, but the scheduler will attempt to
-     * schedule them when it can. If [schedulePolicy] is `true`, all commands will be successfully scheduled.
+     * schedule the commands when it can. If [schedulePolicy] is `true`, all commands will be successfully scheduled.
      *
      * @see schedulePolicy
      */
     fun schedule(vararg commands: Command): Boolean {
         var success = true
         if (isBusy) {
-            scheduleCache += commands
+            actionCache += { schedule(*commands) || schedulePolicy }
             return false
         }
         for (command in commands) {
@@ -82,12 +84,19 @@ open class CommandScheduler {
         return success
     }
 
+    /**
+     * Schedules commands for execution.
+     *
+     * @return `true` if all commands were successfully scheduled immediately, and `false` if they were not. Note that if
+     * the scheduler is currently updating, this method will return `false`, but the scheduler will attempt to
+     * schedule the commands when it can. If [schedulePolicy] is `true`, all commands will be successfully scheduled.
+     *
+     * @see schedulePolicy
+     */
+    fun schedule(vararg runnables: Runnable): Boolean = schedule(*runnables.map { BasicCommand(it) }.toTypedArray())
+
     private var isBusy = false
     fun update() {
-        //Schedules all commands which were unable to be scheduled previously.
-        scheduleCache.removeIf {
-            schedule(it) || !schedulePolicy
-        }
         //Updates all registered components
         components.forEach { it.update() }
 
@@ -109,9 +118,6 @@ open class CommandScheduler {
         }
         isBusy = false
 
-        //Cancels all commands which were attempted to be cancelled while the scheduler was busy.
-        cancel(*cancelCache.toTypedArray())
-
         //Schedules the necessary commands mapped to a condition
         conditions.filterKeys { it.asBoolean }.values.forEach { commands ->
             commands.forEach { schedule(it) }
@@ -125,6 +131,9 @@ open class CommandScheduler {
         }
 
         telemetry.update()
+
+        //Updates action cache
+        actionCache.removeIf { it() }
     }
 
     /**
@@ -158,7 +167,10 @@ open class CommandScheduler {
      */
     fun cancel(vararg commands: Command) {
         if (isBusy) {
-            cancelCache += commands
+            actionCache += {
+                cancel(*commands)
+                true
+            }
             return
         }
         for (command in commands) {
@@ -204,20 +216,23 @@ open class CommandScheduler {
     /**
      * Cancels all currently scheduled commands. Ignores whether a command is interruptable.
      */
-    fun cancelAll() {
-        scheduledCommands.forEach { cancel(it) }
-    }
+    fun cancelAll() = cancel(*scheduledCommands.toTypedArray())
 
     /**
      * Resets this [CommandScheduler]. The telemetry is reset, all commands are cancelled, and all commands, components, and conditions are cleared.
      */
     fun reset() {
-        cancelAll()
-        scheduledCommands.clear()
-        components.clear()
-        requirements.clear()
-        conditions.clear()
-        resetTelemetry()
+        val reset = {
+            cancelAll()
+            scheduledCommands.clear()
+            components.clear()
+            requirements.clear()
+            conditions.clear()
+            resetTelemetry()
+            true
+        }
+        if (isBusy) actionCache += reset
+        else reset()
     }
 
     /**

command/src/main/kotlin/com/amarcolini/joos/command/RobotOpMode.kt

@@ -22,11 +22,31 @@ abstract class RobotOpMode<T : Robot> : OpMode() {
     @JvmField
     protected val isAutonomous = this::class.hasAnnotation<Autonomous>()
 
+    /**
+     * The singleton [Robot] instance of this OpMode.
+     */
     protected lateinit var robot: T
         private set
 
+    /**
+     * This method is called on initialization. Any commands scheduled here will be
+     * run in the init loop. [initialize] **must** be called here.
+     */
+    abstract fun preInit()
+
+    /**
+     * This method is called on start. Any commands scheduled here will be run for the
+     * duration of the OpMode.
+     */
+    abstract fun preStart()
+
+    final override fun init() = preInit()
     final override fun init_loop() = robot.update()
-    final override fun start() = robot.start()
+    final override fun start() {
+        robot.start()
+        preStart()
+    }
+
     final override fun loop() = robot.update()
     override fun stop() = robot.reset()
 

command/src/main/kotlin/com/amarcolini/joos/command/SuperTelemetry.kt

@@ -3,8 +3,18 @@ package com.amarcolini.joos.command
 import com.acmerobotics.dashboard.FtcDashboard
 import com.acmerobotics.dashboard.canvas.Canvas
 import com.acmerobotics.dashboard.telemetry.TelemetryPacket
+import com.amarcolini.joos.geometry.Pose2d
+import com.amarcolini.joos.path.Path
+import com.amarcolini.joos.trajectory.PathTrajectorySegment
+import com.amarcolini.joos.trajectory.Trajectory
+import com.amarcolini.joos.trajectory.TurnSegment
+import com.amarcolini.joos.trajectory.WaitSegment
 import org.firstinspires.ftc.robotcore.external.Telemetry
+import kotlin.math.ceil
 
+/**
+ * A powerful telemetry for both the Driver Station and [FTC Dashboard](https://github.com/acmerobotics/ftc-dashboard).
+ */
 class SuperTelemetry {
     private var packet: TelemetryPacket = TelemetryPacket()
     val lines: MutableList<Lineable> = ArrayList()
@@ -156,7 +166,7 @@ class SuperTelemetry {
 
             }
         }
-        FtcDashboard.getInstance().sendTelemetryPacket(packet)
+        FtcDashboard.getInstance()?.sendTelemetryPacket(packet)
         packet = TelemetryPacket()
         telemetries.forEach { it.update() }
         if (isAutoClear) clear()
@@ -174,4 +184,107 @@ class SuperTelemetry {
     fun setDisplayFormat(displayFormat: Telemetry.DisplayFormat?) {
         telemetries.forEach { it.setDisplayFormat(displayFormat) }
     }
+
+    /**
+     * The radius used by [drawRobot].
+     */
+    val robotRadius = 9.0
+
+    /**
+     * The resolution used by [drawSampledPath] and [drawSampledTrajectory] when sampling.
+     */
+    val resolution = 2.0
+
+    /**
+     * Draws a list of poses on [FTC Dashboard](https://github.com/acmerobotics/ftc-dashboard).
+     */
+    fun drawPoseHistory(
+        poseHistory: List<Pose2d>,
+        color: String
+    ) {
+        val canvas = fieldOverlay()
+        val xPoints = DoubleArray(poseHistory.size)
+        val yPoints = DoubleArray(poseHistory.size)
+        for (i in poseHistory.indices) {
+            val pose = poseHistory[i]
+            xPoints[i] = pose.x
+            yPoints[i] = pose.y
+        }
+        canvas.setStroke(color)
+        canvas.strokePolyline(xPoints, yPoints)
+    }
+
+    /**
+     * Draws a robot on [FTC Dashboard](https://github.com/acmerobotics/ftc-dashboard).
+     */
+    fun drawRobot(
+        pose: Pose2d,
+        color: String
+    ) {
+        val canvas = fieldOverlay()
+        canvas.setStroke(color)
+        canvas.strokeCircle(pose.x, pose.y, robotRadius)
+        val (x, y) = pose.headingVec() * robotRadius
+        val x1: Double = pose.x + x / 2
+        val y1: Double = pose.y + y / 2
+        val x2: Double = pose.x + x
+        val y2: Double = pose.y + y
+        canvas.strokeLine(x1, y1, x2, y2)
+    }
+
+    /**
+     * Draws a path on [FTC Dashboard](https://github.com/acmerobotics/ftc-dashboard).
+     */
+    fun drawSampledPath(
+        path: Path,
+        color: String,
+        resolution: Double = this.resolution
+    ) {
+        val canvas = fieldOverlay()
+        val samples = ceil(path.length() / resolution).toInt()
+        val xPoints = DoubleArray(samples)
+        val yPoints = DoubleArray(samples)
+        val dx: Double = path.length() / (samples - 1)
+        for (i in 0 until samples) {
+            val displacement = i * dx
+            val (x, y, _) = path[displacement]
+            xPoints[i] = x
+            yPoints[i] = y
+        }
+        canvas.setStroke(color)
+        canvas.strokePolyline(xPoints, yPoints)
+    }
+
+    /**
+     * Draws a trajectory on [FTC Dashboard](https://github.com/acmerobotics/ftc-dashboard).
+     */
+    @JvmOverloads
+    fun drawSampledTrajectory(
+        trajectory: Trajectory,
+        pathColor: String = "#4CAF50",
+        turnColor: String = "#7c4dff",
+        waitColor: String = "#dd2c00",
+        resolution: Double = this.resolution
+    ) {
+        val canvas = fieldOverlay()
+        trajectory.segments.forEach {
+            when (it) {
+                is PathTrajectorySegment -> {
+                    canvas.setStrokeWidth(1)
+                    drawSampledPath(it.path, pathColor, resolution)
+                }
+                is TurnSegment -> {
+                    val pose = it.start()
+                    canvas.setFill(turnColor)
+                    canvas.fillCircle(pose.x, pose.y, 2.0)
+                }
+                is WaitSegment -> {
+                    val pose = it.start()
+                    canvas.setStrokeWidth(1)
+                    canvas.setStroke(waitColor)
+                    canvas.strokeCircle(pose.x, pose.y, 3.0)
+                }
+            }
+        }
+    }
 }