Uncategorized

Command-Based Programs with RobotBuilder

Posted on by Keith Rieck

How to create a command-based robot program? There are two ways: start with the Command Robot template or use RobotBuilder.

RobotBuilder is a tool provided with WPILib for constructing robot programs conforming to the command-based application architecture. This is a tool you use when first start constructing your program. It also provides round trip engineering, which means you can switch back and forth between RobotBuilder and Visual Studio Code. RobotBuilder has a number of advantages over using the Command Robot template built into Visual Studio Code:

  • RobotBuilder forces a top-down approach design approach. You first map out the high-level structures before diving into specific code.
  • It generates a correct command-based structure. When building from the template, there is some risk of getting the concepts wrong.
  • RobotBuilder is fast, so you watch the construction of a full program over just an hour. This would allow every programmer to write their own program for experimentation. It becomes plausible to create multiple robot programs and throw away the ones that aren’t prefect.

RobotBuilder just creates the infrastructure, so there will still be plenty of work for the programmers. It gets you started. By the end of the season, most of your code will be non-RobotBuilder code.

The following discusses deploying in Java, but C++ robot programs can be constructed in the same way.

Starting a RobotBuilder Project

You can start RobotBuilder from within Visual Studio Code. Open the Command Palette menu by hitting control-shift-P (or command-shift-P on a Macintosh). Select “WPILib: Start Tool”. A new menu of all the WPILib tools will appear. Select “RobotBuilder”. If you have previously used RobotBuilder, it will open directly into your most recent project. If this is your first time, you will get a dialog asking your project name and team number.

When you first start a project, I recommend you do the following steps first:

  1. Click on the Export Directory. A file dialog will pop up to define which folder your project will be saved into. Specify that you want to “Use Absolute Path”. Select your Documents folder and hit the “Select Folder” button.
  2. Under the Export menu, select Java. This will cause a new project folder to be created in your Documents folder.
  3. Under the File menu, select Save. A file dialog will pop up. Navigate to your documents Directory and then into your new project folder. Save your RobotBuilder config file as “robot.yml”.
  4. Click on the Export Directory again. Change the dialog to “Use path relative”, and then select the Documents directory again.
  5. Click on Wiring File Location, which pops up another file dialog. Navigate to your Documents directory, select your project folder, and click the “Select Folder” button.
  6. Under the File menu, select Save again.

The above procedure will cause all file paths relative to the RobotBuilder save file. This will allow the project to work properly even if it is copied to another computer, or is cloned from git.

Naming Conventions

When you name things within RobotBuilder, you will be creating class names and variables in your code. You’ll want to pick names that work with your language’s naming conventions.

  • Subsystems will be classes, so they should start with a capital letter. It’s a good practice if the name ends in the word “Subsystem”. For example, ClimberSubsystem or ShooterSubsystem.
  • Commands will also be classes, so they should also start with a capital letter. A good practice is to end all command names with the word “Command” and begin command names with the primary subsystem. For example, ClimberUpCommand or ShooterShootCommand.
  • Other components within RobotBuilder will be variables, so they should start with a lower case letter. RobotBuilder will allow you to put spaces in names, but this is a bad practice; don’t do it. Make variables clear about what they represent. For example, leftClimberSolenoid or shooterMotor or forwardRangefinder.

Subsystems

Suppose that your new robot has a subsystem for shooting balls using a spinning wheel. This subsystem has a SparkMax motor controller attached to PWM channel 1. The subsystem also has a pneumatic piston for pushing balls into the wheel. The piston is operated by a double solenoid connected to channels 6 and 7 of a CTRE pneumatics control module. To create a subsystem class in code:

  1. Right-click on the Subsystems folder and select “Add Subsystem”. This will create a new folder under Subsytems.
  2. Click on the new folder to select it. Change its name to ShooterSubsystem.
  3. Right-click on the new ShooterSubsystem folder and select “Add Actuators” and then “Add Motor Controller”. This will create a new icon for the motor controller.
  4. Click on the new motor controller icon. Change its name to shooterWheelMotor. Change the motor controller type to PWMSparkMax and then set its output channel to 1.
  5. Right-click on the new ShooterSubsystem folder again and select “Add Pneumatics” and then “Add Double Solenoid”.
  6. Click on the new solenoid icon. Change its name to ballPusherSolenoid. Set forward channel to 6 and its reverse channel to 7. Make sure the module type is CTREPCM.
  7. Under the File menu select “Save”. Under Export menu select “Java”.

At this point you have a command-based robot program with one subsystem. You can go back to Visual Studio Code and open the new folder for your project. Take a look at the new ShooterSubsystem class. You’ll see private variables pointing to the motor controller and the solenoid.

At the bottom of the ShooterSubsystem class, add these three methods:

    public void setWheelSpeed(double speed) {
        shooterWheelMotor.set(speed);
    }

    public void pushBall() {
        ballPusherSolenoid.set(DoubleSolenoid.Value.kForward);
    }

    public void retractPiston() {
        ballPusherSolenoid.set(DoubleSolenoid.Value.kReverse);
    }

These three methods will allow outside access to the private components inside ShooterSubsystem.

Commands

Go back to RobotBuilder. Create a method for setting the spinning wheel speed.

  1. Right-click on the Commands folder and select “Add Command”.
  2. Click on the new command icon. Change the command’s name to “ShooterSpinCommand”.
  3. Specify that this new command requires the ShooterSubsystem.
  4. Add a new parameter named “speed” of type “double”.
  5. Save everything and then export java code again.

Go back into Visual Studio Code and open the new ShooterSpinCommand class. Change the isFinished method so it always returns true. Then change the initialize method so it looks like this:

    @Override
    public void initialize() {
        m_shooterSubsystem.setWheelSpeed(m_speed);
    }

Next in RobotBuilder, create a command to extend the ball pusher piston:

  1. Right-click on the Commands folder and select “Add Command”.
  2. Click on the new command icon. Change the command’s name to “ShooterPushCommand”.
  3. Specify that this new command requires the ShooterSubsystem.

Now, repeat these three steps for a ShooterRetractCommand. Save and Export.

Switch to Visual Studio Code and open the two new commands. For both of them, the isFinished method should return true. For each of them, modify the initialize method to call the appropriate method within the ShooterSubsystem.

Operator Interfaces

Now we can create buttons to start the commands:

  1. Right-click on the Operator Interface folder and select “Add Xbox Controller”. Click on the new icon and rename it to just “xbox”.
  2. Right-click on the new “xbox” folder and choose “Add Xbox Button”. Click on the new button icon and rename it to “shooterPushButton”. Specify button “Y” and then specify the ShooterPushCommand.
  3. Add another XBox Button. Click on the new button icon and rename it to “shooterRetractButton”. Specify button “A” and then specify the ShooterRetractCommand.
  4. Add another XBox Button. Click on the new button icon and rename it to “shooterSpinButton”. Specify button “B” and then specify the ShooterSpinCommand. Click on the parameter area and specify that the speed should be 1.0.
  5. Add another XBox Button. Click on the new button icon and rename it to “shooterStopButton”. Specify button “X” and then specify the ShooterSpinCommand. You don’t have to specify a speed parameter for this button, since the default value is zero.
  6. Save and Export code.

You can return to Visual Studio Code and examine all the button setup within the RobotContainer class.

Cleanup of RobotBuilder code

There are a couple of frustrations you will encounter using RobotBuilder.

  • The code is often not indented correctly. You should use the auto formatter in Visual Studio Code to correct this.
  • The code is filled with messy comments which help implement the round trip engineering.
  • The round trip engineering will usually break down as programmers add their own classes, or delete classes or rename things.

Although RobotBuilder will get you started, you probably won’t use it for the full season. So, a good practice is to clean up the code after a week or two of development. Auto format each class. Delete all the messy comments. Clean out the unused import statements. Make it beautiful, because code should be easy to read.

RobotBuilder Extensions

One problem you might have already noticed is that RobotBuilder might not contain all the components you want to use on your robot. In particular, you might notice that motor controls from REV Robotics or CTRE are missing. Fortunately RobotBuilder allows extensions to be added for missing parts. These extension files should be put into a folder called RobotBuilder/extensions under your wpilib directory.

One source of extensions is at https://github.com/firstmncsa/Robotbuilder-Extensions.git. Clone this repository and the drag the files into your extensions folder.

You may also need to add 3rd party extension files into the vendordeps folder of your project. You can do this from with Visual Studio Code by opening the Command Palette and selecting Manage Vendor Libraries / Install New Libraries Offline. The vendor library URLs change every year, but for 2023 they are:

Further Reading:

0
Uncategorized

Command-Based Robot Architecture

Posted on by Keith Rieck

It is possible to write simple robot programs in a single Robot class, using the simple TimedRobot template. However, as your program grows, the complexity can snowball. You’re better off if your overall program is designed to be scalable. Scalability is an engineering concept where we observe how problems and solutions change as they grow. You want your program to scale gracefully as it adds functions. You may also want to be able to scale up the number of programmers, so multiple people can work comfortably on separate parts of the code.

A recommended pattern for organizing robot programs for FIRST is the command-based framework.

Command Based Framework

When creating a command-based robot program, we define three types of software:

  • Subsystem classes represent major physical parts of your robot, such as a drive train, shooter, game-piece collector, or climber.
  • Command classes represent major actions that subsystems can do, such as driving, shooting, acquiring, or climbing.
  • Operator interface objects start commands executing on the subsystems. Think of joystick buttons, or custom trigger objects.

For a command-based program these bits of software will work within a standardized software project. Within this framework there is another object called the CommandScheduler, which will handle the lifecycle of commands. The CommandScheduler monitors buttons and triggers. It starts and manages the commands.

At the top level of your project will be three important class files:

  • Main, which starts the whole program. Don’t make any changes to this file.
  • RobotContainer, which is in charge of creating the objects from the subsystem and command classes, and connecting them with operator interfaces.
  • Robot, which handles the lifecycle timing. It will initiate robot initialization calls into the RobotContainer class. It also handles transitions in and out of teleop and autonomous modes, and causes the CommandScheduler to execute.

Separating robot functions out into these three classes is an example of Separation of Concerns, a software engineering principle in which we divide different functions into different areas.

Subsystems

Subsystems represent physical parts of your robot. Think about the physical components that you may manipulate from your program. A subsystem might contain:

  • Motors, and their associated motor controllers and sometimes encoders
  • Pneumatic pistons, and their associated solenoids
  • Servos
  • Relays or Spikes
  • Gyros or Inertial Management Units
  • Cameras
  • Sensors, including ultrasonic sensors, limit switches or potentiometers

Each subsystem class will contain software objects connecting to those physical objects. For instance, if a drive train subsystem has two physical motors, then the DriveTrainSubsystem class will contain two motor controller objects. Typically we make those objects private, meaning that they may not be accessed directly from the outside.

To allow access to the internal components, we next define functions in the subsystem that allow component access. For instance, we might add a function that sets the speed of a shooter motor or another function that causes pneumatic solenoids to push a ball into the shooter. One important quality of these functions is that each one must be quick, only taking a tiny amount of time.

The practice of making internals private but then exposing high-level actions through functions is another software engineering principal called Ecapsulation. Encapsulation hides internal complexity, allowing the overall program to be less complex.

Commands

Commands execute actions on subsystems. Most commands operate only on a single subsystem, but it is possible to run a command on multiple subsystems. When a command operates on a subsystem, we say that the command requires that subsystem. There are three important rules for commands:

  1. A subsystem can only run one command at a time.
  2. If a subsystem is running one command but then another command starts on that subsystem, the first command will be interrupted. We call this action interrupting, but the first command will be completely stopped.
    For instance, suppose the robot is turning clockwise, but then you initiate a command to turn counter-clockwise. It is impossible to do both at the same time, and the second command is what you really want. The clockwise command will be stopped and the counter-clockwise command will start up.
  3. A subsystem may have a default command that runs whenever no other commands are scheduled.
    For instance, a drivetrain subsystem typically has a default driving command that allows joysticks to drive the robot. If we initiate a command to rotate the robot ninety degrees clockwise, then the driving command will be interrupted until the rotate command finishes. The default command will start up again as soon as the turn command is finished.

Some commands will be “instant commands”, meaning that they initiate some simple action and then end immediately. For instance, a command to set the speed on a shooter wheel will do just that, and then end.

Other commands have a lifecycle that can span a much longer period of time. These command have an initialization function when they start, and then an execution function that is run repeatedly until the command is finished, and then a finalization function to run when the command is finished. Each of these functions must be quick and time a tiny amount of time, but the overall life of the command may take many seconds.

Note that you can also combine commands into command groups. One command group can cause multiple commands to run in sequence or in parallel.

Operator Interfaces

The most common way of initiating a command is to attach it to a joystick button. Pressing that button schedules the command and it will start running. You can also cause a command to start when the button is released, or to execute only while the button is held down. You can allow a button to toggle on or off. You can also define combinations of buttons, so commands start when you press multiple buttons. There is a lot of flexibility available.

You can also define custom trigger objects that initiate commands. You could define triggers that start commands whenever certain conditions become true, such as when an ultrasonic sensor detects a wall or when voltage drops on an analog input or when a certain time is reached.

Note that autonomous routines are typically created as group commands. The autonomous command will be scheduled whenever the robot initiates autonomous mode.

Designing Subsystems and Commands

When it is time to divide your robot into subsystems, the choices might seem obvious, but there are a couple of principles to think about. Remember that a subsystem can only run one command at a time. If it seems that you will need to run multiple commands on one area, consider splitting those parts into multiple subsystems. For instance, if you have a subsystem that acquires balls, stores them, and then shoots them, you might want to break that part into two or three separate subsystems. On the other hand, if it seems like two subsystems are always doing the same thing at the same time, then maybe they should be combined. For instance, it might not be optimal to create separate subsystems for the left and right sides of your drive train.

The number of subsystems is usually fixed, but you can have a great many commands. You can keep adding commands as you think of new functions. Don’t worry if you create a lot of commands that aren’t ultimately used in the final robot program. Commands are an area for innovation.

After you’ve learned how basic commands work, read the documentation on all specialized command types, such as the InstantCommand, ConditionalCommand, CommandGroup, etc. After you’ve mastered these heavy-weight commands, you learn light-weight lambda based commands.

Naming Conventions

Most programming languages develop standards on how things should be named. In Java, the long established practice is:

  • Class names start with a capital letter, but then separate words with capital letters. This is called camel case.
  • Variables start with a lower case letter, and then proceed with camel case.
  • Constants, which are variables that are never changed, are all capital letters with words separated by underscores.

When choosing names, don’t be afraid to make long multi-word names that make meaning more clear. For robot programs, you might adopt the following conventions:

  • Subsystem classes should end in the word “Subsystem”. For instance ShooterSubsystem or ClimberSubsystem.
  • Commands classes should end in the word “Command” and begin with the name of the command’s main subystem. For instance, ClimberUpCommand or ClimberDownCommand.

Further Reading:

0
Uncategorized

2022 Week 1 CSA Notes

Posted on by Keith Rieck

As of this writing:

  • The Driver Station must show version 22.0 or later in the title bar.
    The latest DS is part of the FRC Game Tools that can be downloaded from National Instruments.
  • RoboRIO must be imaged to 2022_v4.0 or later.
    You can see the image version inside the Driver Station on the Diagnostics Tab.
    The newest image and Imaging software are also in the National Instruments Game Tools.
  • The latest version of WPILib and Visual Studio Code for Java and C++ developers is 2022.4.1. The latest version can always be downloaded from the WPILib Github Release page. This release works with the latest RoboRIO image version, so you’ll need to update everything.
  • The GradleRIO version will be 2022.4.1 in your build.gradle file. The Visual Studio Code plugin should automatically offer to update your GradleRIO version.
  • Teams should go through all their components to make sure the firmware is up to date.
    RevRobotics components should be updated with the REV Hardware Client.
    CTRE components should be updated with the Phoenix Tuner.
  • The 2022 Inspection Checklist is available online.

If you are headed to a competition, please update all your software before the event.

Notes from Duluth

The 2022 Northern Lights Regional and Lake Superior Regional were both held at the Duluth DECC. Pit areas for both were in the same large room, which made it easier for CSAs to help out teams from both regionals.

  • As usual, the most common problem was with teams whose RIO image, Driver Station, or GradleRIO were not up to date. Usually updating the RIO image would then require that WPILib and VS Code would also need an update.
  • Imaging the new RIO 2s typically required that the SD chip be popped out and then imaged with Etcher. After the SD chip was written, the imaging tool must be used to set the team number.
  • I saw fewer issues with cameras than in past events. There were a few calls for LImeLight questions, which were handled by CSAs that are LimeLight experts.
  • There were a couple problems caused by metal shaving shorting out PWM pins.
0
Tutorial

Unit Testing Commands

Posted on by Keith Rieck

The WPILib command framework divides your robot program into two types of classes: subsystems and commands.  The subsystem classes represent the major physical parts of the robot, such as a shooter subsystem, a drive-train subsystem, or a manipulator arm subsystem.  The command classes define the actions taken by the subsystems, such as shooting a ball, moving the drive-train, or raising the manipulator arm.

unit_test_commands

Most of your programming time will go into creating, refining and debugging new commands.  Commands will be the most sophisticated part of your code.  Therefore they also have the greatest risk of going wrong.  Therefore you should spend a lot of time testing your commands.

So far we have tested simple functions and verified the primitive functionality in subsystems.  The next step is to created automated tests for your commands.

Testing a simple Command

Our simple example robot contains a Shooter subsystem that shoots balls.  The ShooterSubsystem has a high-speed wheel for throwing the ball, and a servo arm that can raise the ball up until it touches the wheel.  We will need a command to set the wheel speed, and another to control the servo arm.

A simple Command

Here is the command to raise or lower the servo arm:

package frc.robot.commands;

import edu.wpi.first.wpilibj.experimental.command.*;
import frc.robot.subsystems.*;

public class ShooterServoArmCommand extends SendableCommandBase {

  private final boolean fire;
  private final ShooterSubsystem shooter;

  public ShooterServoArmCommand(boolean fireArm, ShooterSubsystem shooterSubsystem) {
    fire = fireArm;
    shooter = shooterSubsystem;
    addRequirements(shooter);
  }

  @Override
  public void execute() {
    if (fire) {
      shooter.fire();
    } else {
      shooter.retract();
    }
  }

  @Override
  public boolean isFinished() {
    return true;
  }
}

Take note of the two parameters on the constructor:  fireArm and shooterSubsystem.   This command can either raise the arm or lower it, depending on whether the fireArm parameter is true or false.

By specifying the shooterSubsytem in the constructor we are using Dependency Injection, which makes the code more reusable and more testable.  When testing, we can replace the real subsystems with mock objects that fake the subsystem’s functionality.

A simple Test

Our task does two different things: retract and fire. First let’s test that firing the ball works:

package frc.robot.commands;

import edu.wpi.first.wpilibj.experimental.command.*;
import frc.robot.subsystems.*;
import org.junit.*;

import static org.mockito.Mockito.mock;
import static org.mockito.Mockito.verify;

public class ShooterServoArmCommandTest {

    private CommandScheduler scheduler = null;

    @Before
    public void setup() {
        scheduler = CommandScheduler.getInstance();
    }

    @Test
    public void testFireArm() {
        // Arrange
        ShooterSubsystem shooter = mock(ShooterSubsystem.class);
        ShooterServoArmCommand fireCommand 
                = new ShooterServoArmCommand(true, shooter);

        // Act
        scheduler.schedule(fireCommand);
        scheduler.run();

        // Assert
        verify(shooter).fire();
    }
}

The test follows our Arrange / Act / Assert pattern:

  • We create a mock version of our ShooterSubsystem.  If we wanted, we could also define some mock behaviors at this point.
    We create the actual command we will test.  In this case we set the fireArm parameter to true, indicating that we want to fire the ball.
  • In the command framework, we never explicitly execute the command methods.  Instead, we “put it on the schedule”.   After this, the command scheduler will run the methods appropriately.  On a real robot, the scheduler tries to run all scheduled commands every 20 milliseconds.
    In this case we know that  our command will only run once before it’s done.
  • At the end of the test, we ask the mock framework to verify that the shooter’s “fire” command was called exactly once.

Unit tests will all execute whenever we build the code.  Go ahead and execute the “Build Robot Code” action within Visual Studio code.  Next write a similar test to verify that the command also correctly retracts the servo arm:

@Test
public void testRetractArm() {
    // Arrange
    ShooterSubsystem shooter = mock(ShooterSubsystem.class);
    ShooterServoArmCommand retractCommand = new ShooterServoArmCommand(false, shooter);

    // Act
    scheduler.schedule(retractCommand);
    scheduler.run();

    // Assert
    verify(shooter).retract();
}

Testing a Command Group

Simple commands can be grouped together to run sequentially or in parallel as more complicated commands.

A more complex Command

For instance, actually shooting a ball is a sequence of steps:

package frc.robot.commands;

import edu.wpi.first.wpilibj.experimental.command.*;
import frc.robot.subsystems.*;

public class AutoShootCommand extends SequentialCommandGroup {
    public AutoShootCommand(ShooterSubsystem shooter) {
        super(
                new PrintCommand("BEGIN: AutoShootCommand"),
                new ShooterServoArmCommand(false, shooter),
                new ShooterSetSpeedCommand(1.0, shooter),
                new WaitCommand(0.5),
                new ShooterServoArmCommand(true, shooter),
                new WaitCommand(0.5),
                new ShooterSetSpeedCommand(0.0, shooter),
                new ShooterServoArmCommand(false, shooter),
                new PrintCommand("END: AutoShootCommand")
        );
    }
}

Note that we are again using dependency injection, but that the same ShooterSubsystem will be used in all the internal commands.

Besides the shooter commands, we’ve also thrown in a couple of PrintCommands.  These commands print out to the console at the beginning and end of the command.  They also print to the Log File Viewer to be reviewed after a match.

Also we’ve thrown in a couple of WaitCommands, which give the shooter wheel half a second to spin up before shooting and then maintain speed while the ball is firing.

Testing a Command Group

A command group test follows the same pattern as simpler tests:

package frc.robot.commands;

import static org.junit.Assert.*;
import static org.mockito.Mockito.*;

import org.junit.*;

import edu.wpi.first.wpilibj.experimental.command.CommandScheduler;
import frc.robot.subsystems.ShooterSubsystem;

public class AutoShootCommandTest {

    private CommandScheduler scheduler = null;

    @Before
    public void setup() {
        scheduler = CommandScheduler.getInstance();
    }

    @Test
    public void testShoot() throws InterruptedException {
        // Arrange
        ShooterSubsystem shooter = mock(ShooterSubsystem.class);
        AutoShootCommand command = new AutoShootCommand(shooter);

        // Act
        scheduler.schedule(command);
        for (int i=0; i<100; i++) {
            scheduler.run();
            Thread.sleep(20);
        }

        // Assert
        verify(shooter, times(2)).retract();
        verify(shooter, times(1)).fire();
        verify(shooter).setSpeed(1.0);
        verify(shooter).setSpeed(0.0);
    }
}

This command takes many run cycles, so run it many times, pausing 20 milliseconds between each execution.

After executing everything in the command group, we verify that the subsystem experienced all the actions for shooting.

Writing quality tests

It’s important to remember why we do unit testing.: we create suites of automated tests to improve the quality of our software.  Writing quality tests is a big subject and these last three articles have covered a lot of ground.  It would be easy to be overwhelmed, or in fact dubious, with all of this.  So keep your eye on the end goal:  software quality.

In a sense, writing methodical tests is a stepping stone from just programming into Software Engineering. Engineering means using systematic and disciplined practices when creating things.  Your tests will verify and quantify your software quality, in way that others can read and evaluate.

Further Reading

0
Tutorial

Unit Testing Subsystems

Posted on by Keith Rieck

Testing is an element of any software development, and certainly it’s a big part of robot programming.  You’ve probably already done a lot of robot testing; deploy your code and test the robot.  Hopefully you’re already familiar with the idea of unit testing of small functions, but we can also automate the testing of whole subsystems.

Unit testing with WPILib

To demonstrate automated testing of robot subsystems, we’ll use a simplified robot program.  This program runs on a real robot build for the 2016 game, FIRST Stronghold.

A simple subsystem

In the WPILib command pattern a subsystem class represents a physical subset of the robot.  A subsystem contains physical components, such as motors and sensors.  There will be actions to perform on the subsystem, such as to drive or shoot.  For this example, we have a simple robot with two subsystems representing the robot chassis with its drive motors, and a shooter for throwing balls.

unit_test_subsystems.png

Mostly we’re going to work on testing the ShooterSubsystem.  The shooter has two components: a motor attached to a spinner wheel  and an arm attached to a servo that manipulates the ball.  To shoot a ball we will:

  1. Retract the servo arm so we can pick up a ball.
  2. Start the shooter wheel spinning.
  3. Extend the servo arm so the ball is pushed into the wheel.  The ball will go flying.
  4. Reset the system.  The wheel will be stopped and the servo retracted.

(Shooter Picture)

Here’s the code for the shooter subsystem:

package frc.robot.subsystems;

import static frc.robot.Constants.*;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.experimental.command.*;

public class ShooterSubsystem extends SendableSubsystemBase {

    protected final SpeedController shooterMotor;
    protected final Servo shooterServo;
    protected boolean servoRetracted = true;

    public ShooterSubsystem(SpeedController motor, Servo servo) {
        shooterMotor = motor;
        shooterServo = servo;
    }

    public void setSpeed(double speed) {
        shooterMotor.set(speed);
    }

    public void retract() {
        shooterServo.set(SHOOTER_SERVO_MIN);
        servoRetracted = true;
    }

    public void fire() {
        shooterServo.set(SHOOTER_SERVO_MAX);
        servoRetracted = false;
    }

    public void reset() {
        setSpeed(0.0);
        retract();
    }
}

Note that the constructor takes two parameters as inputs: motor and servo. The motor and servo objects will be created elsewhere and then injected when the subsystem is constructed.

Mock testing with WPILib

The best way to do testing is with the full robot; load your code and go through a methodical test process.  Too often however, we don’t have sufficient access to the robot.  Maybe it hasn’t been built at all, or maybe it is shared with our teammates.  How can we test the code without access to the robot?  The answer is that we can test much of the logic with “mock” components.  Mocks are software objects that stand in for the real classes.  Instead of real motors, servos, and sensors, we’ll use mock motors, mock servos, and mock sensors.

We will use the Mockito framework to create mock SpeedControllers and mock Servos.   Mockito is a professional package for creating Java mocks, defining the mock behavior and checking the results.

To use Mockito, you’ll need to make two simple changes to your build.gradle file.

    1. Change the value of the includeDesktopSupport variable to true.
    2. Add the following line into the dependencies section: testCompile"org.mockito:mockito-core:2.+" .

unit_test_mockito

A simple unit test

Add a “test” directory under “src” for your java unit tests.  Right-click on “src”, select “New Folder” and enter “test/java/frc/robot/subsystems”.  Right-click on “subsystems” and select “create an empty class” named “ShooterSubsystemTest.java”

unit_test_test2.png

Now we can create a test of the subsystem’s constructor:

package frc.robot.subsystems;

import static org.junit.Assert.*;
import static org.mockito.Mockito.*;
import edu.wpi.first.wpilibj.*;
import org.junit.*;

public class ShooterSubsystemTest {

    @Test
    public void testConstructor() {
       // Arrange
        SpeedController motor = mock(SpeedController.class);
        Servo servo = mock(Servo.class);

        // Act
        ShooterSubsystem shooter = new ShooterSubsystem(motor, servo);

        // Assert
        assertEquals(true, shooter.servoRetracted);

    }
}

In this test we first create mock objects for the motor and the servo.  The action we are testing is just to create the shooter object.  After performing the action, we verify that the servo is retracted.

Note that the test is broken into sections.  The Arrange / Act / Assert breakdown is a common pattern for designing tests.  Sometimes we’ll add some extra sections, but most tests will have the basic three parts.

You could argue that this test is a little superficial, and you’d be right. However, this test does serve a purpose. If at some later date someone changed the subsystem so it didn’t initially retract the server, then this test would fail.   We would then need to decide whether the code or the test has become incorrect.

Another unit test

Next let’s write a test for the setSpeed method.  This method sets the speed of the motor.  After it has been executed, the motor controller will have a different speed:

@Test
public void testSetSpeed() {
    // Arrange
    SpeedController motor = mock(SpeedController.class);
    Servo servo = mock(Servo.class);
    ShooterSubsystem shooter = new ShooterSubsystem(motor, servo);

    when(motor.get()).thenReturn(0.5);

    // Act
    shooter.setSpeed(0.5);

    // Assert
    assertEquals(0.5, shooter.shooterMotor.get(), 0.001);
}

First we set up the mock objects and the shooter subsystem. This time we tweak the mock motor a little, specifying that when we get the motor’s speed, then it will return 0.5. The action is to set the speed. Afterwards we check that the speed was really set (and specifying a margin of error of 0.001).

As your tests get more sophisticated, you’ll use the “when” method to add more mock behavior to your mock objects.

The code above is another fairly superficial test, but it does exercise the code and the mock objects.  Let’s consider more features of the mock framework:

Yet another unit test

Let’s test the “reset” method of our subsystem.  In this case we want to verify that the motor has really been stopped and the servo arm has been retracted.

@Test
public void testReset() {
    // Arrange
    SpeedController motor = mock(SpeedController.class);
    Servo servo = mock(Servo.class);
    ShooterSubsystem shooter = new ShooterSubsystem(motor, servo);

    // Act
    shooter.reset();

    // Assert
    assertEquals(true, shooter.servoRetracted);
    verify(motor).set(0.0);
    verify(servo).set(SHOOTER_SERVO_MIN);
}

This time there are more lines of code in the “Assert” section.  Besides verifying that the server arm was retracted, we also run two verifications on the mock objects.

The “when” and “verify” features of mock objects are allow some sophisticated tests.  You may see your tests growing with many fiddly mock behaviors.  This is usually OK.  Just make your tests as simple as possible, but no simpler.

Dependency injection

Our ShooterSubsystem depends on two objects created elsewhere, a servo and a motor speed controller.  Those dependent objects are specified in our subsystems constructor.   This pattern is called Dependency Injection.  The tests described above wouldn’t be possible if we weren’t able to inject mock objects into our system-under-test.

Dependency Injection is an important concept within software engineering.  Besides encouraging testability, it supports the concept of Separation of Concerns. This means that we often break a large program into sections that each handle different concerns.  In this case we have one class that handles creation and definition of physical components (typically a RobotMap or RobotTemplate class) and another class that defines the behavior and interaction between those components (our subsystem).

Further Reading

0
Tutorial

Simple Unit Tests

Posted on by Keith Rieck

Every programmer has at one time deployed code without having tested it.  Simple changes go out with the assumption that they can not possibly fail. And then they betray us.  We learn the lesson:  all code must be tested, thoroughly and repeatedly.

On robots we often need the hardware to do some of the testing, but there are still a lot of tests that can be executed offline.  Ideally, you should build up suites of tests that execute automatically; just start one test program and all tests execute.  There are many categories of automated tests, but the most common is called unit testing, because they test small units of functionality, including the functions you assumed can’t fail.

Well crafted unit tests will improve the quality of your software and insure its quality down the road.  You may choose to organize your development around those tests, a practice called Test Driven Development.  Unit tests are also essential to refactoring, which is a systematic technique for improving your code;  you’ll need automated test to verify that your refactored code still works correctly.

Unit testing with WPILib

GradleRIO is already set up for the unit testing frameworks JUnit and GoogleTest.   If you define unit test classes in your project, they will automatically execute every time you build the code.

Let’s define a simple function and create unit tests for it.  Don’t worry that this code looks too simple to merit testing.   Remember that no code is so trivial that it won’t fail.

A simple function

Suppose you’ve got a Gyro installed on your robot.  When you first start it up, the gyro will show 0 degrees.  Rotate the robot a little to the right and it might read 30 degrees.  However, the gyro’s scale is continuous, so after a lot of driving around it might read 1537 degrees or -2781 degrees.  This might mess up the math in some of your autonomous commands, since 1537 degrees is really the same as 97 degrees.  We need a function that simplifies angles into the range -180 to 180.  Here are some test cases:

  • 270 degree is the same as -90 degrees
  • -315 degrees is the same as 45 degrees
  • 30 degrees is still 30 degrees
  • -60 degrees is still -60 degrees

Here’s a simple conversion function.  It definitely isn’t perfect, but we’ll fix that in a minute:

public int simplifyAngle(int angle) {
    if (angle > 180) {
        angle = angle - 360;
    }
    if (angle < -180) {
        angle = angle + 360;
    }
    return angle;
}

For this example, this function is in your Robot class which is stored with the other java main classes in your “src” directory:

unit_test_func1

A simple unit test

Add a “test” directory under “src” for your java unit tests.  Right-click on “src”, select “New Folder” and enter “test/java/frc/robot”.  Right-click on “robot” and select create an empty class named “RobotTest.java”

unit_test_test1.png

Consider the test method:

@Test
public void testSimplifyAngle() {
    Robot robot = newRobot();
    assertEquals(-90, robot.simplifyAngle(270));
    assertEquals(-45, robot.simplifyAngle(315));
    assertEquals(-60, robot.simplifyAngle(-60));
    assertEquals(30, robot.simplifyAngle(30));
}

The @Test annotation on top means that this method will be executed by the GradleRIO test task.  We create a Robot object and then test our method for each of the test cases.

This test class will execute every time you build robot code.  If any of the assertions fail, the whole build will be rejected. To see what happens on a failure, temporarily change the 30 degree test so it expects -30 degrees. The build will fail and tell you to check line 15:

unit_test_fail1

Improving the function

How many test cases should you use?  Usually more than you would expect, even for simple functions.

Always include a trivial test case, sometimes called the “happy path” case. The 30 degree and -60 degree test might be considered happy path tests, but we could also test 0 degrees.  Add some test scenarios where there are logical transitions; these are called “corner cases”.  For this example, corner tests might be at 180 degrees and -180 degrees.  Also test a couple extreme cases, such as 1537 degrees and -2781 degrees.  Extreme tests at absolute maximums or minimums are called “edge cases”.

Now our test looks like this:

@Test
public void testSimplifyAngle() {
    Robot robot=new Robot();
    assertEquals(-90, robot.simplifyAngle(270));
    assertEquals(-45, robot.simplifyAngle(315));
    assertEquals(-60, robot.simplifyAngle(-60));
    assertEquals(30, robot.simplifyAngle(30));
    assertEquals(0, robot.simplifyAngle(0));
    assertEquals(180, robot.simplifyAngle(180));
    assertEquals(-180, robot.simplifyAngle(-180));
    assertEquals(97, robot.simplifyAngle(1537));
    assertEquals(99, robot.simplifyAngle(-2781));
}

Executing this test reveals that our function fails for the extreme cases.  Our function can’t handle 1537 degrees.  We’ve found a bug in our logic.   We go back to the original function and, after a little thought,  change it to the following:

public int simplifyAngle(int angle) {
    while (angle > 180) {
        angle = angle - 360;
    }
    while (angle < -180) {
        angle = angle + 360;
    }
    return angle;
}
Now our test passes.  The bug is fixed.

Refactoring

At some point, you or one of your teammates will rewrite parts of the robot code, at which point you must retest and verify that the new code is at least as good as the old.  For instance, someone might refactor the angle simplification like this:

public int simplifyAngle(int angle) {
    return angle > 180 
    ? simplifyAngle(angle - 360) 
    : angle < -180 ? simplifyAngle(angle + 360) : angle;
}

Does this function do the same job?  It turns out that it does. Is this function better? Well, it is shorter, but you should decide if it’s really more readable.

Eventually, you might stumble on logic like this:

public int simplifyAngle(int angle) {
    return  (int)Math.round(Math.IEEEremainder(angle,360.0));
}

This is even shorter.  It’s much more cryptic, but it does pass the tests.  You could use any of these functions in your robot.  Unit tests have verified that they all do the same thing.

Writing good tests

Now that you know how to create unit tests, start adding them to your robot projects. You will find that writing good tests is as difficult and subtle a skill as programming the robot code.  You should start watching for opportunities to test.  Break up your big methods into smaller methods and structure them so they are more amenable to testing.  Test the simple things, but especially watch for code that is tricky.

It’s probably possible to write too many tests, but don’t worry about that.  On professional projects the test suites are often larger than the baseline code.

Good unit tests should have the following qualities:

  1. Test the requirements and nothing but requirements.  In the above example we require that 270 degrees is simplified down to -90 degrees.  However, don’t try to craft tests that verify the number of times the “while” loop executes to achieve this.
  2. Tests should be deterministic and always succeed or fail based on the requirements.  Take care around code that depends on hardware or file systems or random functions or timers or large memory usage.  Structure your code so you can manage any randomness.
  3. Unit tests should be fast.  They execute before every build and you don’t want to start regretting how slow they are.
  4. Tests should be easy to read, understand, and maintain later

The above example is intentionally simple.  Once you’ve mastered the concepts you can start to think about automated testing of larger classes, non-trivial state machines,  subsystems and commands.

Further Reading

0
Uncategorized

FRC 2019 – Camera Best Practices

Posted on by Keith Rieck

To get the most out of your cameras for the FRC 2019, please consider following these recommendations. This document does not contain the theory for the recommendations. If the theory is desired or for any questions regarding these recommendations, please contact a MN CSA at firstmn.csa@gmail.com or http://firstmncsa.slack.com.

Desired goals that drive these recommendations

  • Low latency
    • Allows driver to react to the most current status with a minimal delay between driver input and robot action cycle time.
  • Low bandwidth usage
    • Reduced risk of driver input being delayed due to high bandwidth.
      • There is a Quality of Service mechanism that should prevent this, but to fully eliminate the risk, reduce bandwidth if possible.
    • Bandwidth target is below 3/Mbs
  • Ease of use

Possible Misconceptions

  • Higher FPS means lower latency.
    • While higher FPS can appear to reduce latency in a video game, that only occurs when the underlying infrastructure can support the desired FPS with minimal latency to begin with.
    • Low latency is a function of the infrastructure’s ability to get data from point a, the camera, to point b, the DS screen, with minimal delays. This can only occur if that infrastructure has available waiting capacity to process, transmit and display the video.
    • Higher FPS can easily overload the underlying infrastructure, which can cause delays at every stage of the point a to point b pipeline, thus increasing the overall latency.
    • Lowering FPS to a level which the infrastructure can handle the pipeline while still maintaining available waiting capacity, will assist in achieving the lowest possible latency.
  • High Resolution is better
    • High resolution is desirable if additional detail allows for a strategic advantage, but for most tasks, lower latency will offer a much better robot control experience.
    • 640×480 is not twice as much as 320×240. It is 4 times as much. The extra time required to process, transmit and display 4 times the data is most likely not going to offset the higher latency and reduce capacity required for its use.
  • This or that device is the right one for all tasks.
    • Not all devices work well in all situations, you should balance the total cost to implement, maintain and configure additional devices before making changes. Cost in this sense means monetary, time, expertise, weight, etc…

Driver Cam

  • Use FRCVision on Raspberry PI instead of cameras hosted on roboRIO
  • URL: https://wpilib.screenstepslive.com/s/currentCS/m/85074/l/1027241-using-the-raspberry-pi-for-frc
  • Benefits:
    • Potential for robot code to respond faster to driver input by offloading CPU intensive task from roboRIO.
    • Lower video latency and higher frame rates due to increased cpu cycles available on Pi.
    • Ability to handle more concurrent streams than a roboRIO.
    • Ability to control stream from FRC shuffleboard and LabView Dashboard.
    • Ability to control Resolution, FPS and compression per camera feed.
    • Ability to have a per camera vision processing pipeline.
    • Multiple language choices for vision processing pipeline.
    • No need to add code for basic camera streaming.
  • Recommended Usage:
    • Driver video streaming.
    • Video processing, target acquisition and tracking.
  • Recommended Equipment:
    • Raspberry Pi 3 B or B+, B+ preferred.
    • Microsoft Lifecam HD-3000
    • Logitech c920, c930, c270, c310
    • Any Linux UVC  supported USB camera that supports MJPEG and desired resolution and fps in camera hardware: http://www.ideasonboard.org/uvc/#devices
  • Optional Equipment:
  • Recommended hardware settings, per camera.
    • Format: MJPEG
    • Resolution: 320×240
    • FPS: 15-20, reduce as needed to reduce Pi cpu usage.
  • Recommended stream settings, per camera
    • Format: MJPEG
    • Resolution: 320×240
    • FPS: 10-15, reduce as needed to lower Pi cpu usage or bandwidth
    • Compression: 30, adjust as needed to get desired cpu usage, bandwidth and clarity.
  • Considerations:
    • Power:, ensure 2.5+ amps of power are available to the Pi, especially if using 3-4 cameras and / or vision processing pipeline is in use.
    • Actual FPS per video stream as listed in the DS view should match set FPS for the stream as configured for that camera, if it does not, lower FPS and / or Resolution or increase compression until actual FPS and set FPS match and video quality and latency is acceptable.
          • Rather than using driver mode, create a “driver” pipeline. Turn down the exposure to reduce stream bandwidth.
          • Using a USB camera? Use the “stream” NT key to enable picture-in-picture mode. This will dramatically reduce stream bandwidth.
          • Turn the stream rate to “low” in the settings page if streaming isn’t critical for driving.
        • Considerations:
          • Do NOT use for driver vision.
          • Use only for target acquisition and tracking.
          • Stream only output of vision pipeline to DS and only if bandwidth allows.

Chris Roadfeldt

0
Uncategorized

2019 Week 2 CSA Notes

Posted on by Keith Rieck

Duluth Double DECCer

In Minnesota, the week 2 events were the Lake Superior Regional and the Northern Lights Regional, both held in the Duluth Entertainment Convention Center (the DECC).

Our observations include:

  • All roboRIOs had to be re-imaged to version FRC_roboRIO_2019_v14.  This update was released after stop-build day, so every bagged robot had to be updated.
    If you haven’t yet attended your first 2019 competition, you can prepare for this by updating your laptops with the FRC Update 2019.2.0.
    If you are helping teams at competition with this, it might be a little quicker to give them the FRC_roboRIO_2019_v14 file and reimage their RIO.
  • All Java and C++ projects had to be updated to GradleRIO version 2019.4.1.  GradleRIO version changes always require inital software downloads, so the first build after changing your version must be done while connected to the internet.  It’s far better to do this before the competition, while you have a good network connection.
    If you are helping teams at the competition, you can give them the latest WPILib update.  This update will install all the latest GradleRIO dependencies, minimizing download time.
  • We were expecting camera problems for LabView.  At Duluth, Brandon and Logon did extra duty for all the Labview teams.
  • Two teams who had programmed their robot in Eclipse with the 2018 version of WPILib.
    Fortunately, this was easy to fix.  We installed the latest WPILib to their laptops and then used the import wizard to convert their projects to GradleRIO.
  • As usual, plenty of teams suffered from loose electrical connections.
    Pull-test all your connections; nothing should come loose.  All the wires to your batteries, circuit breaker, and PDP should be completely immovable.
  • If using the Limelight camera, consider their bandwidth best practices.
  • If you are using a Limelight and / or frcvision on Raspberry PI, consider bringing an ethernet switch in order to assist troubleshooting.
  • Turn off the firewall on your laptops.
0
Uncategorized

Loop time override warnings

Posted on by Keith Rieck

An important message from Omar Zrien from CTR Electronics came out this weekend.  It addresses some warning messages that teams have been reporting:

  • Watchdog not fed within 0.020000s (see warning below).
  • Loop time of 0.02s overrun.

Anyone who uses CTRE’s components should read all of Omar’s posting, but relevant takeaways are:

  • Install the latest Phoenix firmware and add the corresponding Phoenix.json to your project.
  • Keep an eye on the number of get*, set*, and config* calls you make, since each call might consume a little processor time.
  • Don’t worry too much about the overrun warnings as long as your robot is performing.

 

0
Uncategorized

Labiew Dashboard Camera Fixes

Posted on by Keith Rieck

2019 Camera report

Lake Superior Regional and Northern Lights Regional (Duluth, Minnesota)

The following is a report from the Duluth CSA’s on cameras and the dashboard. As of Saturday Afternoon (Week 2) we have experienced 100% success rate of cameras performing between the 123 teams split between the 2 regionals. Our procedure to get this result will be outlined below.

If the team camera works, we let them go without any changes. This usually included the Limelight, Raspberry Pi, ShuffleBoard, and SmartDashboard. These presented very little problems with the FMS and NT.

For teams encountering issues, LabVIEW teams or teams using a LabVIEW dashboard, the following procedure was done in the pits. If the team was able to connect after any of these steps tethered to the robot we sent them out to the field.

In the Pits:

1. Download the 2019.2.1 LabVIEW Dashboard.

This would get passed on by a flash drive to the team’s driver station from a CSA. The folder would be placed on the desktop with the following path :

C:\Users\pcibam\Desktop\FRC_Dashboard\Dashboard.exe

2. If LabVIEW team, convert their code to a fresh 2019.2 Project as follows. All projects were named 2019 Duluth Cameras so we could determine which teams we applied this to. Always save when prompted to during this conversion.

a) Start a brand new project like normal

lv_cam_1_new

 

b) Delete the following from the begin VI. Once cleared it will look as follows. (Old Code)

lv_cam_2

Cleaned (Old Code)

lv_cam_3_cleaned

 

c) Copy everything except the following from begin. (Old Code) and paste in New Code

lv_cam_4_paste_in_new

 

d) Delete the following from Teleop. (New Code)

lv_cam_5_delete_from_teleop

Cleaned

lv_cam_6_cleaned

 

e) Copy everything except the following from Teleop (Old Code) and paste in New code

lv_cam_6a_cam_copy

 

 

f) Delete the following from Periodic Task (New Code)

lv_cam_7_delete_from_periodic

Cleaned

lv_cam_8_cleaned

 

g) Copy everything except the following from Periodic Task (Old Code) and paste in New code

lv_cam_8_copy_except

 

h) Delete the following from Autonomous Independent (New Code)

lv_cam_9_delete_from_auton

Cleaned

lv_cam_10_cleaned

 

i) Copy everything except the following from Autonomous Independent (Old Code) and paste in New code

lv_cam_11

We have not discovered what to do with robot global variables at this time. To be on the safe side teams should recreate these in the new project and link them to the appropriate locations manually.

 

On the field:

Check if NT Communication Light is on

lv_cam_12_dashboard

Once that light is green do the following:

  • If one camera, select the camera and wait. On average it would take about 7 seconds to connect.
  • If 2 cameras, select the second camera first. Let this camera boot up. Then select the first camera. It does not matter which side that cameras are on. Each camera on average would take 7 seconds.

If you have any question feel free to contact me at the information below. I hope this helps for future events! We will be doing the same procedure at the Great Northern Regional (North Dakota) and will report back with results from that regional.

 

Brandon A. Moe

University of Minnesota – Duluth, 2020 Minnesota CSA
FRC Team 7432 NOS Mentor

Personal: moexx399@d.umn.edu

0