Polymorphic Commands in FF Auto

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


interface Command {
    fun run(): Command?
}

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

    runBlocking(c.run() ?: return)
}


class DeadlineCommand(val ts: Double) : Command {
    override fun run() =
        if (clock() >= ts) {
            null
        } else {
            this
        }
}

class SleepCommand(val dt: Double) : Command {
    override fun run() = DeadlineCommand(clock() + dt).run()
}

class InstantCommand(val f: () -> Unit) : Command {
    override fun run(): Command? {
        f()
        return null
    }
}

class ParallelCommand(val cs: List<Command>) : Command {
    constructor(vararg cs: Command) : this(cs.asList())

    override fun run() =
        cs.mapNotNull { it.run() }.let { cs ->
            if (cs.isEmpty()) {
                null
            } else {
                ParallelCommand(cs)
            }
        }
}

class SequentialCommand(val cs: List<Command>) : Command {
    constructor(vararg cs: Command) : this(cs.asList())

    override fun run() =
        if (cs.first().run() == null) {
            if (cs.size == 1) {
                null
            } else {
                SequentialCommand(cs.drop(1))
            }
        } else {
            this
        }
}

class WaitUntilIdle(val drive: MecanumDrive) : Command {
    override fun run(): Command? {
        drive.update()
        return if (drive.isBusy()) {
            this
        } else {
            null
        }
    }
}

class FollowTrajectorySequence(val drive: MecanumDrive, val ts: TrajectorySequence) : Command {
    override fun run(): Command? {
        drive.followTrajectorySequenceAsync(ts)
        return WaitUntilIdle(drive).run()
    }
}


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