RLD 4: CTRE - TEAM1771/Crash-Course GitHub Wiki

The CTRE Library

Cross The Road Electronics is a FRC vendor that manufactures encoders (sensors), motors, motor controllers, and more. The CTRE library, nicknamed Phoenix (version 6) allows us to communicate with and control all their products in robot code.

Installing/including the library

To install the library, refer to CTRE's guide to online installation.

To use the library in code, you still need one more step. In each file you use it, type #include <ctre/phoenix6/"device".hpp>

Where device can be TalonFX, CANcoder, etc.

TalonFX (controls Falcon/Kraken motors)

TalonFX refers to the integrated motor controller found on Falcon and Kraken motors.

Connecting (initalizing variable in code)

Connecting to a TalonFX motor controller is as simple as creating a variable with the correct type and passing in the CAN ID, as well as the name of the CAN network the device is on.

The CAN ID is the address of the motor on the network with all other motor controllers and encoders.

If you don't know the CAN ID of a CTRE, the Tuner X software can show you each device on the network and information such as ID and configurations and is also useful for testing (more on that in the Tuner X section below).

The CAN network name can either be "rio" (a blank string defaults to "rio" as well) if the device is connected to the CAN network running through the RoboRIO, or it can be the name set to the CANivore that the device is hooked up to. This name can also be checked in Tuner X.

As an example:

ctre::phoenix6::hardware::TalonFX talon_fx{3, "rio"};

Configuring TalonFX

Each TalonFX contains a set of settings that can modified either through Tuner X or through code.

For example, you can set the IdleMode of the controller to be Coast or Break. Coast will allow the motor to spin freely, while break will attempt to resist movement.

You can also set limits on how much power the motor can apply, adjust PID tuning values, and much more.

To set these configuration options, create a configs::TalonFXConfiguration variable

ctre::phoenix6::configs::TalonFXConfiguration CONFIGS{};

Each configuration option include a variety of member variables corresponding to configurations you can change.

To apply the configuration, run the GetConfigurator() function on the talon_fx variable, followed by .Apply("config name")

talon_fx.GetConfigurator().Apply(CONFIGS);
// Constants/CONAVATOR.hpp - we like to keep all constant (known) values in a Constants folder under include/
#pragma once

#include <ctre/phoenix6/TalonFX.hpp>
namesapce CONAVATOR // Namespaces dedicated to constants are all caps
{
    constexpr auto CAN_ID = 7;
    constexpr auto CAN_BUS = "Raptor"; // Name of CANivore
    
    using namespace ctre::phoenix6;

    configs::TalonFXConfiguration const CONFIG = 
        []() // This is called a lambda (temp function) that runs when initalizing this variable to set each setting appropiately
    {
        configs::TalonFXConfiguration falcon_config{};
        falcon_config.MotorOutput.NeutralMode = signals::NeutralModeValue::Coast;
        falcon_config.Slot0.kV = 0.302;
        falcon_config.Slot0.kP = 0;
        falcon_config.Slot1.kP = 4;
        falcon_config.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = 0.01;
        falcon_config.Feedback.SensorToMechanismRatio = 30;
        return falcon_config;
    }();
    ...
}

// Conavator.cpp
#include "Conavator.hpp"

using namespace CONAVATOR;

static ctre::phoenix6::hardware::TalonFX wheelie{CAN_ID, CAN_BUS};

void Conavator::init()
{
    wheelie.GetConfigurator().Apply(CONFIG);
}

CTRE's guide explains in more detail this process.

Setting up units correctly - Gear Ratios and Rotations to Meters

The encoder in TalonFXs (and other devices) records the number of rotations the motor has seen since boot.

For example, the line talonfx.GetPosition().GetValue() will return a units::turn_t variable, which is the number of rotations of the motor. If you spun the motor twice, this number would be 2_tr (2 rotations).

Similarly, the line talonfx.GetVelocity().GetValue() will return a units::turns_per_second_t variable, which is the number of rotations per second of the motor. If you spun the motor three times in one second, the value would be 3.

However, typically this number is not very useful to us in the context of robotic systems.

It would be much more helpful for the driving motors to return a Velocity representing the speed of the wheel, or the arm motors to return a Position representing the postiion of the arm in degrees.

Also, the same logic applies when controlling motors. It is much easier to tell a driving motor to go to 5 f/s than having to calcualate the motor speed each time you wanted to change speed.

Luckily, the Phoenix library provides us with a way to do just that: Conversion Ratios.

You can change two ratios:

  • the MotorToSensorRatio (if you are using an external sensor, like a CANCoder on a SwerveModule),
  • or the SensorToMechanismRatio (the number of rotations of the sensor (aka motor shaft) to rotate the mechanism once)

If the motor is attached to a 27:1 gear system, this means for every 27 rotations of the motor, the system will rotate once. (the specific ratio of your system can found using CAD or robot inspection: talk to your design/engineering team) So, if you have a simple system on a 27:1 gear ratio, you would configure your motor like so:

ctre::phoenix6::configs::TalonFXConfiguration talon_config{};
talon_config.Feedback.SensorToMechanismRatio = 27;

talonfx.GetConfigurator().Apply(falcon_config);

Now, if you spin the motor 27 times, or the final system 1 time, the reported position will be 1_tr.

For a drivetrain system, it might be more helpful to represent position/velocity in terms of meters/feet instead of rotations.

The following formula relates length (in meters or whatever units you choose) and rotations

So, for a drivetrain system with a radius of 2_in (diameter of 4_in) and a gear ratio of 10, our conversion ratio would be 10 / (4pi) (This would return a position and velocity in inches/second.)

Alternatively, we could convert the radius and diameter to meters, and then use the same formula to find a conversion ratio with meters. Diamater of 0.1016_m, conversion ratio is 10 / (.1016 * pi)

Running TalonFX

The TalonFX is a very powerful motor controller with many control options.

The simplest is called DutyCycle, and you simply provide a percentage to run the motor at between -1 and 1 (full reverse and full forward, with 0 being stop).

using namespace ctre::phoenix6;

controls::DutyCycleOut request{0.0};

talon_fx.SetControl(request);

For more examples, check out CTRE's guide to “Open-Loop Control”.

More advanced PID based controls include Position and Velocity.

void SlideDrive::drive(units::meters_per_second_t const x_speed)
{
    // Convert speed (m/s) to motor rotations/turns per second
    units::turns_per_second_t const rot_vel{
        units::turn_t{x_speed.value()} / 1_s};

    // FOC control is a specical way of controlling a motor that results in ~15% greater peak power.
    auto driver_controller = ctre::phoenix6::controls::VelocityDutyCycle(rot_vel).WithEnableFOC(true);
    driver.SetControl(driver_controller);
}

CTRE does a great job explaining how these modes works, and what each variable you can tune does. Here's their guide.

Recieving Telemetry Data

Motor controllers not only allow controlling a motor, but also provide feedback in the form of data.

For example, you can read the temperature, position (displacement in rotations since boot), and velocity of a motor.

CTRE uses something called "Status Signals," which are variables that are used to get each specific data point from the device.

You can get one of these variables by calling the appropiate getter on the device variable

using namespace ctre::phoenix6;
StatusSignal<units::angular_velocity::turns_per_second_t>& velocity_getter = talon_fx.GetVelocity();

This variable now contains the most recent value (of velocity) transmitted by the Talon over the CAN network

To read that value, use .GetValue()

using namespace ctre::phoenix6;
StatusSignal<units::turns_per_second_t>& velocity_getter = talon_fx.GetVelocity();

units::turns_per_second_t const current_velocity = velocity_getter.GetValue();

You can also shorten this into one line

using namespace ctre::phoenix6;
auto const current_velocity = talon_fx.GetVelocity().GetValue();

You can instead choose to save the StatusSignal as a pointer variable to use on each run of a function

WARNING: The StatusSignal does not automatically refresh to get the latest value from the CAN network.

You must use .Refresh() to get the latest value

// Arm.cpp
#include "Constants/ARM.hpp"
#include <ctre/phoenix6/TalonFX.hpp>

using namespace ARM; // contains constants such as CAN ids and CAN bus name

static ctre::phoenix6::hardware::TalonFX boom{BOOM_CAN_ID, CAN_BUS};
static ctre::phoenix6::hardware::TalonFX stick{STICK_CAN_ID, CAN_BUS};
static ctre::phoenix6::StatusSignal<units::turn_t> *boom_pos_getter, *stick_pos_getter;

void Arm::init()
{
    boom.GetConfigurator().Apply(BOOM::CONFIGS);
    stick.GetConfigurator().Apply(STICK::CONFIGS);

    boom_pos_getter = &boom.GetPosition();
    stick_pos_getter = &stick.GetPosition();
}

units::degree_t Arm::getBoomPos()
{
    boom_pos_getter.Refresh();
    return units::degree_t{boom_pos_getter.GetValue().value()};
}

For more detail on how you can use Signal Values, check out what CTRE has to say.

CANCoder

A CANCoder is a magnetic encoder that is used to measure the rotation of a mechanism (i.e., the rotation of each swerve module).

A CANCoder is initialized and configured in code in a very similar manner to the TalonFX above.

// Constants/SWERVEMODULE.hpp
#pragma once

#include <ctre/phoenix6/CANcoder.hpp>

struct SwerveModuleInfo
{
    int const DRIVER_ID, TURNER_ID, CANCODER_ID;
    units::turn_t const MAGNET_OFFSET; // This is difference for each swerve module and must be found using TunerX
};

namespace SWERVEMODULE
{
    namespace CANCODER
    {
        configs::CANcoderConfiguration const CONFIG =
            []()
        {
            configs::CANcoderConfiguration cancoder_config{};

            cancoder_config.MagnetSensor.AbsoluteSensorRange = signals::AbsoluteSensorRangeValue::Signed_PlusMinusHalf;
            cancoder_config.MagnetSensor.SensorDirection = signals::SensorDirectionValue::Clockwise_Positive;

            return cancoder_config;
        }();
    }

    ...
}

// SwerveModule.hpp
#pragma once

class SwerveModule
{
    ctre::phoenix6::hardware::TalonFX driver, turner;
    ctre::phoenix6::hardware::CANcoder cancoder;

    SwerveModule(SwerveModuleInfo const module_info);
}

// SwerveModule.cpp
#include "SwerveModule.hpp
#include "Constants/SWERVEMODULE.hpp"

SwerveModule::SwerveModule(SwerveModuleInfo const module_info)
    : driver{module_info.DRIVER_ID, "Raptor"},
        turner{module_info.TURNER_ID, "Raptor"},
        cancoder{module_info.CANCODER_ID, "Raptor"},
{
    configs::CANcoderConfiguration cancoder_config = CANCODER::CONFIG;

    cancoder_config.MagnetSensor.MagnetOffset = module_info.MAGNET_OFFSET.value();

    cancoder.GetConfigurator().Apply(cancoder_config);

    ...
}

CANCoders are often used as an external sensor to a TalonFX controller, such as is the case with swerve modules.

configs::TalonFXConfiguration turner_config = TURNER::CONFIG;

falcon_config.Feedback.FeedbackSensorSource = signals::FeedbackSensorSourceValue::RemoteCANcoder;

turner_config.Feedback.FeedbackRemoteSensorID = module_info.CANCODER_ID;

CTRE explains how to do this in greater detail here.

Tuner X

Tuner X is a great software for troubleshooting issues with CTRE devices. You can find motor controllers and sensors, see their CAN ID, adjust configurations, test the motors, and much more.

The CTRE docs give a good breakdown on the software and how to use it.

Documentation

To learn more about the Phoenix ecosystem, check out their website.

For specific information on utilizing the library/API, reference their user guide.

For even more detailed information, you can look through the entire API's documentation here (the same information that would show up if you hover over functions or variables in VSCode).

For more examples, check out the examples page on CTRE's site.