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:
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.