Sealed Commands in FF Auto

SealedCommands.kt

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

fun clock() = 0.0

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()
}


sealed interface Command
class DeadlineCommand(val ts: Double) : Command
class SleepCommand(val dt: Double) : Command
class InstantCommand(val f: () -> Unit) : Command
class ParallelCommand(val cs: List<Command>) : Command {
    constructor(vararg cs: Command) : this(cs.asList())
}
class SequentialCommand(val cs: List<Command>) : Command {
    constructor(vararg cs: Command) : this(cs.asList())
}
class WaitUntilIdle(val drive: MecanumDrive) : Command
class FollowTrajectorySequence(val drive: MecanumDrive, val ts: TrajectorySequence) : Command

fun run(c: Command): Command? =
    when (c) {
        is DeadlineCommand ->
            if (clock() >= c.ts) {
                null
            } else {
                c
            }
        is SleepCommand ->
            run(DeadlineCommand(clock() + c.dt))
        is InstantCommand -> {
            c.f()
            null
        }
        is ParallelCommand ->
            c.cs.mapNotNull { run(it) }.let { cs ->
                if (cs.isEmpty()) {
                    null
                } else {
                    ParallelCommand(cs)
                }
            }
        is SequentialCommand ->
            if (run(c.cs.first()) == null) {
                if (c.cs.size == 1) {
                    null
                } else {
                    SequentialCommand(c.cs.drop(1))
                }
            } else {
                c
            }
        is WaitUntilIdle -> {
            c.drive.update()
            if (c.drive.isBusy()) {
                c
            } else {
                null
            }
        }
        is FollowTrajectorySequence -> {
            c.drive.followTrajectorySequenceAsync(c.ts)
            run(WaitUntilIdle(c.drive))
        }
    }

tailrec fun runBlocking(c: Command) {
    if (Thread.currentThread().isInterrupted) {
        return
    }

    runBlocking(run(c) ?: return)
}


enum class Randomization {
    LEVEL_1, LEVEL_2, LEVEL_3
}

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

    fun cycleCommand(intakeCycle: TrajectorySequence, toShippingHub: TrajectorySequence) =
        SequentialCommand(
            ParallelCommand(
                FollowTrajectorySequence(drive, intakeCycle),
                SequentialCommand(
                    InstantCommand(deposit::retract),
                    ParallelCommand(
                        SleepCommand(1.0),
                        InstantCommand(intake::intake)
                    )
                )
            ),
            SleepCommand(1.0),
            InstantCommand { intake.setPower(0.2) },
            ParallelCommand(
                FollowTrajectorySequence(drive, toShippingHub),
                SequentialCommand(
                    InstantCommand { deposit.goToHeight(24.5) },
                    ParallelCommand(
                        SleepCommand(1.0),
                        InstantCommand(intake::outtake)
                    )
                )
            ),
            InstantCommand(intake::stop),
            InstantCommand(deposit::deploy),
            SleepCommand(1.2)
        )

    fun runOpMode() {
        val r = Randomization.LEVEL_1

        waitForStart()

        runBlocking(SequentialCommand(
            ParallelCommand(
                FollowTrajectorySequence(
                    drive,
                    when (r) {
                        Randomization.LEVEL_1 -> "toShippingHubLevel1"
                        Randomization.LEVEL_2 -> "toShippingHubLevel2"
                        Randomization.LEVEL_3 -> "toShippingHubLevel3"
                    }
                ),
                InstantCommand {
                    when (r) {
                        Randomization.LEVEL_1 -> deposit.goToLevel1()
                        Randomization.LEVEL_2 -> deposit.goToHeight(14.0)
                        Randomization.LEVEL_3 -> deposit.goToHeight(24.5)
                    }
                }
            ),
            ParallelCommand(
                InstantCommand(deposit::deploy),
                SleepCommand(1.0)
            ),
            cycleCommand("intakeCycle1", "toShippingHubForCycling1"),
            ParallelCommand(
                FollowTrajectorySequence(drive, "parkFromFirstCycle"),
                InstantCommand(deposit::retract)
            )
        ))
    }
}