Week 8: Tools and Processes + Autonomous

This week we'll dive into some miscellaneous topics that are useful for the season. We'll also cover some of the more advanced topics of FRC programming, like autonomous. We'll also spend time this week hands on with the robots.

FRC Driver Station

(Windows-only)

The FRC Driver Station is the main interface for the robot. It is used to control the robot, view the robot's status, and view the robot's telemetry.

Main uses:

• See if the robot is connected.

• See the robot's battery level.

• See and set the robot's mode (teleop, autonomous, etc.).

• Enable and disable the software on the robot, and see status of deployments.

• Setup controllers.

• Read logs from the robot.

See: https://docs.wpilib.org/en/stable/docs/software/driverstation/driver-station.html Note: The Driver Station must be used to Enable/Disable the robot and runs on Windows PCs.

Hardware interaction

Use these tools to interact with the hardware on the robot directly. Useful for debugging and things like determining the CAN IDs of hardware components. You may need to use a USB cable to connect to the robot.

REV Hardware Client

See: https://docs.revrobotics.com/rev-hardware-client

Phoenix Tuner X (from CTRE)

See: https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/index.html

AdvantageScope

We use AdvantageScope, along with our AdvantageKit code, to view live information from the robot, tune our control systems, and debug hardware and software issues. It is a very powerful tool.

See: https://docs.wpilib.org/en/stable/docs/software/dashboards/advantagescope.html

Running a Simulation

1. In VS Code, open the WPILib menu and select “Simulate Robot Code”.

2. When prompted, choose the Sim GUI.

3. In the Sim GUI, add “keyboard 0” to “joystick 0” and set mode to Teleop. You may need to adjust keyboard settings to emulate a joystick. You can also use a controller.

4. Open AdvantageScope, File → “Connect to Simulator”. Visualize your IO, and use the 3D field widget to see Drivebase Pose.

The Sim GUI is fine for sanity checks; AdvantageScope is preferred for deeper tuning/visualization during development.

FRC roboRIO Imaging Tool

Use for updating the software on the roboRIO. It should come installed with the NI Game Tools.

Elastic, other dashboards

We may use these during competition to see robot status during matches.

See: https://docs.wpilib.org/en/stable/docs/software/dashboards/elastic.html

Vision Processing

When we navigate the world, we have our eyes and the parts of our brain that process that visual input to help us. This is also the case for our robots—we have a camera connected to a processor that lets us use visual input in our code.

We primarily have two uses for vision: target detection and pose estimation. The first usually involves some sort of visual fiducial marker (something that can act as a known point of reference to find your current location) near the target that we can align to and then score, which is relatively easy to set up. The second involves a more complicated system of identifying where certain visual fiducial markers are relative to the robot and then using that to generate an estimated pose of the robot on the field.

AprilTags

AprilTags are a system of visual tags developed by researchers at the University of Michigan to provide low overhead, high accuracy localization for many different applications.

They look similar to QR codes, but with less pixels. This trades the amount of information in each tag for quick, easy, and robust detection. There are several different "families" of tags, which have different sizes, shapes, and patterns. These provide tradeoffs between detail, robustness, and number of potential IDs. Always check the current season manual for the tag family in use. In 2023, the FRC game Charged Up used 16h5, while in 2024 36h11 was used instead. In each frame in the camera stream, we are looking for these tags. Through a series of image processing algorithms (removing colors, decimation, identifying regions of relative lightness/darkness), we're able to identify both which tag it is (a numerical ID corresponding to the unique pattern of the tag) and where the corners of the tag are in the frame.

In the context of FRC, because AprilTags are placed in various known locations around the field, we can use them in order for the robot to know where it is on the field. The simplest application of this is single tag detection and 2D alignment. For example, we might know that when a certain tag is in the middle of the camera's field of vision, our robot's shooter is lined up correctly with the target. We can then write code that instructs the shooter to release a game piece once that tag has been centered, the drivetrain to adjust so that the tag is centered, etc.

Pose Estimation

We can also use AprilTags in a more sophisticated way to perform pose estimation. Once a tag and its corner locations have been detected, we can compare that to the known pose of that tag on the field and calculate an estimate of the distance, angle, etc. that the camera is from that tag. We know where the camera is relative to the rest of the robot, so we can then estimate where the robot is on the field. We'll also combine this estimate with the estimated pose returned by the wheels and gyro of the robot, which is known as odometry. This is useful because with a reliable pose estimation system, we can automate more of the scoring process. For example, in 2024 we were able to develop an accurate auto aim system based on the robot's estimated pose.

Pose estimation is more accurate when there are multiple tags in frame. Each tag generates its own estimate, which can then be combined and used or discarded if one is inaccurate.

How would an estimate be incorrect? One cause of this is pose ambiguity.

Humans are typically able to rely on things like depth perception and lighting to determine the difference between something like these two boxes, but it is more difficult for computers. If we also have pose estimates from other tags that are less ambiguous, we can throw out those (likely) incorrect estimates if they're significantly off. Having multiple cameras also helps with this, as another camera at a different angle could generate a more accurate pose with the same tag. Comparing a vision pose against odometry can also rule out incorrect poses.

There are several different strategies to pick which pose we want to use. One of these is lowest ambiguity, which finds the pose corresponding to the vision result with the lowest reprojection error (a measurement of the difference between a measured 3D point and its projected 2D point in the camera frame). This is useful when we only have one tag in frame that we can use to differentiate between these possible poses. Another is to choose the pose closest to some other known pose, such as the camera's pose or the last calculated pose. Historically in FRC, it's been unlikely that a robot would be above the ground or outside the field, so this can eliminate such poses that are further than the robot could have traveled in the time since the last measurement. Lastly, the best way to take advantage of multiple tags is by using the multitag PNP (Perspective-n-Point) strategy. This solves some equations to get a robot pose based on all the poses that we calculate from each tag and "averages" it out.

An important step to take before using 3D pose estimation is to correctly calibrate the camera. All camera lenses have some level of distortion that we need to account for when processing images gathered from them, especially in applications where it's important to accurately calculate where things are in 3D space.

It's possible for us to undistort these images—we just need to find the camera-specific numbers that will allow us to perform that math, which we can do by calibrating the camera. There are a couple ways to do this, but one way is to take a lot of photos with the camera being calibrated of a chessboard pattern (which we know has straight lines and certain measurements) at different angles and positions and send those photos to programs (such as PhotonVision or mrcal) that then handle the math for us and return those numbers.

Object Detection

Sometimes we want to be able to detect other things on the field that aren't AprilTags. In the past, FRC games have included retroreflective tape on various game elements, which is made out of the same shiny material on safety vests and road medians. This primarily includes game pieces, but can include other robots or more. One way of accomplishing this is through tuning HSV (hue, saturation, and value, which is a way of defining colors) thresholds to only pick up on a certain color, such as the orange of a Crescendo note.

Another way is contour filtering, which looks for the outlines of a shape like a circle or a triangle, such as a cone from Charged Up.

A more complicated system is through machine learning models that have been specially trained on the objects we want to detect, such as game pieces or other robots. Due to the significant processing power that this requires, in FRC this is typically done on the Rockchip Neural Processing Unit (NPU), which is on the Orange Pi 5 and similar coprocessors, or a Google Coral. While we haven't implemented this, the greater FRC community has developed several models and other resources. PhotonVision has also shipped ML-based game piece detection recently, and capabilities evolve—check current docs.

PhotonVision

PhotonVision is a community, open source library that gets vision data from the robot for a variety of applications. It simplifies interfacing with the camera and also has plenty of utility methods to get information like distance and position, as well as having some built in pose estimator features and more.

Limelight

Limelight (LL) is a camera and processor system for FRC designed to make vision easy and simple. We used a Limelight 2+ with Limelight's software in 2022 for retroreflective tape detection, and a LL2+ with PhotonVision software in 2023 for AprilTag detection.

Limelight has recently released the LL3, which possesses more processing power for AprilTags and object detection, and other teams have had success with it. The LL3G also features a global shutter camera for further improved AprilTag performance. At time of writing we have not tested either.

The LL2+ was not satisfactory for AprilTag detection for us, so we switched to the...

Orange Pi + Arducam

While we have the roboRIO to run much of our code on the robot, it is not capable of handling vision processing at a usable speed. Instead, we run this on a coprocessor, which is a second, faster computer also onboard the robot. For vision, we've been using the Orange Pi 5, a single board computer which is quite similar to Raspberry Pis you may have seen elsewhere. This is what we run PhotonVision on.

Of course, we also need a camera. We've been using Arducam USB cameras (specifically OV9281s and OV2311s), which have worked well, though other USB cameras also work. Generally, a reasonably high resolution, high speed, global shutter camera is best for FRC applications. Some test data of other frequently used cameras is linked below in the Resources section.