Robot Software
FRC 2

Prerequisites: RT 1
Corequisites: CS 1, CT 1

Disclaimer

While the goal every year is to create the most competitive robot possible with the tools and abstractions we've built up so far, we want everyone to understand the entire robot's code base each year. Therefore, reusing complicated code from previous year's robots is off-limits unless everyone understands how it works internally (if it breaks, they'll know how to attempt fixing it). Writing documentation or tutorials/labs is encouraged to help other students understand pieces of software quicker in the future.

Installation

Eclipse should already be installed from following the bootstrap page. For robot software development, follow the instructions here to install the FRC plugins and roboRIO ARM toolchain. Other robot software resources can be found here.

Design Patterns

Design patterns are used all over software development. Expert programmers know the idiomatic design patterns for their language to solve a problem quickly, efficiently, and with few bugs. We have common design patterns in robot software. A list of design patterns with descriptions, usage advice, and examples is provided below.

See this WPILib ScreenSteps page for more resources on performing various common robot tasks in C++. We have robot software from previous years on GitHub that students can study, understand, and potentially reuse.

SampleRobot control flow

The SampleRobot robot base class provides several virtual functions the subclass can override (see SampleRobot.h).

The functions Disabled(), Autonomous(), and OperatorControl() correspond to the modes in which the Driver Station can set the robot. They are called once upon switching to each mode. After one of the functions exits, RobotBase will continuously wait for Driver Station data until another mode change occurs. Therefore, code in each function should be run in a while loop until the mode changes. The following is an example Robot.cpp.

#include "Robot.hpp"

using namespace std::chrono_literals;

void Robot::Autonomous() {
    while (IsAutonomous() && IsEnabled()) {
        // Perform autonomous tasks

        std::this_thread::sleep_for(10ms);
    }
}

void Robot::Disabled() {
    while (IsDisabled()) {
        // Wait for robot to become enabled

        std::this_thread::sleep_for(10ms);
    }
}

void Robot::OperatorControl() {
    while (IsOperatorControl() && IsEnabled()) {
        // Perform normal robot actions and check for user input

        std::this_thread::sleep_for(10ms);
    }
}

Drivetrain with joysticks

There are multiple ways to drive a robot around; some are in open loop (the driver provides direct input to the system) and others are in closed loop (autonomous driving using encoders). This section discusses the former while the latter is covered in detail in CT 2.

The two most common ways to drive a robot manually are with WPILib's RobotDrive class and with another drive class implementing a custom drive scheme (e.g., Cheesy Drive). To make a four-wheeled robot drive around as quick as possible, the following is all that is required.

CANTalon flMotor(0);
CANTalon frMotor(1);
CANTalon rlMotor(2);
CANTalon rrMotor(3);
RobotDrive drive(flMotor, frMotor, rlMotor, rrMotor);
Joystick forwardJoystick(0);
Joystick turnJoystick(1);

while (IsOperatorControl() && IsEnabled()) {
    drive.ArcadeDrive(forwardJoystick.GetX(), turnJoystick.GetY());

    std::this_thread::sleep_for(10ms);
}

While it is easy to use, a creating custom drive scheme allows for more complex functionality and features. Alternatively, WPILib may simply not support your drive train configuration. To implement a custom drive scheme, the following is required.

The following is an example of one.

#include <cmath>

void DriveTrain::Drive(double moveValue, double rotateValue) {
    if (moveValue > 1.0) {
        moveValue = 1.0;
    } else if (moveValue < -1.0) {
        moveValue = -1.0;
    }
    if (rotateValue > 1.0) {
        rotateValue = 1.0;
    } else if (rotateValue < -1.0) {
        rotateValue = -1.0;
    }

    // Local variables to hold the computed PWM values for the motors
    double leftMotorOutput;
    double rightMotorOutput;

    if (moveValue > 0.0) {
        if (rotateValue > 0.0) {
            leftMotorOutput = moveValue - rotateValue;
            rightMotorOutput = std::max(moveValue, rotateValue);
        } else {
            leftMotorOutput = std::max(moveValue, -rotateValue);
            rightMotorOutput = moveValue + rotateValue;
        }
    } else {
        if (rotateValue > 0.0) {
            leftMotorOutput = -std::max(-moveValue, rotateValue);
            rightMotorOutput = moveValue + rotateValue;
        } else {
            leftMotorOutput = moveValue - rotateValue;
            rightMotorOutput = -std::max(-moveValue, -rotateValue);
        }
    }

    SetLeftRightMotorOutputs(leftMotorOutput, rightMotorOutput);
}

Cheesy Drive is a popular method of driving robots that maintains a constant turning radius at different linear speeds. See here for an implementation. The following explains each part of it.


This function takes a throttle value, a turn value, and a boolean representing whether the center of rotation should be within the robot's chassis.

void DriveTrain::Drive(double throttle, double turn, bool isQuickTurn) {

This simply inverts the throttle since pushing forward on the joystick produces a negative value.

  throttle *= -1;

    // Limit values to [-1 .. 1]
    throttle = Limit(throttle, 1.0);
    turn = Limit(turn, 1.0);

This applies a deadband around the center of the range to compensate for joysticks which give non-zero values for the origin.

    /* Apply joystick deadband
     * (Negate turn since joystick X-axis is reversed)
     */
    throttle = ApplyDeadband(throttle, m_deadband);
    turn = ApplyDeadband(turn, m_deadband);

    double turnNonLinearity = k_turnNonLinearity;

    /* Apply a sine function that's scaled to make turning sensitivity feel
     * better. turnNonLinearity should never be zero, but can be close.
     */
    turn = std::sin(M_PI / 2.0 * turnNonLinearity * turn) /
           std::sin(M_PI / 2.0 * turnNonLinearity);

    double angularPower = 0.0;
    double linearPower = throttle;
    double leftPwm = linearPower, rightPwm = linearPower;

Negative inertia applies a rotation in the direction opposite of rotation when the driver requests a large change in the turning rate. This overcomes the moment of inertia of the robot and makes the robot more responsive. Names prefixed with "k_" are constants.

    double negInertia = turn - m_oldTurn;
    m_oldTurn = turn;

    // Negative inertia!
    double negInertiaScalar;
    if (turn * negInertia > 0) {
        negInertiaScalar = k_inertiaDampen;
    } else {
        if (std::fabs(turn) > 0.65) {
            negInertiaScalar = k_inertiaHighTurn;
        } else {
            negInertiaScalar = k_inertiaLowTurn;
        }
    }

    m_negInertiaAccumulator +=
        negInertia * negInertiaScalar;  // adds negInertiaPower

    // Apply negative inertia
    turn += m_negInertiaAccumulator;
    if (m_negInertiaAccumulator > 1) {
        m_negInertiaAccumulator -= 1;
    } else if (m_negInertiaAccumulator < -1) {
        m_negInertiaAccumulator += 1;
    } else {
        m_negInertiaAccumulator = 0;
    }

QuickTurn allows a turning radius within the robot. The quick stop accumulator performs a similar function to negative inertia by opposing the robot's rotation when the driver requests the turn to stop (the joystick turn value is close to zero). This makes the robot's turns stiffer.

    // QuickTurn!
    if (isQuickTurn) {
        if (std::fabs(linearPower) < 0.2) {
            double alpha = 0.1;
            m_quickStopAccumulator = (1 - alpha) * m_quickStopAccumulator +
                                     alpha * Limit(turn, 1.0) * 5;
        }

        angularPower = turn;
    } else {
        angularPower =
            std::fabs(throttle) * turn * m_sensitivity - m_quickStopAccumulator;
        if (m_quickStopAccumulator > 1) {
            m_quickStopAccumulator -= 1;
        } else if (m_quickStopAccumulator < -1) {
            m_quickStopAccumulator += 1;
        } else {
            m_quickStopAccumulator = 0.0;
        }
    }

This performs calculations to ensure the robot performs the same radius turn at all forward speeds.

    // Adjust straight path for turn
    leftPwm += angularPower;
    rightPwm -= angularPower;

    // Limit PWM bounds to [-1..1]
    if (leftPwm > 1.0) {
        // If overpowered turning enabled
        if (isQuickTurn) {
            rightPwm -= (leftPwm - 1.0);
        }

        leftPwm = 1.0;
    } else if (rightPwm > 1.0) {
        // If overpowered turning enabled
        if (isQuickTurn) {
            leftPwm -= (rightPwm - 1.0);
        }

        rightPwm = 1.0;
    } else if (leftPwm < -1.0) {
        // If overpowered turning enabled
        if (isQuickTurn) {
            rightPwm += (-leftPwm - 1.0);
        }

        leftPwm = -1.0;
    } else if (rightPwm < -1.0) {
        // If overpowered turning enabled
        if (isQuickTurn) {
            leftPwm += (-rightPwm - 1.0);
        }

        rightPwm = -1.0;
    }

This sets the motor speeds to those that were calculated.

    m_leftGrbx.Set(leftPwm);
    m_rightGrbx.Set(rightPwm);
}

ButtonTracker internals

In OperatorControl(), performing actions when the user presses a button is typically required. The simplest way to do this uses a Joystick and two booleans for tracking the past and current state of one of its buttons.

Joystick joystick(0);
bool oldButtonState = false;
bool newButtonState = false;

while (IsOperatorControl() && IsEnabled()) {
    // If button was pressed
    if (!oldButtonState && newButtonState) {
        std::cout << "Joystick button 2 pressed" << std::endl;
    }

    // If button was released
    if (oldButtonState && !newButtonState) {
        std::cout << "Joystick button 2 released" << std::endl;
    }

    // If button was held
    if (oldButtonState && newButtonState) {
        std::cout << "Joystick button 2 held" << std::endl;
    }

    // Update step
    oldButtonState = newButtonState;
    newButtonState = joystick.GetRawButton(2);

    std::this_thread::sleep_for(10ms);
}
    

First, the booleans are initialized to false to represent a released button. At the end of every while loop iteration, the current state is copied to the old state and a new one is obtained from the joystick. By performing this operation once every loop iteration, we are guaranteed to see every rising or trailing edge and can act on it.

Unfortunately, as more buttons are checked, this approach becomes messy and unmaintainable. Also, the possibility of mistyping a boolean check increases and encourages copy-pasting code. We wrote the ButtonTracker class to provide an abstraction for detecting leading and trailing edges in button state which eliminates boilerplate. See an example of its usage below.

ButtonTracker joystick2Buttons(2);

while (IsOperatorControl() && IsEnabled()) {
    if (joystick2Buttons.PressedButton(2)) {
        std::cout << "Joystick button 2 pressed" << std::endl;
    }

    if (joystick2Buttons.ReleasedButton(2)) {
        std::cout << "Joystick button 2 released" << std::endl;
    }

    if (joystick2Buttons.HeldButton(2)) {
        std::cout << "Joystick button 2 held" << std::endl;
    }

    // Update step
    joystick2Buttons.Update();

    std::this_thread::sleep_for(10ms);
}

To check other buttons, one has to simply pass a different button number to the appropriate member function. To check other joysticks, one has to simply make another instance of ButtonTracker and add it to the update step at the bottom of the while loop.

Toggle solenoid state

Toggling solenoids to extend or retract mechanisms can be done with the following pattern.

Solenoid solenoid(1);

solenoid.Set(!solenoid.Get());

The following is a more verbose way of doing the same thing.

Solenoid solenoid(1);

if (solenoid.Get()) {
    solenoid.Set(false);
} else {
    solenoid.Set(true);
}

Spin motor with joystick

This pattern is typically used to manually move mechanisms driven by a motor up or down.

Joystick joystick(0);
CANTalon motor(1);

motor.Set(-joystick.GetY());

There is a negative sign because GetY() returns negative values when the joystick is pushed forward.

State machines

See the state machine and event framework labs for how we implement state machines, since they are commonly used tools.

Actuate motor to several points quickly

This is done when a mechanism driven by a motor should be moved from one position to another reliably and quickly. Note that this should only be done when the device could potentially be set to more than two positions within its range. If only two are required, a solenoid should be used instead.

See the labs in CT 1 for examples on how to do this.

The following is the "Encoder" example provided in the 2017 WPILib Eclipse plugins.

#include <Encoder.h>
#include <SampleRobot.h>
#include <SmartDashboard/SmartDashboard.h>
#include <Timer.h>

/**
 * Sample program displaying the value of a quadrature encoder on the
 *   SmartDashboard.
 * Quadrature Encoders are digital sensors which can detect the amount the
 *   encoder has rotated since starting as well as the direction in which the
 *   encoder shaft is rotating. However, encoders can not tell you the absolute
 *   position of the encoder shaft (ie, it considers where it starts to be the
 *   zero position, no matter where it starts), and so can only tell you how
 *   much the encoder has rotated since starting.
 * Depending on the precision of an encoder, it will have fewer or greater
 *   ticks per revolution; the number of ticks per revolution will affect the
 *   conversion between ticks and distance, as specified by DistancePerPulse.
 * One of the most common uses of encoders is in the drivetrain, so that the
 *   distance that the robot drives can be precisely controlled during the
 *   autonomous mode.
 */
class Robot: public frc::SampleRobot {
    /**
     * The Encoder object is constructed with 4 parameters, the last two being
     *   optional.
     * The first two parameters (1, 2 in this case) refer to the ports on the
     *   roboRIO which the encoder uses. Because a quadrature encoder has
     *   two signal wires, the signal from two DIO ports on the roboRIO are used.
     * The third (optional) parameter is a boolean which defaults to false.
     *   If you set this parameter to true, the direction of the encoder will
     *   be reversed, in case it makes more sense mechanically.
     * The final (optional) parameter specifies encoding rate (k1X, k2X, or k4X)
     *   and defaults to k4X. Faster (k4X) encoding gives greater positional
     *   precision but more noise in the rate.
     */
    frc::Encoder m_encoder { 1, 2, false, Encoder::k4X };

    /**
     * Time to wait between updating SmartDashboard values.
     * It is generally a good idea to stick a short wait in these loops
     *   to avoid hogging CPU power, especially as there will be no
     *   perceivable difference in the SmartDashboard display.
     */
    static constexpr double kUpdatePeriod = 0.005; // 5milliseconds / 0.005 seconds.

public:
    Robot() {
        /* Defines the number of samples to average when determining the rate.
         * On a quadrature encoder, values range from 1-255; larger values
         *   result in smoother but potentially less accurate rates than lower
         *   values.
         */
        m_encoder.SetSamplesToAverage(5);

        /* Defines how far the mechanism attached to the encoder moves per pulse.
         * In this case, we assume that a 360 count encoder is directly attached
         * to a 3 inch diameter (1.5inch radius) wheel, and that we want to
         * measure distance in inches.
         */
        m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5);

        /* Defines the lowest rate at which the encoder will not be considered
         *   stopped, for the purposes of the GetStopped() method.
         * Units are in distance / second, where distance refers to the units
         *   of distance that you are using, in this case inches.
         */
        m_encoder.SetMinRate(1.0);
    }

    /**
     * Retrieve various information from the encoder and display it on the
     *   SmartDashboard.
     */
    void OperatorControl() {
        while (IsOperatorControl() && IsEnabled()) {
            // Retrieve the net displacement of the Encoder since the lsat Reset.
            frc::SmartDashboard::PutNumber("Encoder Distance",
                    m_encoder.GetDistance());

            // Retrieve the current rate of the encoder.
            frc::SmartDashboard::PutNumber("Encoder Rate", m_encoder.GetRate());

            frc::Wait(kUpdatePeriod); // Wait a short bit before updating again.
        }
    }
};

START_ROBOT_CLASS(Robot)

Project Setup

Each year, a new robot project will need to be created in Gerrit (our code review platform) and on GitHub for our competition robot code. First, an admin should create a new empty repository in the Team3512 GitHub organization (no README or license) and in Gerrit. Give them a name and description following the form of the previous years' repositories like "Robot-2017" and "The source code for the 2017 FRC robot.". Next, clone the repository from Gerrit and add the following files to it. See below the list for exceptions in downloading the provided files.

Generic Download

These files should be included in all of FRC Team 3512's software projects.

.gitreview will need the project name updated. COPYING will need the initial copyright year updated to the current year. README.md should be edited to be unique to the project.

Robot Download

These files should only be included in robot code projects. Some replace the generic files listed above.

README.md will need the year and robot name updated. "?" can be used until the robot name is chosen. src/Constants.hpp is a file we typically use every year and is provided as a placeholder so the src folder is added to Git history.

Generated via Robot Project

Change the following Eclipse project properties before copying the metadata files into the Git repository.

The optimization level can be restored to "None" with "Other optimization flags" of "-Og" and debug level can be restored to "Maximum" when needed for debugging. However, the settings described here are used for performance and should probably be used in competition.

Initial Commit

After the files are added to the repository, they should be committed in the first commit with the commit message "Initial Commit". This can then be pushed to Gerrit for review or pushed directly to master by admins. Gerrit will automatically mirror commits to master on GitHub.