Lab 2: Drive a West Coast Drive#

Why it matters#

In Lab 1 you spun one motor. Now you'll drive a whole robot.

The drivetrain is the most important mechanism on any robot: if it can't drive well, nothing else matters. In this lab you'll program the team's West Coast Drive (WCD) so a driver can move it around with an Xbox controller. Along the way you'll learn three ideas every drivetrain uses:

• Leaders and followers: several motors acting as one
• Inversion: why one side of the robot has to run "backward"
• Skid steer driving: each joystick runs one side of the robot, just like the two levers on a skid steer loader

Before you start: finish Lab 1.1. Lesson 10 explains the drive code in more depth; read it before or after.

Read this#

• Using the WPILib Drive Classes
• Joysticks

Walkthrough#

What's a West Coast Drive?#

A WCD steers like a skid steer loader: the left wheels all move together, the right wheels all move together, and there's no steering. To turn, one side drives faster than the other, and the wheels skid sideways across the floor. That's where the name skid steer comes from.

• Each side has one gearbox with two CIM motors. Both CIMs turn the same gear, so they must always push in the same direction.
• The middle wheels sit slightly lower ("dropped center"), so the robot rocks onto fewer wheels and turns more easily.

The hardware#

The RoboJacks West Coast Drive chassis, before the elevator goes on. Front is at the top of the photo.

Find each of these on the real chassis before you write any code:

Trace the wires from one motor to its SPARK MAX, and from the SPARK MAX to its breaker on the Power Distribution Hub. Knowing which controller drives which motor makes Step 1 much easier.

New project#

The WCD is its own robot with its own roboRIO, so it gets its own project. Repeat Lab 0, Parts 2 to 4, but name the project WestCoastDrive. Remember to install REVLib.

Then copy the telemetry folder from your TestBed project (Lab 1.1) into the new project's frc/robot folder, next to Robot.java. (It's also on GitHub.) Every subsystem uses it to send values to Elastic.

Step 1: Check the SPARK MAXes#

1. Connect each SPARK MAX to your laptop over USB-C and open the REV Hardware Client.
2. Write down each one's CAN ID and which motor it drives (left front, left back, and so on). Put tape labels on the controllers.
3. Pick one controller on each side as the leader. The other is its follower.
4. Check that no two SPARK MAXes share an ID. A duplicate makes motors twitch or not respond (Lesson 4).

Step 2: Constants#

Open Constants.java and add a class for the drivetrain. Use the CAN IDs you wrote down.

public final class Constants {
  public static class OperatorConstants {
    public static final int kControllerPort = 0;
    public static final double kDeadband = 0.08;   // ignore tiny stick movements
  }

  public static class kDriveConstants {
    public static final int LeftLeaderCanId = 1;
    public static final int LeftFollowerCanId = 2;
    public static final int RightLeaderCanId = 3;
    public static final int RightFollowerCanId = 4;

    public static final int CurrentLimit = 40;     // amps, matches the 40 A breakers
    public static final double MaxOutput = 0.5;    // top speed, 0 to 1. Start slow!
  }
}

MaxOutput caps the robot at 50% speed. Raise it only after the robot drives correctly.

Step 3: Leaders and followers#

Each side has two motors in one gearbox. Instead of sending every command to both, you send commands to the leader, and tell the follower to copy it.

Following happens on the SPARK MAXes themselves, so the code only ever talks to two motors: left leader and right leader.

Step 4: Why one side is inverted#

The left and right gearboxes face opposite directions. If you send "forward" to both sides, the left wheels roll forward and the right wheels roll backward, and the robot spins in place.

The fix is to invert one side's leader, so "forward" means forward on both. We'll invert the right side. If your robot drives backward or spins when you push forward, you'll invert the other side instead (Step 8).

Step 5: The drive subsystem#

Create subsystems/DriveSubsystem.java (right-click subsystems → Create a new class/command → Subsystem (New)). Replace its contents:

package frc.robot.subsystems;

import java.util.function.DoubleSupplier;

import com.revrobotics.PersistMode;
import com.revrobotics.ResetMode;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkMaxConfig;

import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

import frc.robot.Constants.kDriveConstants;
import frc.robot.telemetry.Telemetry;
import frc.robot.telemetry.TelemetryTable;

public class DriveSubsystem extends SubsystemBase {
  // Four SPARK MAXes, all BRUSHED, because CIMs are brushed motors.
  private final SparkMax m_leftLeader =
      new SparkMax(kDriveConstants.LeftLeaderCanId, MotorType.kBrushed);
  private final SparkMax m_leftFollower =
      new SparkMax(kDriveConstants.LeftFollowerCanId, MotorType.kBrushed);
  private final SparkMax m_rightLeader =
      new SparkMax(kDriveConstants.RightLeaderCanId, MotorType.kBrushed);
  private final SparkMax m_rightFollower =
      new SparkMax(kDriveConstants.RightFollowerCanId, MotorType.kBrushed);

  // DifferentialDrive is WPILib's class for any drivetrain with a left side and a right side.
  // We only hand it the two leaders. The followers copy them.
  private final DifferentialDrive m_drive =
      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);

  // Everything this subsystem sends shows up under "Drive" in Elastic
  private final TelemetryTable m_telemetry = Telemetry.getTable("Drive");

  public DriveSubsystem() {
    // Left leader: normal direction
    SparkMaxConfig leftLeaderConfig = baseConfig();

    // Left follower: copy the left leader
    SparkMaxConfig leftFollowerConfig = baseConfig();
    leftFollowerConfig.follow(kDriveConstants.LeftLeaderCanId);

    // Right leader: inverted, because the right gearbox faces the other way
    SparkMaxConfig rightLeaderConfig = baseConfig();
    rightLeaderConfig.inverted(true);

    // Right follower: copy the right leader
    SparkMaxConfig rightFollowerConfig = baseConfig();
    rightFollowerConfig.follow(kDriveConstants.RightLeaderCanId);

    m_leftLeader.configure(leftLeaderConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
    m_leftFollower.configure(leftFollowerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
    m_rightLeader.configure(rightLeaderConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
    m_rightFollower.configure(rightFollowerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);

    // Cap the top speed while we're learning. Raise this in Constants later.
    m_drive.setMaxOutput(kDriveConstants.MaxOutput);
  }

  /** Settings every drive motor shares. */
  private static SparkMaxConfig baseConfig() {
    SparkMaxConfig config = new SparkMaxConfig();
    config.smartCurrentLimit(kDriveConstants.CurrentLimit);  // protects motors and breakers
    config.idleMode(IdleMode.kBrake);                        // stop quickly when sticks are released
    return config;
  }

  /**
   * Differential drive with two sticks: one value for the left side, one for the right.
   * Suppliers are read every loop, so the robot follows the sticks as they move.
   */
  public Command differentialDriveCommand(DoubleSupplier leftSpeed, DoubleSupplier rightSpeed) {
    return run(() -> m_drive.tankDrive(leftSpeed.getAsDouble(), rightSpeed.getAsDouble()))
        .withName("Differential Drive");
  }

  @Override
  public void periodic() {
    m_telemetry.log("Left Output", m_leftLeader.getAppliedOutput());
    m_telemetry.log("Right Output", m_rightLeader.getAppliedOutput());
    m_telemetry.log("Left Leader Current (A)", m_leftLeader.getOutputCurrent());
    m_telemetry.log("Left Follower Current (A)", m_leftFollower.getOutputCurrent());
    m_telemetry.log("Right Leader Current (A)", m_rightLeader.getOutputCurrent());
    m_telemetry.log("Right Follower Current (A)", m_rightFollower.getOutputCurrent());
    m_telemetry.log("Battery (V)", RobotController.getBatteryVoltage());
  }
}

The drive subsystem, piece by piece#

1. Four motor controllers, all brushed#

private final SparkMax m_leftLeader =
    new SparkMax(kDriveConstants.LeftLeaderCanId, MotorType.kBrushed);
private final SparkMax m_leftFollower =
    new SparkMax(kDriveConstants.LeftFollowerCanId, MotorType.kBrushed);
// ...and the same for the right leader and right follower

One SparkMax object for each SPARK MAX on the robot, each with its own CAN ID from Constants. They're MotorType.kBrushed because CIMs are brushed motors. (Lab 1's NEO was kBrushless.)

Notice the import at the top: import frc.robot.Constants.kDriveConstants;. Importing the inner class lets you write kDriveConstants.LeftLeaderCanId instead of Constants.kDriveConstants.LeftLeaderCanId.

2. The DifferentialDrive#

private final DifferentialDrive m_drive =
    new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);

DifferentialDrive is WPILib's class for any robot with a left side and a right side. It does the math for driving, and it has a safety feature: if your code stops sending it speeds, it stops the motors.

3. The telemetry table#

private final TelemetryTable m_telemetry = Telemetry.getTable("Drive");

Everything this subsystem sends goes in Telemetry → Drive in Elastic (Lab 1.1).

4. The constructor: one config per motor#

SparkMaxConfig leftLeaderConfig = baseConfig();

SparkMaxConfig leftFollowerConfig = baseConfig();
leftFollowerConfig.follow(kDriveConstants.LeftLeaderCanId);

SparkMaxConfig rightLeaderConfig = baseConfig();
rightLeaderConfig.inverted(true);

SparkMaxConfig rightFollowerConfig = baseConfig();
rightFollowerConfig.follow(kDriveConstants.RightLeaderCanId);

Each motor controller gets its own settings. All four start from baseConfig() (piece 5), then:

m_leftLeader.configure(leftLeaderConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
// ...and the same for the other three
m_drive.setMaxOutput(kDriveConstants.MaxOutput);

configure(...) sends each set of settings to its SPARK MAX, the same way as Lab 1. Then setMaxOutput(0.5) tells DifferentialDrive to scale every output down to at most 50%, so full stick is only half speed while you're learning.

5. Shared settings in one place#

private static SparkMaxConfig baseConfig() {
  SparkMaxConfig config = new SparkMaxConfig();
  config.smartCurrentLimit(kDriveConstants.CurrentLimit);
  config.idleMode(IdleMode.kBrake);
  return config;
}

All four motors need the same current limit and brake mode. Instead of writing those lines four times, this helper method builds a config with them and returns it. Each call makes a brand-new config, so changing one motor's config (like inverting it) doesn't change the others.

• private: only this class uses it.
• static: it doesn't use any of the subsystem's fields, so it doesn't need an object to run.

6. The drive command#

public Command differentialDriveCommand(DoubleSupplier leftSpeed, DoubleSupplier rightSpeed) {
  return run(() -> m_drive.tankDrive(leftSpeed.getAsDouble(), rightSpeed.getAsDouble()))
      .withName("Differential Drive");
}

Why a DoubleSupplier instead of a plain double? A double would be read once, when the command is created, and the robot would drive at that one speed forever. A supplier is read every loop, so the robot follows the sticks as they move.

7. Sending values to the dashboard#

@Override
public void periodic() {
  m_telemetry.log("Left Output", m_leftLeader.getAppliedOutput());
  m_telemetry.log("Left Leader Current (A)", m_leftLeader.getOutputCurrent());
  m_telemetry.log("Battery (V)", RobotController.getBatteryVoltage());
  // ...
}

periodic() runs every 20 ms, enabled or not. @Override means "this replaces the empty periodic() that SubsystemBase already has." The values let you check the robot without guessing: each side's output, every motor's current, and the battery voltage.

Step 6: Skid steer controls#

If you've ever run a skid steer loader, you already know how to drive this robot. A skid steer has two levers: the left lever runs the left wheels and the right lever runs the right wheels. Push both forward to go straight, push one further to turn, and push them opposite ways to spin in place.

The Xbox controller's two sticks are the two levers. In code, WPILib calls this kind of drivetrain a differential drive, and this control style tank drive.

m_drive.tankDrive(left, right) sends those two values straight to the two sides. It also squares them by default, which gives finer control at low speed: half stick gives about a quarter of full speed (Lesson 10).

Open RobotContainer.java. Add the imports and create the subsystem:

import edu.wpi.first.math.MathUtil;
import frc.robot.subsystems.DriveSubsystem;

private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();

Then set the drive's default command in configureBindings():

private void configureBindings() {
  // Left stick drives the left side, right stick drives the right side,
  // whenever no other command is using the drivetrain
  m_driveSubsystem.setDefaultCommand(
      m_driveSubsystem.differentialDriveCommand(
          () -> -MathUtil.applyDeadband(m_controller.getLeftY(), OperatorConstants.kDeadband),
          () -> -MathUtil.applyDeadband(m_controller.getRightY(), OperatorConstants.kDeadband)));
}

Delete or comment out the template's example subsystem and its auto in getAutonomousCommand() if you like; for now, make it return null so autonomous does nothing:

public Command getAutonomousCommand() {
  return null;   // no autonomous yet
}

Step 7: First drive, on blocks#

1. Build and Deploy.
2. Plug in the Xbox controller and check it's in slot 0 on the Driver Station USB tab.
3. Say "Enabling!" and enable TeleOperated.
4. Gently push one stick at a time, then both together.

Check every row:

Step 8: Fixing directions#

Change one thing at a time, deploy, and test again on blocks.

Step 9: Drive on the floor#

Once every row in Step 7 passes on blocks:

1. Put the robot on the floor in a big open space, with nobody in front of it.
2. Keep MaxOutput at 0.5.
3. Drive slowly: forward, backward, gentle turns, then a figure eight.
4. Watch Elastic while you drive (put the currents and battery voltage on Graph widgets). Look at the current on each side when you start moving and when you turn in place.

Spinning in place usually pulls the most current, because the wheels have to skid sideways across the carpet, the same reason a skid steer tears up a lawn when it turns. Watch the battery voltage drop while you do it. That's where brownouts come from (Lesson 4).

Step 10: Commit#

git add .
git commit -m "Lab 2: differential drive on the WCD"

Try it#

• Get every row in the Step 7 table to pass on blocks
• Drive a figure eight on the floor at 50% max output
• Add slow mode: while the right bumper is held, multiply both stick values by 0.4. (Hint: m_controller.rightBumper().getAsBoolean() inside the lambdas.)
• Drive a straight line across the room. Is it hard to push both sticks exactly the same? What could the code do to help?
• Change IdleMode.kBrake to IdleMode.kCoast for all motors. How does stopping feel different? Which would a driver want?
• Raise MaxOutput to 0.75 and watch the battery voltage while turning in place
• Challenge: some WCDs have 3 CIMs per side. What would you add to support a third motor on each side?

Check yourself#