diff --git a/wpi/src/main/kotlin/org/ghrobotics/lib/utils/TimeUtils.kt b/wpi/src/main/kotlin/org/ghrobotics/lib/utils/TimeUtils.kt new file mode 100644 index 0000000..3016816 --- /dev/null +++ b/wpi/src/main/kotlin/org/ghrobotics/lib/utils/TimeUtils.kt @@ -0,0 +1,17 @@ +/* + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. + * + * Copyright 2019, Green Hope Falcons + */ + +package org.ghrobotics.lib.utils + +import edu.wpi.first.wpilibj.Timer + +fun measureTimeFPGA(block: () -> Unit): Double { + val start = Timer.getFPGATimestamp() + block() + return Timer.getFPGATimestamp() - start +} diff --git a/wpi/src/main/kotlin/org/ghrobotics/lib/utils/WpiCoroutineUtils.kt b/wpi/src/main/kotlin/org/ghrobotics/lib/utils/WpiCoroutineUtils.kt index e4ef95b..4fe908f 100644 --- a/wpi/src/main/kotlin/org/ghrobotics/lib/utils/WpiCoroutineUtils.kt +++ b/wpi/src/main/kotlin/org/ghrobotics/lib/utils/WpiCoroutineUtils.kt @@ -8,18 +8,28 @@ package org.ghrobotics.lib.utils +import edu.wpi.first.wpilibj.DriverStation +import edu.wpi.first.wpilibj.Watchdog +import kotlinx.coroutines.CoroutineDispatcher import kotlinx.coroutines.CoroutineScope import kotlinx.coroutines.CoroutineStart import kotlinx.coroutines.Job +import kotlinx.coroutines.asCoroutineDispatcher +import kotlinx.coroutines.delay import kotlinx.coroutines.isActive import kotlinx.coroutines.launch +import kotlinx.coroutines.yield +import org.ghrobotics.lib.mathematics.units.SIUnit +import org.ghrobotics.lib.mathematics.units.Second import org.ghrobotics.lib.wrappers.FalconNotifier +import java.util.concurrent.Executors import kotlin.coroutines.CoroutineContext -import kotlin.coroutines.EmptyCoroutineContext + +val FalconDispatcher: CoroutineDispatcher = Executors.newFixedThreadPool(2).asCoroutineDispatcher() fun CoroutineScope.launchFrequency( frequency: Int = 50, - context: CoroutineContext = EmptyCoroutineContext, + context: CoroutineContext = FalconDispatcher, start: CoroutineStart = CoroutineStart.DEFAULT, block: suspend CoroutineScope.() -> Unit, ): Job { @@ -42,3 +52,48 @@ suspend fun CoroutineScope.loopFrequency( notifier.updateAlarm() } } + +class PeriodicScope @PublishedApi internal constructor(val period: Double) { + @PublishedApi + internal var isDone = false + + fun stop() { + isDone = true + } +} + +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.reportError("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.toLong()) + } + } +} + +suspend inline fun periodic( + period: SIUnit, + watchOverrun: Boolean = false, + crossinline body: PeriodicScope.() -> Unit, +) = periodic(period.value, watchOverrun, body)