diff --git a/road-runner/actions/src/main/kotlin/com/acmerobotics/roadrunner/Actions.kt b/road-runner/actions/src/main/kotlin/com/acmerobotics/roadrunner/Actions.kt index 5dfad7b7..841e853e 100644 --- a/road-runner/actions/src/main/kotlin/com/acmerobotics/roadrunner/Actions.kt +++ b/road-runner/actions/src/main/kotlin/com/acmerobotics/roadrunner/Actions.kt @@ -134,6 +134,38 @@ class InstantAction(val f: InstantFunction) : Action { } } +fun interface ConditionalFunction { + fun condition(): Boolean +} + +/** + * Blocking action that waits until [f] is true, for a minimum duration of [minTime] + * and a maximum duration of [maxTime]. + */ +data class WaitUntilAction( + val f: ConditionalFunction, + val minTime: Double = 0.0, + val maxTime: Double = Double.MAX_VALUE +) : Action { + private var beginTs = -1.0; + + override fun run(p: TelemetryPacket): Boolean { + val t = if (beginTs < 0) { + beginTs = now() + 0.0 + } else { + now() - beginTs + } + + if (t < minTime) return true + if (t >= maxTime) return false + + return !f.condition() + } + + override fun preview(fieldOverlay: Canvas) {} +} + /** * Null action that does nothing. */ diff --git a/road-runner/actions/src/test/kotlin/com/acmerobotics/roadrunner/ActionRegressionTest.kt b/road-runner/actions/src/test/kotlin/com/acmerobotics/roadrunner/ActionRegressionTest.kt index 6dac823f..81f52296 100644 --- a/road-runner/actions/src/test/kotlin/com/acmerobotics/roadrunner/ActionRegressionTest.kt +++ b/road-runner/actions/src/test/kotlin/com/acmerobotics/roadrunner/ActionRegressionTest.kt @@ -55,6 +55,11 @@ fun sexpFromAction(a: Action): Sexp = is SleepAction -> Sexp.list(Sexp.Atom("sleep"), Sexp.Atom(String.format("%.10f", a.dt))) is SequentialAction -> Sexp.list(listOf(Sexp.Atom("seq")) + a.initialActions.map(::sexpFromAction)) is ParallelAction -> Sexp.list(listOf(Sexp.Atom("par")) + a.initialActions.map(::sexpFromAction)) + is WaitUntilAction -> Sexp.list( + Sexp.atom("waitUntil"), + Sexp.Atom(String.format("%.10f", a.minTime)), + Sexp.Atom(String.format("%.10f", a.maxTime)) + ) is NullAction -> Sexp.Atom("null") else -> Sexp.Atom("unk") } diff --git a/road-runner/actions/src/test/kotlin/com/acmerobotics/roadrunner/ActionsTest.kt b/road-runner/actions/src/test/kotlin/com/acmerobotics/roadrunner/ActionsTest.kt index 4630fd6b..36ff8a40 100644 --- a/road-runner/actions/src/test/kotlin/com/acmerobotics/roadrunner/ActionsTest.kt +++ b/road-runner/actions/src/test/kotlin/com/acmerobotics/roadrunner/ActionsTest.kt @@ -120,4 +120,22 @@ class ActionsTest { ) assertEquals(listOf(1), steps) } + + @Test + fun testWaitUntilCondition() { + var condition = false + var count = 0 + + assertEquals( + 2, + runBlockingCount( + WaitUntilAction({ + count++ + condition = count >= 3 + condition + }) + ) + ) + assert(condition) + } }