New Command and Control Framework for FRC LabVIEW programming.
In addition to the usual iterative framework, this is the first year FIRST will offer the option to use a command-based framework to program the RoboRIO. This option already existed in JAVA (and maybe C++) for at least a couple of years, it has now been extended to LabVIEW. Team 4153 is part of the 2016 beta test program so had the opprotunity to test it. As a reminder, in the iterative framework, there is a main loop that check controls and sensors and set the actuator states accordingly. In contrast, in a command-based framework, the main loop still checks for controls and sensors but sends command to various subsystems to act; the difference is that the main loop schedules tasks but does not execute them. Thus, each subsystem runs in its own thread, breaking any dependencies between them and thus avoiding conflicts. Another important consequence of having a command-based framework is that the speed of the main loop is no longer determined by the slowest subsystem. A final implication is that a command can independently persist a long time without blocking the other systems from executing. More on this detail when we look at the controllers later in this document. This document was written to give an overview of the new FRC command-based framework.
Here is the block diagram of the robot Main for the new command-based framework:
At first glance Robot Main looks very similar to the iterative framework. The big difference is that instead of the “PeriodicTasks.vi” there is a “SubSystems.vi”. In the command-based framework, the SubSystems.vi is where most of the action takes place. Indeed, this VI contains the controllers of all the subsystems. The “Teleop.vi” only sends a command.
In fact, the whole new framework is based on subsystems. By default, there is one subsystem for the drive. As an example, we’ll use the code needed for running our (last year’s) robot Fermi. Using the new framework, we rewrote the code and ended with 5 subsystems: one for the drive, one for moving an elevator, one to control the arms on the elevator, one for controlling a brake that holds the elevator in place, and one to control LEDs to create a visual feedback to the drive team. Even though we ended up with 5 for Fermi, there is no limit on how many subsystems can be created.
In the LabVIEW project, each subsystem has 2 categories of Vis:
1) the commands that are Vis called to schedule a given task for that subsystem. There can be as many as you want. For example, for a drive system, there could be a driveForaTime function, a turnleft or turnright or backup function and maybe an immediate function. The first ones would likely be very useful for easy programming of the autonomous part of competition, the immediate might be used for a reaction to the use of a joystick or other input.
2) the implementation VIs. They consist of
a) An operation type definition to specify which commands exist for that subsystem
b) A setpoint type definition that is a cluster including the operation control and all the parameters you need to provides the commands and
c) 3 infrastructure VIs: The controller that will operate the subsystem according to what command is sent, an initialization VI that will initialize all the notifiers needed by the framework, and a command helper that is the basic VI used to create the command VIs.
The framework is based on the use of notifier (basically a single element queue). In a simple way, the initialization VI creates a notifier specific for the subsystem that will be used to pass commands around. The command VIs format the parameters for a specific command and pass them to the helper that will use the notifier to notify the controller that it should execute that command. The controller waits on the notifier to receive an element, decode the command, and activate the actuator accordingly.
To make things clearer, let’s have a look at the drive subsystem FRC provided as an example.
In this example, we have a simple arcade drive with 2 motors. 3 commands are defined for this drive:
- Drive immediate which needs to get the desired left and right speeds for the motors and will set the motors immediately to those set points
- Drive For Time which needs to get the desired left and right speeds and for how long they will be applied and
- Stop Driving which sets the motors to 0.
As a result, here is the set points type definition for this subsystem:
It contains an enumerative type definition with all the possible commands and all the parameters that could be needed for the commands.
Here is the block diagram of the main element, the controller.vi that need to be inserted in the subsystems.vi in the main robot.vi:
On the left side is the initialization of the motors. This could also be done in the Begin.vi as is usually done in which case you will just call for the drive reference here instead. Bottom-left to the while loop, there is also the controller initialization Vi that will define the default command and create the notifier used to pass commands around. Because of the use of type definitions, you will likely not have to modify this VI.
Within the while loop, the current command on the shift register is looked at and the case structure is used to define the new speeds for the arcade drive on the right of the case structure. The green Vi on the right of the case structure is the Check for New Command.vi. This VI is part of the framework and you will likely not have to change it. It has three input parameters:
- The current command
- A Boolean flag whether the current command is finished or not
- A timeout numeric specifying how long to wait for the next command
If the Boolean flag is true, it will return the default command (here called reserve). If the Boolean is false, it will wait for a new command or until the time specified by the timeout parameter has passed. If a new command is received, it is returned and will interrupt the current one, if no command is received, the current command is returned.
The final case structure is just here to create a rolling log of the 20 previous commands executed.
Finally, let’s have a closer look at the command execution. The immediate command is simple:
It just forwards the left and right speed from the command cluster to the arcade drive.vi. The 2 constants at the bottom are indicators whether the command is finished or not and the time out for waiting for another command. In this case, the Boolean is true telling the Check for new command VI that it should return the default command, which is the reserve case:
The reserve case will send a -1 timeout to the Check for new command which means that it will wait for a new command indefinitely. Until a new command is received, the subsystem will be dormant and not use any resources.
The Drive For Time command has the following case:
The shift register does include a time stamp of when a new command was received. In the drive for time, the time difference between the “now” and the “command received” time is compared to the desired duration. If the required time has elapsed, the command is deemed finished which will cause the Check for new command VI to return the default command, if not, the Check for new command VI will wait for the timeout and return the same command. Here the timeout is set to 50ms or the difference between the elapsed time and the desired duration, whichever is smaller. The main reason to do this is to prevent the safety of the drive (the feature that stop the motors if they do not receive an instruction for a given time) to kick in if it is enabled. Otherwise, you could likely use the duration converted to milliseconds as timeout.
The stop command is actually implemented as an immediate command with 0 for both speed so it does not need its own case.
Now that we know how the behavior for each command has been defined, we need to have Vis to call the commands. The simplest one is the Stop Driving.vi:
The green VI with the old telephone icon is a command helper provided by the framework. You do not need to modify it; it will send the command to the controller using the notifier; this is all already coded for you. The description input is an optional string that could be used for logging and debugging. The command Info in and out are optional to and are used mainly when you want to send several commands in a row and keep them timed to a schedule. This is very useful for autonomous programming (see below).
The Drive immediate command is basically the same:
The only difference is that the left and right speed are not constants but controls and that there is a test to prevent driving while the joystick is near zero.
The Drive For Time command has a couple of Vis in addition to the helper.
The first one does create a notifier to know when the command has ended. The second one waits for this notifier before allowing the dataflow to resume if the Synchronization is set to “wait”. The other option is “start” which allow using the same command in a non blocking fashion (this is the default value).
Here is how all this is implemented in the Teleop.vi
In this part of the VI, the system checks for the state of the driving joystick (define in Begin.vi). The values of the axis are sent to the Drive Immediate.vi which triggers the immediate command. In addition, if button 1 is pressed the Drive for Time.vi is used to turn right for 5 seconds (this is an arcade drive and the first parameter (1) is left speed, the second (-1) right speed and the last one (5) time). If the second button is pressed, then the Drive For time.vi is used to send the command to turn left for 10 seconds.
The Buttons Pressed.vi is just using a feedback node to be sure to detect a change in the status of the button instead of the current state to prevent sending the command several times. This is important and it is recommended that the Teleop.vi loops in 20ms or less to avoid missing packets from the driver station, so it is likely that 2 successive operations will see the buttons being pressed.
Another part of the example code is in the autonomous mode. The command based framework makes the code extremely simple. Note that in this case the command Info cluster flows from one VI to the next to fix the order of the command and that the Synchronization option is set to “Wait” to prevent the next command from interrupting the previous one.
If you want to add a new system, simply right click on the roboRIO target and select New>subsystem:
This will display a new dialog box where you need to enter the name of the new subsystem, the color that will be use as background for the associated icons and the operations for the new subsystem:
It is worth noting that by default the immediate and reserve operation are created so there is no need to list them here.