Getting Started

This guide attempts to highlight the parts of the WPILib Documentation that are important or potentially unclear. The WPILib Documentation is the main source of information about FRC robot programming.

Programming a robot requires both software development skills and a good familiarity with the hardware connected to the robot. The integration of both the hardware and the software can make robot programming very challenging. It is NOT easy but don’t get discouraged.

Team 4698 competes in the FIRST Robotics Competition (FRC). FRC provides many software tools and programming libraries that are used to create the program that runs on the robot’s controller. It is possible to program using LabVIEW, Java, or C++. Our team writes the robot program in the C++ programming language.

As a first starting point, a basic understanding of C++ is necessary. If you don’t know C++ then you can follow the C++ Lessons which give topics and exercises to get you up to speed.

FRC Game Tools Install

The Game Tools and the WPI Library are already installed on the Team 4698 computers.

The FRC Game tools are the programs that handle the communication between the laptop and the robot. The processor on the robot that controls all of the robot functions is called a RoboRIO. The RoboRIO connects to the laptop in one of three ways: A USB cable, an ethernet cable, or wirelessly. The RoboRIO is connected to a wireless bridge that creates a WiFi network.

The main application is the Driver Station. It connects to the RoboRIO and can enable and disable the robot. It also handles the joystick inputs to the robot and displays statistics and logging information about the robot’s status.

The FRC Games Tools need to be installed before other components and libraries are installed. The directions for installing are: Installation instructions for the FRC Game Tools. You do not need to install anything for LabVIEW or Java since we do not use those programming languages.

WPILib Library Install

The software library that FRC uses in called WPILib. WPILib is very large and contains many elements including hardware interfacing, control systems, networking, and vision processing. The Microsoft VSCode compiler and Integrated Development Environment (IDE) are installed when the WPILib software is installed. This should be installed after the FRC Game Tools. Installation Instructions

Some of the motors and motor controllers require software libraries that don’t come in the WPILib libraries. We use hardware that require the Rev Robotics REVLib library and the CTRE Phoenix Framework. Information on installing these libraries.

If you are using your own computer then you can run the robot program in simulation mode without having access to the robot. See Simulator Installation

Test Bench

Team 4698 has a test bench that can be used to learn the programming concepts that are needed to write good programs on an actual robot. There are example problems that you can work through using the test bench. They will greatly help with understanding many of the concepts needed for good FRC robot programming.

The Test Bench Document

Hardware Overview

Read the WPILib sections on Hardware-Basics and Sensors, also check out the Hardware Tutorials

roboRIO

The roboRIO is the processor on the robot that executes the robot program. It can communicate with many different motors and sensors through its various inputs and outputs.

Motors

There are two types of motors used in FRC, brushed motors and brushless motors.

Brushed motors are very simple electrically and inexpensive but have "brushes" than slide on the rotation shaft of the motor and can wear faster than a brushless motor. They have much lower power-to-weight ratios than brushless motors as well. Brushed motors only have two wires, usually red and black. Reversing the polarity of the wires reverses the direction of the motor.

Brushless motors are much more electrically complex but mechanically simpler. Brushless motors have three wires that cannot be connected directly to a battery. Due to the complicated control needed for brushless motors they must have an encoder built into them to function correctly (See Encoders) and they must be connected to a brushless motor controller (sometimes called a "Smart Controller"). Having a built-in encoder makes brushless motors work with complex motion control without having to add an external encoder. The Falcon motor from CTRE has the controller and the encoder built into the motor. It is a complete control system module.

Table 1. Common Motor Specs and kV Values
Motor Max Speed (rpm) Power (W) kV (rpm/Volt) Encoder tics

Mini CIM

5,840 rpm

215 W

487 rpm/V

Brushed

NEO 550

11,000 rpm

279 W

917 rpm/V

42

CIM

5,330 rpm

337 W

444 rpm/V

Brushed

775pro

18,500 rpm

347 W

1542 rpm/V

Brushed

NEO

5676 rpm

540 W

473 rpm/V

42

Falcon 500

6380 rpm

783 W

532 rpm/V

2048

A more complete table is the ReCalc Motors Table

Motor Controllers

Motor controllers receive commands from the robot program and talk directly to the motors. Team 4698 uses TalonFX, TalonSRX, and SparkMAX motor controllers mainly. The TalonSRX can only control brushed motors while the SparkMAX can control brushed or brushless. The TalonFX is the built-in controller for Falcon and Kraken motors which are brushless with integrated encoders. The TalonSRX must use an external encoder whereas the SparkMAX can use a brushless motor’s built-in encoder or an external encoder. All three controllers have built-in PID control (see Smart Controller PID).

Encoders

See Encoders — Hardware and Encoders — Software

Encoders are devices that measure the rotation of a shaft. Usually encoders are either built into motors or added to the motor shaft but it is also possible to add an encoder to any rotating shaft. Encoders have a resolution which is specified in "tics" per rotation. The Falcon motor has a built-in encoder which has a resolution of 2048. That means that it can detect rotations of 360 / 2048 = 0.176 degrees.

Encoders are either "relative encoders" or "absolute encoders". Most encoders are relative which means that the encoder doesn’t know where the motor shaft is physically but only knows how far it has turned since it was powered on. An absolute encoder on the other hand knows where a zero position is in the rotation even when power is lost and restored.

Gyros / IMUs

Gyros measure the rotation around an axis by sensing inertial movement. They essentially detect their acceleration and integrate that to determine the angle of rotation. All but the simplest gyros are actually Inertial Measurement Units (IMUs) which measure rotation and acceleration around three orthogonal axes (3-axis gyro). They also usually have a magnetometer (i.e. compass) as well. Because an IMU has to communicate so much information to the robot, it is usually connected via CANbus or SPI.

Switches

Typical application of switches in robotics are limit switches. They are triggered when a moving part of the robot gets to the end point of it’s travel.

PWM

Pulse Width Modulation (PWM) is a way to send a varying signal (like an absolute encoder position) over a digital channel. See SparkFun PWM Page

AnalogIO

It is also possible to read (or output) an analog signal. An analog signal is one that can vary from 0V - 12V.

Robot Program

The Driver Station controls what part of the robot program is allowed to run and whether the motor can be activated. During a competition, control of the Driver Station is taken over by the competition system and they control what the robot can do.

There are three modes that the robot can be in: "TeleOperated", "Autonomous", and "Test". TeleOperated mode is when the driver can control all functions of the robot. Autonomous mode is when the robot code can run but it cannot take input from the Driver Station controllers. Test mode is for testing motors and sensors during setup and practice. A typical competition round will consist of some amount of time in Autonomous mode and then switch to TeleOperated mode for the rest of the round.

Robot Program Structure

When you write a program to control the robot you are actually just writing some subset of the program that is actually running on the RoboRIO. You may have noticed that your robot program doesn’t have a main() function. The WPILib is actually controlling the control flow of the program and calls your code at certain times during its execution. It basically gives you control every so often and you must do something while you have control and return control back without taking too much time.

WPILib provides two main ways to structure a robot program. One is called "TimedRobot" based and the other is "Command" based. Both program structures have methods that are called by the WPILib scheduler but when and how those methods are called differ between the two program structures. A TimedRobot program is given control at a fixed interval of time (20 milliseconds). A Command based program is only given control when some kind of condition is met (like a button was pressed on an Xbox controller). Command based programs are structured such that they force the programmer to segment their code into classes that represent they types of Actions that the robot does.

* See Creating a Robot Program

Units Library

The Units Library is very powerful and useful for robot programming. It is a bit difficult to learn and understand initially but it will help keep your code’s units consistent and avoid conversion errors.

See the FRC Units Library Documentation to get an overview.

One of the more useful aspects of using the Units Library is being able to define custom units that pertain to your robot code. One example is converting from motor revolutions to distance traveled for the robot drivetrain. If you have a gear box between the motor and the wheel that has a gear ratio of 6.86 to 1 and a wheel diameter of 4 inches then you could define a custom unit type of "meters per rotation" and then create a constant with those units that you can use to multiply desired linear velocities by to get motor angular velocities.

Custom Unit For Drivetrain
       // Gear ratio of the drive motors. 6.86 rotations of the drive motor is one rotation of the wheel.
    constexpr double kDriveGearRatio = 6.86;

      // Compound unit for the meter per revolution constant.
    using meters_per_rev = units::compound_unit<units::meters, units::inverse<units::turns>>;
    using meters_per_rev_t = units::unit_t<meters_per_rev> meters_per_rev_t;

      // The number of meters traveled per rotation of the drive motor
      // wheel circumference / gear ratio
    constexpr meters_per_rev_t kDriveMetersPerRotation = std::numbers::pi * 4_in / (kDriveGearRatio *  1_tr );

Note that the units library uses "turns" for rotations with the suffix "tr". Also notice that on the last line the wheel diameter is specified in inches but the units library automatically converts inches to meters.

The new Phoenix 6 library no longer uses "tics_per_100ms" units so the info below is not as relevant.

Another example is using the TalonFX smart motor controller library’s Set() function. It requires the position or velocity inputs in very awkward units (See [talon_pid_table]). The position should be in "encoder tics" and the velocity should be in "encoder tics" per 100 milliseconds. There are 2048 encoder tics per rotation for the TalonFX built-in encoder. Custom units can help with converting from these strange units to more physically meaningful units. You could define a custom angular position unit that is ("tics") and a custom angular velocity unit that is ("tics" / 100_ms).

Custom Unit for Encoder Tics
        // Create a unit called "tics" that represents 1/2048th of a revolution
        // and make a type qualifier called "tics_t"
    using tics = units::unit<std::ratio<1,2048>, units::turns>;
    using tics_t = units::unit_t<tics>;

        // Create a unit called "tics_per_100ms" that represents (tics / 0.1 seconds)
        // and make a type qualifier called "tics_per_100ms_t"
    using tics_per_100ms = units::compound_unit<tics, units::inverse<units::deciseconds>>;
    using tics_per_100ms_t = units::unit_t<tics_per_100ms>;

        // Alternatively "tics_per_100ms" could be defined as:
        // using tics_per_100ms = units::compound_unit<tics, units::inverse<
        //           units::unit<std::ratio<1,10>, units::seconds>>>;

Once these types are defined then the programmer doesn’t need to worry about converting from tics to degrees or from RPM to tics_per_100ms. The units types will do all the conversions automatically. The code below shows how to use these types.

Automatic Units Conversion
tics_t talon_position;
tics_per_100ms_t talon_velocity;
ctre::phoenix::motorcontrol::can::TalonFX talon{2};

    // This automatically converts from degrees to tics
talon_position = 45_deg;

    // value() returns the position in tics as a double
    // which is 256 tics ( 45 * 2048 / 360 )
talon.Set( ctre::phoenix::motorcontrol::ControlMode::Position,
            talon_position.value() );

    // This automatically converts from RPM to tics_per_100ms.
talon_velocity = 2400_rpm;

    // value() returns the velocity in tics_per_100ms as a double
    // which is 8192 tics_per_100ms ( 2400 * 2048 / 600 )
talon.Set( ctre::phoenix::motorcontrol::ControlMode::Velocity,
            talon_velocity.value() );

    // If you need to convert a variable in one unit to another
    // without creating a variable you can use:
printf( "Talon Velocity = %7.2f rpm\n",
        units::revolutions_per_minute_t(talon_velocity).value() );
    // This will print "Talon Velocity = 2400.00 rpm"

This choice could be a bit inconvienent as well since in order to print out the value of the talon_velocity or to send it to the Network Tables (See NetworkTables) in RPM you must use the syntax units::revolutions_per_minute_t(talon_velocity).value(). A better approach might be to create a class that encapsulates the mechanism that the motor is used in (like a shooter). Then create a member function in that class that sets the velocity of the motor and does the necessary conversion from RPM to tics_per_100ms. So for example if the TalonFX motor was connected to a flywheel that is used to shoot a ball then you might want to create a Shooter class that looks like:

Encapsulating Unit Conversion
class Shooter {
public:
    Shooter( const int canId ) : m_talon{canId} {}
    void SetVelocity( units::revolutions_per_minute_t rpm ) {
        m_talon.Set( ctre::phoenix::motorcontrol::ControlMode::Velocity,
                     rpm.value() * 2048.0 / 600.0 );
    }
    void Stop( void ) { m_talon.Set( ctre::phoenix::motorcontrol::TalonFXControlMode::PercentOutput, 0.0); }
private:
    ctre::phoenix::motorcontrol::can::TalonFX m_talon;
};

The Shooter class is then used in the main robot program and can be called with whatever angular velocity units you want (RPM, radians per second, etc) and it will convert them to the correct units for the Set() command inside Shooter::SetVelocity().

Motion Control

Suppose you want to control a flywheel that will be used to shoot a ball towards a target. Suppose also that you need the ball to be going just the right speed so that it hits the target correctly. The simplest thing that you could do is to put a certain voltage on the motor so the flywheel so it is going just the right speed to work. This method is called "open-loop control" because you actually don’t know how fast the motor is spinning but you adjust the voltage so it "just works". The problem with this method is that if the motor heats up, or the battery voltage drops, or the temperature outside changes then the speed of the flywheel will change. Then you have to change the voltage that is sent to the motor to make it shoot correctly under the new conditions. It will always need to be tweaked to work because we don’t know what the angular velocity of the flywheel is when the ball is shot.

The basic idea of motion control is that you use a motor to move something and then use some kind of sensor to measure what the motor is doing. The sensor provides "feedback" about what the motor is actually doing. In the shooter example above we would have an encoder that would measure the RPMs of the flywheel. We would need to determine what RPM the shooter flywheel needs to spin to have the ball hit the target correctly. Then we need to "make sure" that the flywheel is going the correct RPM when we want to shoot the ball.

We can calculate the difference between the current flywheel RPMs and the target RPMs as the rpm error. PID control uses this calculation of error to adjust the motor voltage in order to "make sure" the flywheel spins the desired (target) RPM. The way we "make sure" that our flywheel is going the correct RPM is using a technique called PID control. PID stands for Position Integral and Derivative.

PID Control Overview

WPILib has a good discussion of PID Control in:

Introduction to PID

The videos below by FRC 0 to Autonomous are really good at describing PID and showing the PID loop calculations. The IZone parameter that they implement is not a very good solution and in general it is best if you can avoid integral control.

Feed Forward

We will continue with the flywheel shooter example from Motion Control. If you want your flywheel to achieve a certain RPM then you can use the fact that you know what the maximum RPM of the motor attached to the flywheel. Using this maximum RPM, you can make a good guess about what voltage will be required to get close to the target RPMs. For example if you are using a NEO Motor (see Table 1) then we know that it has a maximum speed of 5676 RPM when 12 volts is applied under no load. Lets assume that the motor has 1-to-1 gearing to the flywheel and our target RPM of the flywheel is 3000 RPM. We therefore want to spin the motor to (3000/5676) = 0.529 or 52.9% of its maximum speed. So we should be able to apply (0.529*12 volts) = 6.35 volts to the motor to get our desired 3000 RPM.

The idea of using the physics of the motor to estimate what the motor output should be is called "feed forward". Feed forward is used in addition of PID Control to achieve very good motor behavior. WPILib provides some classes to help do some of the feed forward calculations.

See WPILib Feed Forward.

Smart Controller PID

WPILib provides classes to do PID calculations within the robot program however, smart controllers can perform PID calculations themselves. These "onboard" PID calculations are typically done at a much faster rate than is possible in the robot program (1ms vs 20ms). The faster PID calculations should provide better control of the motor.

PID Tuning

It is best if you can find PID values that do not use the Integral (kI) term. That is because including kI makes the PID controller have "memory" and can cause very unexpected behavior. Try to use only Kp, Kd and feed forward.

PID Units and Feed Forward Values

The WPILib frc2::PIDController class can use any units the programmer decides to use since the measurement values are passed into the Calculate() method which returns a percent output value from [-1,1]. Therefore the units of the PID constants will vary depending on the units used in the code. The feed forward classes in WPILib use the units library and are templated on whatever units are used to measure distance (either linear or angular).

Each software vendor uses different units for their PID Controllers. The table below summarizes the differences between the different vendor libraries.

Table 2. SparkMAX and Phoenix 6 TalonFX Onboard Controller Units
Control Type Controller Units Configurable

Duty Cycle

[-1,1]

no

Voltage

[0,12] volts

no

Position

rotations

yes

Velocity

RPM

yes

Current

Amps

no

The way the feed forward values are configured differs between the RevLib and Phoenix libraries. The RevLib Library (SparkMAX) only uses a single feed forward value (kFF) which works the same as kV in WPILib. There is also an Arbitrary feed forward value that can be used which can act like kS in WPILib or can be customized (e.g. to vary with arm angle to compensate for varying gravity effect like kG). The Phoenix 6 Library (TalonFX) uses kS, kG, kV, and kA for onboard control.

Links to the RevLib and CTRE Phoenix V6 APIs are in Resources.

Mechanism Tuning Procedure

BREAD 5940 gave a very good workshop at the Capital City Classic in the Fall of 2023 on tuning mechanisms (Fall 2023 Capital City Classic Workshop on Tuning / Autonomous). In the workshop they outlined the order in which the Feed Forward and Feedback (PID) constants should be tuned (pages 20-34). Their basic procedure is outlined here with some information added.

  1. With either Pheonix Tuner or REV Client determine the direction of travel when a positive motor move is made. This should be done before any control is attempted with a very small percent output value. If reversing what is considered positive travel makes sense then invert the motor.

  2. Determine if the above positive motor movement results in a positive change in the position measurement (i.e. encoder). If a positive movement does not result in a positive change in position then invert the encoder output.

    If a positive movement does not result in a positive change in position then the mechanism will move violently and break itself (and maybe other things as well).

  3. Ensure the the units for the position measurement are correct. For an arm mechanism make sure the zero angle position is defined correctly (for an arm 0-deg should be when the arm is horizontal).

  4. Enable the mechanism with full motion control enabled but all coeffients set to zero. Determine gravity compensation (for elevators and arms). Increase kG until the mechansim no longer sags (note value). Increase kG until mechanism starts to move upward (note value). Set kG to the midpoint value and kS to half the difference between the two values.

  5. Setup two joystick buttons to move between two setpoints using trapezoidal profile moves (either on-board or software based). Plot the desired position against the actual position and the desired velocity against the actual velocity. Adjust kV until the velocities match. Position will not yet match.

  6. (Optionally) Increase kA to get better agreement of the plot in the velocity transition regions.

  7. Add kP until you see small oscillations then decrease kP a little. The mechanism should be tracking position very well now.

  8. Usually don’t need kD and never use kI.

Swerve Drive

The swerve drive has three control inputs: Vx, Vy, and omega (angular velocity). Straight forward is positive x direction, straight left is positive y direction, and counter-clockwise is positive omega. The ChassisSpeeds object stores these three values in one object that is passed to the Swerve Drive control functions. Typically the left joystick determines the Vx and Vy values and the X-axis of the right joystick determines the angular velocity (omega). The ChassisSpeeds values can be either robot relative or field relative. Robot relative speeds can be thought of as though you are sitting on the robot so positive Vx is forward on the robot. Field relative speeds are relative to the room that the robot is driving in which is essential for swerve drives due to the rapid rotations that can happen while driving. To obtain field relative control a gyroscope must be used to track the angular orientation of the robot at all times.

The ChassisSpeeds object is used to calculate each swerve module speed (4 of them usually) which are stored in SwerveModuleStates objects using the SwerveDriveKinematics class. These SwerveModuleStates are then used to set each individual swerve module to the correct steering angle and speed.

More information can be found at this blog and in this video.

NetworkTables

See WPILib Documentation on NetworkTables

Don’t use NetworkTables to hold the values of the robot program variables. Read from and write to the NT when needed. Preferably only read settings that change the robot behavior at the very beginning of code execution (TeleopInit() or AutonomousInit()).

Autonomous Control

TODO "Self Driving Robot"

Vision / Cameras

LINKS:

Other Teams

Resources

TOP