diff --git a/build.gradle b/build.gradle index fe72eeb..2a4e7f0 100644 --- a/build.gradle +++ b/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() diff --git a/src/main/kotlin/org/team5499/monkeyLib/util/coroutines/CoroutineUtils.kt b/src/main/kotlin/org/team5499/monkeyLib/util/coroutines/CoroutineUtils.kt new file mode 100644 index 0000000..9cdf37a --- /dev/null +++ b/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? = null +// var notifier: Notifier? = null +// try { +// suspendCancellableCoroutine { 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 {} diff --git a/src/main/kotlin/org/team5499/monkeyLib/util/coroutines/MonkeyDispatcher.kt b/src/main/kotlin/org/team5499/monkeyLib/util/coroutines/MonkeyDispatcher.kt new file mode 100644 index 0000000..d861715 --- /dev/null +++ b/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() diff --git a/src/main/kotlin/org/team5499/monkeyLib/util/time/TimeUtils.kt b/src/main/kotlin/org/team5499/monkeyLib/util/time/TimeUtils.kt new file mode 100644 index 0000000..7f77653 --- /dev/null +++ b/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 +}