Lab 6: Count Turns with an Encoder#

Why it matters#

The limit switch told you whether something happened. The knob told you where a dial was. An encoder tells you how far a shaft has turned, and how fast it's turning, which is what you need to move a mechanism a precise amount.

In this lab you'll read the encoder on the test bed's 775pro wheel, turn its raw counts into wheel rotations, and then try to make the wheel turn exactly 10 times two different ways. Neither way will be perfect. Seeing exactly how they miss is what makes PID make sense in Lab 9.

Before you start: finish Lab 5. You'll add to the same TestBed project. Lesson 8 explains encoders and units. Lessons 4.1 (the 775pro) and 4.3 (the Through Bore Encoder) cover this lab's hardware.

Read this#

• Encoders – Software
• Encoders – Hardware

Walkthrough#

The hardware#

Wiring the encoder#

The REV Through Bore Encoder's cable ends in four 3-pin plugs. Only two go to the roboRIO:

The encoder end of the cable is the white JST plug (right). The four 3-pin Dupont plugs go to the roboRIO. Each has a red wire (5 V), a black wire (ground), and one colored signal wire.

Which way around? Each roboRIO DIO port is three pins in a row, and the roboRIO has a label printed next to each row of pins: the ground symbol (⏚), V (5 V), and S (signal). Turn each plug so the black wire is on ground (⏚), the red wire is on V, and the blue or yellow wire is on S. Every plug on this cable has its own red and black, so each one powers itself; that's normal. (Lesson 4.3 has more.)

We count turns of the wheel, not the motor. A 775pro spins about 18,700 RPM with no load, so 10 turns of the motor shaft would be over in a blink. The gearbox slows it down enough to watch.

How a quadrature encoder counts#

A quadrature encoder sends two square-wave signals, A and B, a quarter-cycle apart. Each full cycle means the shaft turned a tiny bit. Which signal changes first tells the roboRIO the direction.

Which channel rises first tells the roboRIO the direction. From the WPILib docs, Encoders - Software (CC BY 4.0).

The roboRIO counts every rising and falling edge on both channels: that's 4 counts per cycle ("4x decoding"). A REV Through Bore Encoder makes 2048 cycles per revolution, which is 8192 counts.

The roboRIO's FPGA chip counts in hardware, so it never misses a count, even though your code only runs every 20 ms.

Step 1: Check the SPARK MAX#

Open the REV Hardware Client and write down the CAN ID of the 775pro's SPARK MAX. It's a brushed motor, so this SPARK MAX must be in brushed mode, unlike the NEO's.

Step 2: Constants#

Add this class inside Constants in Constants.java. Fill in your real CAN ID and DIO ports.

public static class kWheelConstants {
    public static final int MotorCanId = 9;
    public static final boolean MotorInverted = false;

    public static final int EncoderChannelA = 0;
    public static final int EncoderChannelB = 1;
    public static final boolean EncoderInverted = false;
    // WPILib counts in quadrature CYCLES. Through Bore Encoder = 2048 (not 8192).
    public static final int EncoderCyclesPerRev = 2048;

    public static final int CurrentLimitAmps = 30;   // protects the 775pro
    public static final double MaxVolts = 6.0;       // keeps PID tests gentle in Lab 9
}

Step 3: The subsystem#

Create subsystems/WheelSubsystem.java. This part sets up the motor and encoder, converts to wheel rotations, and publishes telemetry. No movement commands yet.

package frc.robot.subsystems;

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.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.kWheelConstants;
import frc.robot.telemetry.Telemetry;
import frc.robot.telemetry.TelemetryTable;

public class WheelSubsystem extends SubsystemBase {
    private final SparkMax m_motor =
        new SparkMax(kWheelConstants.MotorCanId, MotorType.kBrushed);

    private final Encoder m_encoder = new Encoder(
        kWheelConstants.EncoderChannelA,
        kWheelConstants.EncoderChannelB,
        kWheelConstants.EncoderInverted,
        EncodingType.k4X);

    private double m_target = 0.0;     // where the current attempt is trying to stop
    private double m_lastVolts = 0.0;  // for the dashboard

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

    public WheelSubsystem() {
        SparkMaxConfig config = new SparkMaxConfig();
        config.smartCurrentLimit(kWheelConstants.CurrentLimitAmps);
        config.idleMode(IdleMode.kBrake);
        config.inverted(kWheelConstants.MotorInverted);
        m_motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);

        // One "pulse" = one quadrature cycle. After this, getDistance() is in wheel rotations.
        m_encoder.setDistancePerPulse(1.0 / kWheelConstants.EncoderCyclesPerRev);
    }

    public double getRotations() { return m_encoder.getDistance(); }
    public double getRps()       { return m_encoder.getRate(); }

    private void setVolts(double volts) {
        m_lastVolts = volts;
        m_motor.setVoltage(volts);
    }

    private void stop() {
        m_lastVolts = 0.0;
        m_motor.stopMotor();
    }

    /** Every attempt turns a number of rotations FROM WHEREVER THE WHEEL IS NOW. */
    private void setTargetFromHere(double rotations) {
        m_target = getRotations() + rotations;
    }

    public Command zero() {
        return runOnce(m_encoder::reset).ignoringDisable(true).withName("Zero");
    }

    public Command idle() {
        return run(this::stop).withName("Idle");
    }

    @Override
    public void periodic() {
        m_telemetry.log("Position (rot)", getRotations());
        m_telemetry.log("Target (rot)", m_target);
        m_telemetry.log("Error (rot)", m_target - getRotations());
        m_telemetry.log("Velocity (rot per s)", getRps());
        m_telemetry.log("Output (V)", m_lastVolts);
        m_telemetry.log("Current (A)", m_motor.getOutputCurrent());
    }
}

The wheel subsystem, piece by piece#

1. The motor: a brushed 775pro#

private final SparkMax m_motor =
    new SparkMax(kWheelConstants.MotorCanId, MotorType.kBrushed);

The same kind of field as Lab 1, but this SPARK MAX drives a 775pro, which is brushed, on CAN ID 9.

2. The encoder#

private final Encoder m_encoder = new Encoder(
    kWheelConstants.EncoderChannelA,
    kWheelConstants.EncoderChannelB,
    kWheelConstants.EncoderInverted,
    EncodingType.k4X);

Encoder is a WPILib class: the encoder is wired to the roboRIO, not to the SPARK MAX.

3. Remembering the target and the output#

private double m_target = 0.0;     // where the current attempt is trying to stop
private double m_lastVolts = 0.0;  // for the dashboard

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

These aren't final, because they change while the robot runs. m_target is where the current move is trying to stop. m_lastVolts remembers the last voltage sent, so the dashboard can show it.

4. The constructor#

SparkMaxConfig config = new SparkMaxConfig();
config.smartCurrentLimit(kWheelConstants.CurrentLimitAmps);
config.idleMode(IdleMode.kBrake);
config.inverted(kWheelConstants.MotorInverted);
m_motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);

m_encoder.setDistancePerPulse(1.0 / kWheelConstants.EncoderCyclesPerRev);

The first five lines set up the motor like Lab 1, with a 30 A limit (a 775pro overheats quickly) and a setting to flip the motor's direction if needed.

The last line is the important one. setDistancePerPulse tells the encoder how far one "pulse" (one full cycle) moves. One wheel rotation is 2048 cycles, so one cycle is 1.0 / 2048 of a rotation. After this line, getDistance() answers in wheel rotations instead of raw counts. (The 4X counting is handled for you; that's why it's 2048, not 8192.)

5. Small methods with clear names#

public double getRotations() { return m_encoder.getDistance(); }
public double getRps()       { return m_encoder.getRate(); }

These are written on one line each because they're so short. Giving them names means the rest of the code reads like English: getRotations() >= m_target.

6. Setting the motor, and stopping it#

private void setVolts(double volts) {
    m_lastVolts = volts;
    m_motor.setVoltage(volts);
}

private void stop() {
    m_lastVolts = 0.0;
    m_motor.stopMotor();
}

setVoltage asks for a number of volts (up to about 12), instead of a fraction like set(0.5). 4 V is 4 V whether the battery is full or tired, so tests are more repeatable. Both methods also update m_lastVolts, so the dashboard always shows what the motor was told. They're private: only this subsystem's commands can move the motor.

7. Picking a target from where the wheel is now#

private void setTargetFromHere(double rotations) {
    m_target = getRotations() + rotations;
}

"Turn 10 rotations" means 10 more from here, not "go to position 10." If the wheel is at 3.2, the target becomes 13.2. That way you can run an attempt again without zeroing first.

8. Two simple commands#

public Command zero() {
    return runOnce(m_encoder::reset).ignoringDisable(true).withName("Zero");
}

public Command idle() {
    return run(this::stop).withName("Idle");
}

9. The dashboard#

@Override
public void periodic() {
    m_telemetry.log("Position (rot)", getRotations());
    m_telemetry.log("Target (rot)", m_target);
    m_telemetry.log("Error (rot)", m_target - getRotations());
    // ...velocity, output, and current
}

Error is the target minus the position: how far is left to go. It's positive before the target and negative after an overshoot. You'll watch it a lot in this lab and in Lab 9.

Step 4: Bind the wheel controller#

Open RobotContainer.java. Add the wheel subsystem and a second controller:

  private final WheelSubsystem m_wheel = new WheelSubsystem();
  private final CommandXboxController m_wheelController = new CommandXboxController(1);

In configureBindings(), keep everything from earlier labs and add:

    // ---- Wheel (controller 1) ----
    m_wheel.setDefaultCommand(m_wheel.idle());
    m_wheelController.back().onTrue(m_wheel.zero());

You'll add the move commands in Steps 6 and 7.

Step 5: Check the sensor before anything else#

Deploy, enable nothing yet, and open the dashboard.

1. Direction. Turn the wheel by hand in the direction the motor will push. Position must go up. If it goes down, flip kEncoderInverted.
2. Scale. Press Back on the wheel controller to zero, then turn the wheel exactly one full turn (put a tape mark on it). Position should read about 1.00. If it reads about 0.25 or 4.0, your cycles-per-rev is wrong.

Step 5b: Look at the raw counts#

m_encoder.getDistance() already gives rotations. To see what it's built from, add two more lines to periodic():

        m_telemetry.log("Raw Counts", m_encoder.getRaw());
        m_telemetry.log("Cycles", m_encoder.get());

Zero the encoder and turn the wheel exactly once by hand. You should see about 8192 raw counts, about 2048 cycles, and 1.00 rotations. The setDistancePerPulse(1.0 / 2048) line is what turns cycles into rotations.

Step 6: Attempt 1, timed (open loop)#

The idea: "If it turns about 5 times a second at 4 volts, running it for 2 seconds should give 10 turns."

public Command turnTimed(double volts, double seconds) {
    return startEnd(() -> setVolts(volts), this::stop)
        .withTimeout(seconds)
        .withName("Attempt 1: Timed");
}

Nowhere does this code read the encoder. That's what open loop means: it acts without checking the result.

1. Run it once with a guess like turnTimed(4.0, 2.0). See how many turns you got.
2. Adjust the time until one run lands near 10.
3. Now run it three times without changing anything, and record the results.
4. Hold a cloth lightly against the wheel (friction, like carpet) and run it again.

What you'll see: Each run lands somewhere different, and the extra friction cuts it short. The code never looks at the encoder, so it has no idea how far the wheel actually went. Battery level, temperature, and friction all change the result.

Bind it on the wheel controller:

    m_wheelController.a().onTrue(m_wheel.turnTimed(4.0, 2.0));    // Attempt 1

Step 7: Attempt 2, bang-bang (stop when you get there)#

The idea: "Use the encoder. Run until the position reaches 10, then stop."

public Command turnBangBang(double rotations, double volts) {
    return runOnce(() -> setTargetFromHere(rotations))
        .andThen(run(() -> setVolts(volts)))         // full speed the whole way
        .until(() -> getRotations() >= m_target)     // stop the moment we arrive
        .withTimeout(5.0)                            // safety: give up if the encoder never gets there
        .finallyDo(this::stop)
        .withName("Attempt 2: Bang-Bang");
}

Read it as a list of steps that run in order:

>= is "greater than or equal to." It uses >= instead of == because the position jumps past 10.000 between loops; it would almost never be exactly 10.

It's called bang-bang because the output is either fully on or fully off, nothing in between.

Run turnBangBang(10, 4.0) three times. Then try it at 2 V and at 6 V.

What you'll see: Much more consistent than timing, but it always overshoots. When the code says stop, the wheel is still moving at full speed, and it takes time to coast to a halt, even in brake mode. The faster it's going, the further it overshoots. Watch Error go negative on the dashboard.

Bind it:

    m_wheelController.b().onTrue(m_wheel.turnBangBang(10, 4.0));  // Attempt 2

Both use onTrue because each command ends by itself. Press once and let it run.

The scorecard#

Run each attempt three times and record the final position from the dashboard. Keep this table. You'll add three more rows in Lab 9.

Commit#

git add .
git commit -m "Lab 6: encoder, timed and bang-bang"

Try it#

• Check direction and scale, and get 1.00 for one turn by hand
• Watch Raw Counts, Cycles, and Position for one turn. Explain how they're related.
• Spin the wheel by hand and watch Velocity. What's the fastest you can spin it, in rotations per second?
• Complete the scorecard
• Press B three times in a row, waiting for each move to finish. Does the overshoot add up?
• Hold a cloth lightly against the wheel during a timed run and a bang-bang run. Which one still reaches 10?
• Challenge: write turnBangBang for backward moves too (negative rotations)

Check yourself#