Team 1757's Robot Code

Code structure and features

Andrew Cabey - March 9, 2017


During this past year, FRC Team 1757 has undergone a huge number of changes ranging from the team’s composition, hierarchy, infrastructure and goals for competition; among these changes has been an increased focus on the programming side of the game challenge. With our two Senior programmers soon leaving responsibility in the hands of our less-experienced underclassmen, we dedicated a huge portion of our time and effort this build season to writing clean, understandable and reusable code to serve as an example for years to come. With this focus, we pioneered the team’s use of technologies such as closed-loop error correction (PID) and vision processing ultimately resulting in an impressive, well-featured codebase that has given us the edge in competition.

Our Robot

To give some context as to what these features enable us to do, our robot is equipped with a mecanum drivetrain, a fuel collector, hopper and shooter, an active gear loading/placing mechanism and a rope lifter; our data is sourced from a Kauai labs NavX-MXP as well as two onboard Lifecam 3000 cameras fed directly into the RoboRIO. Using the onboard gyroscope, we are able to precisely control the mecanum wheels to drive perfectly straight on the X or Y axes with active correction, as well as rotate the robot to a specified angle for alignment. Taking advantage of the cameras, we are able to use vision processing through GRIP, a wrapper of OpenCV specifically developed for FIRST teams, to face the robot at a target on the field with reflective tape, for Steamworks, the gear peg and the boiler’s high goal; this technology allows us to consistently place gears in autonomous as well as align the robot to shoot fuel.

Command-based Programming

To start off, the biggest and probably most beneficial change we made this year was moving from iterative Java to command-based Java programming; the writers of the WPIlib have put copious effort into the command scheduling system, allowing absurdly simple button bindings, autonomous routines and coordination between subsystems. If you are not familiar with command-based programming in FRC (just as we were to begin the season), it is essentially a pre-made skeletal structure that separates your code into commands, subsystems and ‘administrative’ driver classes while providing a fantastic scheduler for interoperability between subsystems and close integration with the other tools provided by the WPIlib such as the SmartDashboard. Because there is a lot of new material introduced with command-based, we began the season using WPIlib’s RobotBuilder utility, which provides a graphical interface for modeling the robot’s systems and actuators in code, then exporting this model into auto-generated Java code. For our ‘production’ code we did not RobotBuilder, but used the auto-generated code as a reference.


The program’s ‘entry point’ (as visible to us) lies in the “robot” package under the class “Robot,” where those who are in any way familiar with WPIlib Java code will immediately recognize methods such as robotInit(), teleopInit(), and teleopPeriodic(). For each of the robot control modes -- disabled, autonomous, teleop, and test -- this class provides *Init (called only once when entering the mode) and *Periodic (called by default every 20ms while the robot is in the mode) methods where we are free to write code depending on how we’d like the robot to behave. In, we declare each of our subsystems as public static objects and initialize them within robotInit(). Within the periodic methods, the class includes auto-generated code simply running the WPIlib scheduler; because the scheduler provides many simple ways to manage routines, we don’t need anything else within the periodic methods, and with that the class itself.


In order to maintain some abstraction from the grittiness of the hardware, every physical device is contained and managed within the second ‘administrative’ class in the “robot” package, “RobotMap.” Within RobotMap, we declare each physical device as a public static object and initialize them in the provided init() method that is called inside of robotInit(). We also made the choice to include any near-hardware level persistent objects (PID controllers and vision processing wrappers) within RobotMap as they have similar access requirements as the hardware itself. With the initialization of these objects, we do any hardware configuration changes such as Talon control modes, inversion, soft limits, etc.


The final class in the “robot” package is “OI.” Fairly self-explanatory, this class contains anything to do with the Operator Interface, including the gamepad object(s), button bindings, and SmartDashboard layout.


Past the robot subsystem lies the real ‘meat’ of the robot’s functionality, the subsystems. Each subsystem should represent (in a high level) a physical, hardware system on the robot. For the Steamworks game, our robot is comprised of 7 subsystems, DriveTrain, BallCollector, GearLoader, Indexer, Shooter, Lifter, and Vision. Each subsystem class contains all of the code needed to interface with hardware such that a command is able to simply call multiple methods in order to perform a high level action on the robot: because a command may only access whatever the subsystem allows it to (no direct interface with hardware), what we found works well are highly modular methods in the subsystems. To demonstrate the difference between the methods in a subsystem and a command, we can look at our implementation of the GearLoader subsystem: we provide methods to enable the motor controller, initialize its PID control, determine if the controller has reached its setpoint and of course set a target for the controller. Each of these are relatively ‘low-level’ methods that depend on the specifics of hardware and modular to the point that each method performs a very specific action. A command is then able to chain together enabling the controller, setting the target and disabling when the target is reached in order to perform a high level action, ‘scoring a gear.’ Each subsystem serves as the mid-level layer between low-level hardware specifics and high-level in-game actions.


At the highest level of abstraction, the commands represent in-game actions that the robot is able to perform. Commands are able to utilize the provisions of any subsystem to form runnable routines for the scheduler. Along with the generic “Command” type, the WPIlib provides a number specialized types of commands to serve almost any conceivable requirement: the TimedCommand, which adds a timeout to the command’s run-length; the InstantCommand, which will execute exactly once before it is ended by the scheduler, the ConditionalCommand, which will choose to run one of two other commands based on a condition; and the CommandGroup, which will allow the programmer to schedule a number of commands sequentially or in parallel.

For each of our subsystems we wrote a number of commands for essentially every possible distinct action that might be of possible use in the game, which resulted in an intimidating 49 class files; whether or not a cleanup of the commands that are not actually used in game is up for debate, but my personal take is that since we aren’t be charged per-file, I would rather keep options open. Perhaps with the final refactoring and archival of this years codebase I would consider it.


Soon into our ventures with close-loop control and vision processing we found that the ‘unique’ way that we implement PID-assisted drive we required some extra ‘utility’ classes such as wrappers for PIDSource and PIDOutput (an explanation for which would probably merit a post of its own).


This year has been one of the first in the team’s history where we can take pride in every aspect of our product. The robot is well-built and competitive with a programmatic backing that sets us apart from other teams. Moving onto other adventures, I’m happy with what we are leaving behind.

To see our complete code for 2017-Steamworks, check us out on Github