Coroutines in FF Auto

Coroutines.kt

// inspired by https://github.com/JDroids/FreightFrenzy/blob/master/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/CyclingWarehouseSideAuto.java

import kotlinx.coroutines.*

typealias TrajectorySequence = String

interface MecanumDrive {
    fun followTrajectorySequenceAsync(ts: TrajectorySequence)
    fun update()
    fun isBusy(): Boolean
}

interface Deposit {
    fun goToLevel1()
    fun goToHeight(x: Double)
    fun deploy()
    fun retract()
}

interface Intake {
    fun intake()
    fun outtake()
    fun setPower(x: Double)
    fun stop()
}


enum class Randomization {
    LEVEL_1, LEVEL_2, LEVEL_3
}

class OpMode(val drive: MecanumDrive, val deposit: Deposit, val intake: Intake) {
    fun waitForStart() {}

    suspend fun followTrajectorySequence(ts: TrajectorySequence) {
        drive.followTrajectorySequenceAsync(ts)

        while (drive.isBusy()) {
            drive.update()

            yield()
        }
    }

    suspend fun cycleCommand(intakeCycle: TrajectorySequence, toShippingHub: TrajectorySequence) = coroutineScope {
        listOf(
            async { followTrajectorySequence(intakeCycle) },
            async {
                deposit.retract()

                yield()

                intake.intake()

                delay(1000L)
            }
        ).forEach { it.await() }

        delay(1000L)

        intake.setPower(0.2)

        listOf(
            async { followTrajectorySequence(toShippingHub) },
            async {
                deposit.goToHeight(24.5)

                yield()

                intake.outtake()

                delay(1000L)
            }
        ).forEach { it.await() }

        intake.stop()

        yield()

        deposit.deploy()

        delay(1200L)
    }

    fun runOpMode() {
        val r = Randomization.LEVEL_1

        waitForStart()

        runBlocking {
            when (r) {
                Randomization.LEVEL_1 -> deposit.goToLevel1()
                Randomization.LEVEL_2 -> deposit.goToHeight(14.0)
                Randomization.LEVEL_3 -> deposit.goToHeight(24.5)
            }

            followTrajectorySequence(when (r) {
                Randomization.LEVEL_1 -> "toShippingHubLevel1"
                Randomization.LEVEL_2 -> "toShippingHubLevel2"
                Randomization.LEVEL_3 -> "toShippingHubLevel3"
            })

            deposit.deploy()

            delay(1000L)

            cycleCommand("intakeCycle1", "toShippingHubForCycling1")

            deposit.retract()

            followTrajectorySequence("parkFromFirstCycle")
        }
    }
}