A module system for coding FTC robots

1 Abstract

Writing code for FTC1 often turns into a mess — the need to quickly and often change what the robot does to get the perfect autonomous, the modifications done on the physical systems by the engineers, the fact that you didn't account for some edge case when the robot doesn't want to rotate more than 66.6 degrees on a full moon etc., all those things mean that you, as a programmer, will often have to resort to dropping some quick hacks in the middle of your codebase. This article will present a way to structure your code in such a way that you'll be able to develop faster and bugs will be easier to triangulate by representing (roughly) every physical system as a module.

2 Vision

What do I mean by module?

Well, a module should:

  • encapsulate all the data and logic of a physical system (e.g. represent all the motors of the drive train and contain functions that make them go vrummm)
  • be somehow generic (in the sense that different modules can be used together thanks to their abstractions; e.g. to have the capacity to store multiple modules in a container)
  • be easily swappable (very important to point out, but this point is practically a product of the previous one)
  • be easy to use (this means that it requires implementing some helpers around the project to decrease the code needed to access and use a module's properties and functions; the API has to be intuitive)

On one hand, modules should greatly improve a team's productivity during the testing period. The ease of quickly swapping components instead of commenting large regions or adapting them for new systems will quickly win over both the organized and the lazy programmers. Your engineers decide to change the drive train? While it may take 30 minutes for them, for you it's just a few seconds – replace HolonomicDriveTrainModule with MecanumDriveTrainModule and you're done.

On the other hand, high stress moments such as quickly fixing something between matches will also benefit from such a neat way of organizing code as modules. Finding the place where you need to perform an intervention will be easier now that you don't have all your code in a single monolithic OpMode file.

Even more so, modules reduce duplicated code. We all had a time when we would jump between OpModes dancing with the fingers on Control, C, X and V. That time is over — modules come with the philosophy of write once, use anywhere.

In the end all of this will basically turn into a bit of boilerplate, but boilerplate which, in my opinion, is worth the effort because it makes organising the code, localizing bugs and switching components very easy.

NOTE: All the code in this article is Kotlin.

NOTE: I will be removing the import statements from the files because only God knows which ones to keep and which to discard.2

NOTE: I will be modifying some of the snippets in place (that means I'm copying them from a git repo into this article and then, without any tool to verify the correctness of the code, I'm messing with it) — this might lead to some bugs here and there.

3 The base for our modules

Enough with the propaganda, let's get to work.

Since we've established that a module should, more or less, fully encapsulate a physical system, it should contain a container for robot components: a HashMap<String, HardwareDevice> where the key is the name of a component and the value is the actual component itself as a HardwareDevice. We'll want to firstly create a blueprint of what a module should be, so we'll use an interface.

Here's the definition of the base of all our modules:

interface RobotModule {
    var components: HashMap<String, HardwareDevice>
    val opMode: OpMode
    val hardwareMap get() = opMode.hardwareMap
    val telemetry get() = opMode.telemetry

    fun init() { }
    fun start() { }
    fun stop() { }

    fun <T: HardwareDevice> get(name: String): T = components[name] as T
}

As you see, it contains a bit more than a hashmap.

To be able to initialize all the hardware it uses, the module also needs to hold a reference to the opMode it's used from. hardwareMap and telemetry are just properties that get frequently used fields of the opMode.

I also defined three methods: init, start, stop. Those do nothing by default, but it's nice to have some guaranteed standard way to initialize a module. Custom modules will override those methods with their own logic.

Finally, the get method accepts a string representing the name of a component, but also a type parameter that inherits HardwareDevice. We use get to retrieve a hardware component from a module's hashmap and to convert it to a specified type. For example:

val frontLeftMotor = driveTrain.get<DcMotor>("front-left")

4 Constructing a Robot

We can imagine a robot as being represented by the set of all its mechanisms. For programming reasons, it also has to have access to the opMode you'll be using with it.

Let's see how can we represent that:

class Robot(val opMode: OpMode, val modules: Set<RobotModule>) {
    inline fun <reified T: RobotModule> get(): T = modules.first { x -> x is T } as T
}

The get function, similar to the one from RobotModule, returns us a module by searching for it's type. Supposing you had a robot: Robot which includes a DriveTrain module, to acces that you would do:

robot.get<DriveTrain>()

5 Making use of our Robot in OpModes

The simple and obvious approach would be to create a robot field in the desired opMode. That's fine, but it kinda sucks for the reason that you always have to write robot. when getting its modules, and it's a lot of typing. Ain't nobody got time for that.

Let's design our own OpMode and LinearOpMode such that they offer better integration with their robots.

interface MyOpModeBase  {
    val robot: Robot
}

inline fun <reified T: RobotModule> MyOpModeBase.get(): T = robot.get()
abstract class MyOpMode : OpMode(), MyOpModeBase
abstract class MyLinearOpMode : LinearOpMode(), MyOpModeBase

Now you just have to make your opModes inherit those two new classes rather than the old, plain OpMode and LinearOpMode.

6 Sample modules and usage

6.1 Mecanum drivetrain

I'll show you a sample implementation for a Mecanum drive train module. Since we want to take advantage of our module system, we'll first create an abstract class that represents what a generic drive train can do:

abstract class DriveTrainModule : RobotModule {
    val motors get() = components.filter { it.value is DcMotorEx }.map { it.value as DcMotorEx }.toList()
    val motorsWithNames get() = components.map { Pair(it.key, it.value as DcMotorEx) }.toMap()

    abstract fun encoderDrive(inches: Double, power: Double, timeout: Double)
    abstract fun forward(inches: Double, power: Double = DEFAULT_POWER, timeout: Double)
    abstract fun sideways(inches: Double, power: Double = DEFAULT_POWER, timeout: Double)

    companion object {
	const val DEFAULT_POWER = 0.5
    }
}

Only then, can we implement our Mecanum drive train, making it inherit the thing above. This approach is nice because you can have modules ready for other drive trains, too, and if your engineers decide to swap them out, all you need is to change MecanumDriveTrainModule with HolonomicDriveTrainModule in your robot's set of modules (and anywhere in your opMode where you get<MecanumDriveTrainModule>()). All the functions have the same name with this approach, so after a simple and quick change you're ready to go.

class MecanumDriveTrainModule(override val opMode: OpMode) : DriveTrainModule() {
    override var components: HashMap<String, HardwareDevice> = hashMapOf()

    override fun init() {
	listOf("lf", "rf", "lb", "rb")
	    .forEach { name -> components[name] = hardwareMap.get(DcMotorEx::class.java, name) }

	motorsWithNames
	    .forEach { (name, motor) ->
		when (name) {
		    "rf", "rb" -> motor.direction = DcMotorSimple.Direction.REVERSE
		}
		motor.zeroPowerBehaviour = DcMotor.ZeroPowerBehavior.BRAKE
	    }
    }

    override fun stop() {
	motors.forEach { it.power = 0.0 }
    }

    override fun encoderDrive(inches: Double, power: Double, timeout: Double) {
	val newTarget = (inches * COUNTS_PER_INCH).toInt()
	val stopwatch = ElapsedTime()

	motors.forEach { it.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER }
	motors.forEach { it.targetPosition = newTarget }
	motors.forEach { it.mode = DcMotor.RunMode.RUN_TO_POSITION }
	motors.forEach { it.power = abs(power) }

	while (linearOpMode.opModeIsActive() &&
	    stopwatch.seconds() < timeout &&
	    motors.all { it.isBusy }) 
	{
	    motorsWithNames.forEach {
		telemetry.addData(it.key, "${it.value.currentPosition} ->> ${it.value.targetPosition}")
	    }
	    telemetry.update()
	}

	stop()
    }

    override fun forward(inches: Double, power: Double, timeout: Double) {
	get<DcMotorEx>("lf").direction = DcMotorSimple.Direction.FORWARD
	get<DcMotorEx>("rf").direction = DcMotorSimple.Direction.REVERSE
	get<DcMotorEx>("lb").direction = DcMotorSimple.Direction.FORWARD
	get<DcMotorEx>("rb").direction = DcMotorSimple.Direction.REVERSE
	encoderDrive(inches, power, timeout)
    }

    override fun sideways(inches: Double, power: Double, timeout: Double) {
	get<DcMotorEx>("lf").direction = DcMotorSimple.Direction.FORWARD
	get<DcMotorEx>("rf").direction = DcMotorSimple.Direction.FORWARD
	get<DcMotorEx>("lb").direction = DcMotorSimple.Direction.REVERSE
	get<DcMotorEx>("rb").direction = DcMotorSimple.Direction.REVERSE
	encoderDrive(inches, power, timeout)
    }

    companion object {
	const val COUNTS_PER_MOTOR_REV = 383.6
	const val WHEEL_DIAMETER = 4.0 // in inches
	const val DRIVE_GEAR_REDUCTION = 2.0
	const val COUNTS_PER_INCH = COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION / (WHEEL_DIAMETER * PI)
	const val DEFAULT_POWER = 0.5
    }
}

6.2 Example OpMode structure using modules

@TeleOp(name = "Sample", group = "TESTS")
class ControlledSimple : MyOpMode() {
    override val robot: Robot = Robot(this,
	setOf(
	    Mecanum(this),
	    Hook(this),
	    Lift(this),
	    Intake(this)
	))

    override fun init() {
	robot.modules.forEach { it.init() }
    }

    override fun loop() {
	// ...
    }
}

7 Using the same module class multiple times

Imagine your robot has two identical pieces that can be represented as a module, but each one of those require some special values to work right. You could think of two servos that do the same things but only on opposite sides of the robot, so they would require different positions.

The solution for this is to abuse the object-oriented type system. Since our robots can hold only one module of each type, we'll make multiple types for the same module. How? Create a new class for each instance of that module on your robot, class in which you don't write a single thing, but make it inherit your module.

If you're getting any errors that say that you can't inherit the module, try making it open.

Say you end up with LeftArm and RightArm, both inheriting ArmModule. Now all you need is to treat them like two separate modules, including them both in a robot's module set. If you want to do something with the left arm, all you need is get<LeftArm>().

Footnotes:

2

I'm writing this from a machine with GNU Guix, which doesn't have Android Studio to give me insights as to what is needed.