Experimental Coroutines

build.gradle

@@ -21,6 +21,7 @@ repositories {
 dependencies {
     api 'org.jetbrains.kotlin:kotlin-stdlib:1.3.0'
     api 'org.jetbrains.kotlin:kotlin-reflect:1.3.0'
+    api 'org.jetbrains.kotlinx:kotlinx-coroutines-core:1.0.0'
 
     compile wpi.deps.wpilib()
     compile wpi.deps.vendor.java()

src/main/kotlin/org/team5499/monkeyLib/util/coroutines/CoroutineUtils.kt

@@ -0,0 +1,111 @@
+package org.team5499.monkeyLib.util.coroutines
+
+import edu.wpi.first.wpilibj.DriverStation
+import edu.wpi.first.wpilibj.Watchdog
+@Suppress("WildcardImport")
+import kotlinx.coroutines.*
+import org.team5499.monkeyLib.util.time.measureTimeFPGA
+
+// thanks 2471
+
+class PeriodicScope @PublishedApi internal constructor(val period: Double) {
+    @PublishedApi
+    internal var isDone = false
+
+    fun stop() {
+        isDone = true
+    }
+}
+
+/**
+ * Runs the provided [body] of code periodically per [period] seconds.
+ *
+ * The provided [body] loop will continue to loop until [PeriodicScope.stop] is called, or an exception is thrown.
+ * Note that if [PeriodicScope.stop] is called the body will continue to run to the end of the loop. If your
+ * intention is to exit the code early, insert a return after calling [PeriodicScope.stop].
+ *
+ * The [period] parameter defaults to 0.02 seconds, or 20 milliseconds.
+ *
+ * If the [body] takes longer than the [period] to complete, a warning is printed. This can
+ * be disabled by setting the [watchOverrun] parameter to false.
+ */
+suspend inline fun periodic(
+    period: Double = 0.02,
+    watchOverrun: Boolean = false,
+    crossinline body: PeriodicScope.() -> Unit
+) {
+    val scope = PeriodicScope(period)
+
+    val watchdog = if (watchOverrun) {
+        Watchdog(period) { DriverStation.reportWarning("Periodic loop overrun", true) }
+    } else {
+        null
+    }
+
+    while (true) {
+        watchdog?.reset()
+        val dt = measureTimeFPGA {
+            body(scope)
+        }
+        if (scope.isDone) break
+        val remainder = period - dt
+        if (remainder <= 0.0) {
+            yield()
+        } else {
+            delay(remainder)
+        }
+    }
+//    var cont: CancellableContinuation<Unit>? = null
+//    var notifier: Notifier? = null
+//    try {
+//        suspendCancellableCoroutine<Unit> { c ->
+//            cont = c
+//            notifier = Notifier {
+//                body(scope)
+//                watchdog?.reset()
+//                if (scope.isDone && c.isActive) c.resume(Unit)
+//            }.apply { startPeriodic(period) }
+//        }
+//    } catch (e: Throwable) {
+//        cont?.cancel(e)
+//    } finally {
+//        notifier?.close()
+//        watchdog?.close()
+//    }
+}
+
+/**
+ * Suspends until [condition] evaluates to true.
+ *
+ * @param pollingRate The time between each check, in milliseconds
+ */
+suspend inline fun suspendUntil(pollingRate: Int = 20, condition: () -> Boolean) {
+    while (!condition()) delay(pollingRate.toLong())
+}
+
+/**
+ * Runs each block in [blocks] in a child coroutine, and suspends until all of them have completed.
+ *
+ * If one child is cancelled, the remaining children are stopped, and the exception is propagated upwards.
+ */
+suspend inline fun CoroutineScope.parallel(vararg blocks: suspend () -> Unit) {
+    blocks.map { block ->
+        launch { block() }
+    }.forEach {
+        it.join()
+    }
+}
+
+/**
+ * Suspends the coroutine for [time] seconds.
+ *
+ * @see kotlinx.coroutines.delay
+ */
+suspend inline fun delay(time: Double) = delay((time * 1000).toLong())
+
+/**
+ * Suspends the coroutine forever.
+ *
+ * A halted coroutine can still be canceled.
+ */
+suspend inline fun halt(): Nothing = suspendCancellableCoroutine {}

src/main/kotlin/org/team5499/monkeyLib/util/coroutines/MonkeyDispatcher.kt

@@ -0,0 +1,7 @@
+package org.team5499.monkeyLib.util.coroutines
+
+import kotlinx.coroutines.CoroutineDispatcher
+import kotlinx.coroutines.asCoroutineDispatcher
+import java.util.concurrent.Executors
+
+val MonkeylibDispatcher: CoroutineDispatcher = Executors.newFixedThreadPool(2).asCoroutineDispatcher()

src/main/kotlin/org/team5499/monkeyLib/util/time/TimeUtils.kt

@@ -0,0 +1,22 @@
+package org.team5499.monkeyLib.util.time
+
+import edu.wpi.first.wpilibj.RobotController
+import edu.wpi.first.wpilibj.Timer
+
+/**
+* Executes the given block and returns elapsed time in seconds.
+*/
+inline fun measureTimeFPGA(body: () -> Unit): Double {
+    val start = Timer.getFPGATimestamp()
+    body()
+    return Timer.getFPGATimestamp() - start
+}
+
+/**
+ * Executes the given block and returns elapsed time in nanoseconds.
+ */
+inline fun measureTimeFPGAMicros(body: () -> Unit): Long {
+    val start = RobotController.getFPGATime()
+    body()
+    return RobotController.getFPGATime() - start
+}