- FRC Game Manual
- FRC Game Q&A

# Step 3: Creating a Drive Subsystem

Now that our drive is characterized, it is time to start writing our robot code proper. As mentioned before, we will use the command-based framework for our robot code. Accordingly, our first step is to write a suitable drive subsystem class.

The full drive class from the RamseteCommand Example Project (Java, C++) can be seen below. The rest of the article will describe the steps involved in writing this class.

Java

      5package edu.wpi.first.wpilibj.examples.ramsetecommand.subsystems;
      6
      7import edu.wpi.first.math.geometry.Pose2d;
      8import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
      9import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
     10import edu.wpi.first.util.sendable.SendableRegistry;
     11import edu.wpi.first.wpilibj.ADXRS450_Gyro;
     12import edu.wpi.first.wpilibj.Encoder;
     13import edu.wpi.first.wpilibj.drive.DifferentialDrive;
     14import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants;
     15import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
     16import edu.wpi.first.wpilibj2.command.SubsystemBase;
     17
     18public class DriveSubsystem extends SubsystemBase {
     19  // The motors on the left side of the drive.
     20  private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
     21  private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
     22
     23  // The motors on the right side of the drive.
     24  private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
     25  private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
     26
     27  // The robot's drive
     28  private final DifferentialDrive m_drive =
     29      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
     30
     31  // The left-side drive encoder
     32  private final Encoder m_leftEncoder =
     33      new Encoder(
     34          DriveConstants.kLeftEncoderPorts[0],
     35          DriveConstants.kLeftEncoderPorts[1],
     36          DriveConstants.kLeftEncoderReversed);
     37
     38  // The right-side drive encoder
     39  private final Encoder m_rightEncoder =
     40      new Encoder(
     41          DriveConstants.kRightEncoderPorts[0],
     42          DriveConstants.kRightEncoderPorts[1],
     43          DriveConstants.kRightEncoderReversed);
     44
     45  // The gyro sensor
     46  private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();
     47
     48  // Odometry class for tracking robot pose
     49  private final DifferentialDriveOdometry m_odometry;
     50
     51  /** Creates a new DriveSubsystem. */
     52  public DriveSubsystem() {
     53    SendableRegistry.addChild(m_drive, m_leftLeader);
     54    SendableRegistry.addChild(m_drive, m_rightLeader);
     55
     56    m_leftLeader.addFollower(m_leftFollower);
     57    m_rightLeader.addFollower(m_rightFollower);
     58
     59    // We need to invert one side of the drivetrain so that positive voltages
     60    // result in both sides moving forward. Depending on how your robot's
     61    // gearbox is constructed, you might have to invert the left side instead.
     62    m_rightLeader.setInverted(true);
     63
     64    // Sets the distance per pulse for the encoders
     65    m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
     66    m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
     67
     68    resetEncoders();
     69    m_odometry =
     70        new DifferentialDriveOdometry(
     71            m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
     72  }
     73
     74  @Override
     75  public void periodic() {
     76    // Update the odometry in the periodic block
     77    m_odometry.update(
     78        m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
     79  }
     80
     81  /**
     82   * Returns the currently-estimated pose of the robot.
     83   *
     84   * @return The pose.
     85   */
     86  public Pose2d getPose() {
     87    return m_odometry.getPoseMeters();
     88  }
     89
     90  /**
     91   * Returns the current wheel speeds of the robot.
     92   *
     93   * @return The current wheel speeds.
     94   */
     95  public DifferentialDriveWheelSpeeds getWheelSpeeds() {
     96    return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
     97  }
     98
     99  /**
    100   * Resets the odometry to the specified pose.
    101   *
    102   * @param pose The pose to which to set the odometry.
    103   */
    104  public void resetOdometry(Pose2d pose) {
    105    m_odometry.resetPosition(
    106        m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), pose);
    107  }
    108
    109  /**
    110   * Drives the robot using arcade controls.
    111   *
    112   * @param fwd the commanded forward movement
    113   * @param rot the commanded rotation
    114   */
    115  public void arcadeDrive(double fwd, double rot) {
    116    m_drive.arcadeDrive(fwd, rot);
    117  }
    118
    119  /**
    120   * Controls the left and right sides of the drive directly with voltages.
    121   *
    122   * @param leftVolts the commanded left output
    123   * @param rightVolts the commanded right output
    124   */
    125  public void tankDriveVolts(double leftVolts, double rightVolts) {
    126    m_leftLeader.setVoltage(leftVolts);
    127    m_rightLeader.setVoltage(rightVolts);
    128    m_drive.feed();
    129  }
    130
    131  /** Resets the drive encoders to currently read a position of 0. */
    132  public void resetEncoders() {
    133    m_leftEncoder.reset();
    134    m_rightEncoder.reset();
    135  }
    136
    137  /**
    138   * Gets the average distance of the two encoders.
    139   *
    140   * @return the average of the two encoder readings
    141   */
    142  public double getAverageEncoderDistance() {
    143    return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
    144  }
    145
    146  /**
    147   * Gets the left drive encoder.
    148   *
    149   * @return the left drive encoder
    150   */
    151  public Encoder getLeftEncoder() {
    152    return m_leftEncoder;
    153  }
    154
    155  /**
    156   * Gets the right drive encoder.
    157   *
    158   * @return the right drive encoder
    159   */
    160  public Encoder getRightEncoder() {
    161    return m_rightEncoder;
    162  }
    163
    164  /**
    165   * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
    166   *
    167   * @param maxOutput the maximum output to which the drive will be constrained
    168   */
    169  public void setMaxOutput(double maxOutput) {
    170    m_drive.setMaxOutput(maxOutput);
    171  }
    172
    173  /** Zeroes the heading of the robot. */
    174  public void zeroHeading() {
    175    m_gyro.reset();
    176  }
    177
    178  /**
    179   * Returns the heading of the robot.
    180   *
    181   * @return the robot's heading in degrees, from -180 to 180
    182   */
    183  public double getHeading() {
    184    return m_gyro.getRotation2d().getDegrees();
    185  }
    186
    187  /**
    188   * Returns the turn rate of the robot.
    189   *
    190   * @return The turn rate of the robot, in degrees per second
    191   */
    192  public double getTurnRate() {
    193    return -m_gyro.getRate();
    194  }
    195}

C++ (Header)

      5#pragma once
      6
      7#include <frc/ADXRS450_Gyro.h>
      8#include <frc/Encoder.h>
      9#include <frc/drive/DifferentialDrive.h>
     10#include <frc/geometry/Pose2d.h>
     11#include <frc/kinematics/DifferentialDriveOdometry.h>
     12#include <frc/motorcontrol/PWMSparkMax.h>
     13#include <frc2/command/SubsystemBase.h>
     14#include <units/voltage.h>
     15
     16#include "Constants.h"
     17
     18class DriveSubsystem : public frc2::SubsystemBase {
     19 public:
     20  DriveSubsystem();
     21
     22  /**
     23   * Will be called periodically whenever the CommandScheduler runs.
     24   */
     25  void Periodic() override;
     26
     27  // Subsystem methods go here.
     28
     29  /**
     30   * Drives the robot using arcade controls.
     31   *
     32   * @param fwd the commanded forward movement
     33   * @param rot the commanded rotation
     34   */
     35  void ArcadeDrive(double fwd, double rot);
     36
     37  /**
     38   * Controls each side of the drive directly with a voltage.
     39   *
     40   * @param left the commanded left output
     41   * @param right the commanded right output
     42   */
     43  void TankDriveVolts(units::volt_t left, units::volt_t right);
     44
     45  /**
     46   * Resets the drive encoders to currently read a position of 0.
     47   */
     48  void ResetEncoders();
     49
     50  /**
     51   * Gets the average distance of the TWO encoders.
     52   *
     53   * @return the average of the TWO encoder readings
     54   */
     55  double GetAverageEncoderDistance();
     56
     57  /**
     58   * Gets the left drive encoder.
     59   *
     60   * @return the left drive encoder
     61   */
     62  frc::Encoder& GetLeftEncoder();
     63
     64  /**
     65   * Gets the right drive encoder.
     66   *
     67   * @return the right drive encoder
     68   */
     69  frc::Encoder& GetRightEncoder();
     70
     71  /**
     72   * Sets the max output of the drive.  Useful for scaling the drive to drive
     73   * more slowly.
     74   *
     75   * @param maxOutput the maximum output to which the drive will be constrained
     76   */
     77  void SetMaxOutput(double maxOutput);
     78
     79  /**
     80   * Returns the heading of the robot.
     81   *
     82   * @return the robot's heading in degrees, from -180 to 180
     83   */
     84  units::degree_t GetHeading() const;
     85
     86  /**
     87   * Returns the turn rate of the robot.
     88   *
     89   * @return The turn rate of the robot, in degrees per second
     90   */
     91  double GetTurnRate();
     92
     93  /**
     94   * Returns the currently-estimated pose of the robot.
     95   *
     96   * @return The pose.
     97   */
     98  frc::Pose2d GetPose();
     99
    100  /**
    101   * Returns the current wheel speeds of the robot.
    102   *
    103   * @return The current wheel speeds.
    104   */
    105  frc::DifferentialDriveWheelSpeeds GetWheelSpeeds();
    106
    107  /**
    108   * Resets the odometry to the specified pose.
    109   *
    110   * @param pose The pose to which to set the odometry.
    111   */
    112  void ResetOdometry(frc::Pose2d pose);
    113
    114 private:
    115  // Components (e.g. motor controllers and sensors) should generally be
    116  // declared private and exposed only through public methods.
    117
    118  // The motor controllers
    119  frc::PWMSparkMax m_left1;
    120  frc::PWMSparkMax m_left2;
    121  frc::PWMSparkMax m_right1;
    122  frc::PWMSparkMax m_right2;
    123
    124  // The robot's drive
    125  frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
    126                                 [&](double output) { m_right1.Set(output); }};
    127
    128  // The left-side drive encoder
    129  frc::Encoder m_leftEncoder;
    130
    131  // The right-side drive encoder
    132  frc::Encoder m_rightEncoder;
    133
    134  // The gyro sensor
    135  frc::ADXRS450_Gyro m_gyro;
    136
    137  // Odometry class for tracking robot pose
    138  frc::DifferentialDriveOdometry m_odometry;
    139};

C++ (Source)

     5#include "subsystems/DriveSubsystem.h"
     6
     7#include <frc/geometry/Rotation2d.h>
     8#include <frc/kinematics/DifferentialDriveWheelSpeeds.h>
     9
    10using namespace DriveConstants;
    11
    12DriveSubsystem::DriveSubsystem()
    13    : m_left1{kLeftMotor1Port},
    14      m_left2{kLeftMotor2Port},
    15      m_right1{kRightMotor1Port},
    16      m_right2{kRightMotor2Port},
    17      m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
    18      m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},
    19      m_odometry{m_gyro.GetRotation2d(), units::meter_t{0}, units::meter_t{0}} {
    20  wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
    21  wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
    22
    23  m_left1.AddFollower(m_left2);
    24  m_right1.AddFollower(m_right2);
    25
    26  // We need to invert one side of the drivetrain so that positive voltages
    27  // result in both sides moving forward. Depending on how your robot's
    28  // gearbox is constructed, you might have to invert the left side instead.
    29  m_right1.SetInverted(true);
    30
    31  // Set the distance per pulse for the encoders
    32  m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
    33  m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
    34
    35  ResetEncoders();
    36}
    37
    38void DriveSubsystem::Periodic() {
    39  // Implementation of subsystem periodic method goes here.
    40  m_odometry.Update(m_gyro.GetRotation2d(),
    41                    units::meter_t{m_leftEncoder.GetDistance()},
    42                    units::meter_t{m_rightEncoder.GetDistance()});
    43}
    44
    45void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
    46  m_drive.ArcadeDrive(fwd, rot);
    47}
    48
    49void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
    50  m_left1.SetVoltage(left);
    51  m_right1.SetVoltage(right);
    52  m_drive.Feed();
    53}
    54
    55void DriveSubsystem::ResetEncoders() {
    56  m_leftEncoder.Reset();
    57  m_rightEncoder.Reset();
    58}
    59
    60double DriveSubsystem::GetAverageEncoderDistance() {
    61  return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
    62}
    63
    64frc::Encoder& DriveSubsystem::GetLeftEncoder() {
    65  return m_leftEncoder;
    66}
    67
    68frc::Encoder& DriveSubsystem::GetRightEncoder() {
    69  return m_rightEncoder;
    70}
    71
    72void DriveSubsystem::SetMaxOutput(double maxOutput) {
    73  m_drive.SetMaxOutput(maxOutput);
    74}
    75
    76units::degree_t DriveSubsystem::GetHeading() const {
    77  return m_gyro.GetRotation2d().Degrees();
    78}
    79
    80double DriveSubsystem::GetTurnRate() {
    81  return -m_gyro.GetRate();
    82}
    83
    84frc::Pose2d DriveSubsystem::GetPose() {
    85  return m_odometry.GetPose();
    86}
    87
    88frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
    89  return {units::meters_per_second_t{m_leftEncoder.GetRate()},
    90          units::meters_per_second_t{m_rightEncoder.GetRate()}};
    91}
    92
    93void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
    94  m_odometry.ResetPosition(m_gyro.GetRotation2d(),
    95                           units::meter_t{m_leftEncoder.GetDistance()},
    96                           units::meter_t{m_rightEncoder.GetDistance()}, pose);
    97}

## Configuring the Drive Encoders

The drive encoders measure the rotation of the wheels on each side of the drive. To properly configure the encoders, we need to specify two things: the ports the encoders are plugged into, and the distance per encoder pulse. Then, we need to write methods allowing access to the encoder values from code that uses the subsystem.

### Encoder Ports

The encoder ports are specified in the encoder’s constructor, like so:

Java

    31  // The left-side drive encoder
    32  private final Encoder m_leftEncoder =
    33      new Encoder(
    34          DriveConstants.kLeftEncoderPorts[0],
    35          DriveConstants.kLeftEncoderPorts[1],
    36          DriveConstants.kLeftEncoderReversed);
    37
    38  // The right-side drive encoder
    39  private final Encoder m_rightEncoder =
    40      new Encoder(
    41          DriveConstants.kRightEncoderPorts[0],
    42          DriveConstants.kRightEncoderPorts[1],
    43          DriveConstants.kRightEncoderReversed);

C++ (Source)

    17      m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
    18      m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},

### Encoder Distance per Pulse

The distance per pulse is specified by calling the encoder’s `setDistancePerPulse` method. Note that for the WPILib Encoder class, “pulse” refers to a full encoder cycle (i.e. four edges), and thus will be 1/4 the value that was specified in the SysId config. Remember, as well, that the distance should be measured in meters!

Java

    65    m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
    66    m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);

C++ (Source)

    32  m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
    33  m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());

### Encoder Accessor Method

To access the values measured by the encoders, we include the following method:

Important

The returned velocities must be in meters! Because we configured the distance per pulse on the encoders above, calling `getRate()` will automatically apply the conversion factor from encoder units to meters. If you are not using WPILib’s `Encoder` class, you must perform this conversion either through the respective vendor’s API or by manually multiplying by a conversion factor.

Java

    90  /**
    91   * Returns the current wheel speeds of the robot.
    92   *
    93   * @return The current wheel speeds.
    94   */
    95  public DifferentialDriveWheelSpeeds getWheelSpeeds() {
    96    return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
    97  }

C++ (Source)

    88frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
    89  return {units::meters_per_second_t{m_leftEncoder.GetRate()},
    90          units::meters_per_second_t{m_rightEncoder.GetRate()}};
    91}

We wrap the measured encoder values in a `DifferentialDriveWheelSpeeds` object for easier integration with the `RamseteCommand` class later on.

## Configuring the Gyroscope

The gyroscope measures the rate of change of the robot’s heading (which can then be integrated to provide a measurement of the robot’s heading relative to when it first turned on). In our example, we use the Analog Devices ADXRS450 FRC Gyro Board, which was included in the kit of parts for several years:

Java

    45  // The gyro sensor
    46  private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();

C++ (Header)

    134  // The gyro sensor
    135  frc::ADXRS450_Gyro m_gyro;

### Gyroscope Accessor Method

To access the current heading measured by the gyroscope, we include the following method:

Java

    178  /**
    179   * Returns the heading of the robot.
    180   *
    181   * @return the robot's heading in degrees, from -180 to 180
    182   */
    183  public double getHeading() {
    184    return m_gyro.getRotation2d().getDegrees();
    185  }

C++ (Source)

    76units::degree_t DriveSubsystem::GetHeading() const {
    77  return m_gyro.GetRotation2d().Degrees();
    78}

## Configuring the Odometry

Now that we have our encoders and gyroscope configured, it is time to set up our drive subsystem to automatically compute its position from the encoder and gyroscope readings.

First, we create a member instance of the `DifferentialDriveOdometry` class:

Java

    48  // Odometry class for tracking robot pose
    49  private final DifferentialDriveOdometry m_odometry;

C++ (Header)

    137  // Odometry class for tracking robot pose
    138  frc::DifferentialDriveOdometry m_odometry;

Then we initialize the `DifferentialDriveOdometry`.

Java

    69    m_odometry =
    70        new DifferentialDriveOdometry(
    71            m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());

C++ (Source)

    19      m_odometry{m_gyro.GetRotation2d(), units::meter_t{0}, units::meter_t{0}} {

### Updating the Odometry

The odometry class must be regularly updated to incorporate new readings from the encoder and gyroscope. We accomplish this inside the subsystem’s `periodic` method, which is automatically called once per main loop iteration:

Java

    74  @Override
    75  public void periodic() {
    76    // Update the odometry in the periodic block
    77    m_odometry.update(
    78        m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
    79  }

C++ (Source)

    38void DriveSubsystem::Periodic() {
    39  // Implementation of subsystem periodic method goes here.
    40  m_odometry.Update(m_gyro.GetRotation2d(),
    41                    units::meter_t{m_leftEncoder.GetDistance()},
    42                    units::meter_t{m_rightEncoder.GetDistance()});
    43}

### Odometry Accessor Method

To access the robot’s current computed pose, we include the following method:

Java

    81  /**
    82   * Returns the currently-estimated pose of the robot.
    83   *
    84   * @return The pose.
    85   */
    86  public Pose2d getPose() {
    87    return m_odometry.getPoseMeters();
    88  }

C++ (Source)

    84frc::Pose2d DriveSubsystem::GetPose() {
    85  return m_odometry.GetPose();
    86}

Important

Before running a `RamseteCommand`, teams are strongly encouraged to deploy and test the odometry code alone, with values sent to the SmartDashboard or Shuffleboard during the `DriveSubsystem`’s `periodic()`. This odometry must be correct for a RamseteCommand to successfully work, as sign or unit errors can cause a robot to move at high speeds in unpredictable directions.

## Voltage-Based Drive Method

Finally, we must include one additional method - a method that allows us to set the voltage to each side of the drive using the `setVoltage()` method of the `MotorController` interface. The default WPILib drive class does not include this functionality, so we must write it ourselves:

Java

    119  /**
    120   * Controls the left and right sides of the drive directly with voltages.
    121   *
    122   * @param leftVolts the commanded left output
    123   * @param rightVolts the commanded right output
    124   */
    125  public void tankDriveVolts(double leftVolts, double rightVolts) {
    126    m_leftLeader.setVoltage(leftVolts);
    127    m_rightLeader.setVoltage(rightVolts);
    128    m_drive.feed();
    129  }

C++ (Source)

    49void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
    50  m_left1.SetVoltage(left);
    51  m_right1.SetVoltage(right);
    52  m_drive.Feed();
    53}

It is very important to use the `setVoltage()` method rather than the ordinary `set()` method, as this will automatically compensate for battery “voltage sag” during operation. Since our feedforward voltages are physically-meaningful (as they are based on measured identification data), this is essential to ensuring their accuracy.

Warning

`RamseteCommand` itself does not internally enforce any speed or acceleration limits before providing motor voltage parameters to this method. During initial code development, teams are strongly encouraged to apply both maximum and minimum bounds on the input variables before passing these values to `setVoltage()` while ensuring the trajectory velocity and acceleration are achievable. For example, generate a trajectory with a little less than half of the Robot’s maximum velocity and limit voltage to 6 volts.