Encoders in FRC

Encoders are hardware that measure rotation. The encoder generates pulses that contains the information.

Rotation is directly proportional to distance. One revolution of the encoder can relate to a certain amount of distance:

encoder ticks * K = distance, where K is the distance of one rotation divided by the ticks of one rotation. K is a decimal (double).

A very simple way of determining K is to read the encoder values (console, SmartDashboard, etc.) while rotating the encoder. For example, K for a drive train can be found by moving the robot forward one rotation of the wheels. The distance traveled is the circumference of the wheels, and the change in ticks (final tick - initial tick) can be found using the console/SmartDashboard. The units for distance can be arbitrarily chosen depending on the need.

Encoders in WPILIB

WPILIB provides a class to read encoder values:

// a = the pin that Channel A is connected to in the DI/O
// b = the pin that Channel B is connected to in the DI/O
// reverse = true/false if the encoder is physically backwards (e.g. if the robot moves forward and the encoder values are negative, setting this parameter to `true` will make the sign correct)
Encoder encoder = new Encoder(a, b, reverse);

The encoder ticks can be obtained using encoder.get(). You could use the equation above to find the distance that the encoder is reading:

double distance = encoder.get() * K;

When I did FRC this was the only way to obtain distance. WPILIB has been updated, and provides distance calculations automatically:

Encoder encoder = new Encoder(a, b, reverse); // Same as before
encoder.setDistancePerPulse(K); // Set the distance conversion for the encoder

...

double distance = encoder.getDistance();

Displacement

The use of "distance" for encoder readings is not actually true. Encoders read displacement, not distance (they are interchanged due to their use). This has a couple of meanings. First, encoder distance can be negative. This signals if the encoder distance was in reverse or not. Second, the getDistance method doesn't always increase; it returns the displacement from the initial reading of the encoder (encoders initialize with their distance reading zero).

If you walk 10ft forward, your distance from the start is 10ft. If you walk backwards 3ft, your displacement is 7ft, because your starting point didn't change.

What does this mean for your robot? The encoder can't know the height of your lift, only the distance it has rasied/lowered from the start of your robot code. Start your lift halfway up, and your encoder will read distance from that point instead of the bottom of the lift, which is desired.

This is easily fixed, though, using homing. The "home" of the lift is the bottom. This is where we want the encoder to read as "zero". In robotInit and autonomousInit in Robot.java, reset the encoder:

encoder.reset();

This "zeros out" the encoder. There is one catch, and it is extremely important; your robot must have its lift down when the robot is turned on and when autonomous is started. You can add a reminder note next to your power switch or something similar to remind everyone. This is actually exactly what the SOTABots did in 2018, and it worked fine.

This is called "manual homing". There is alternative which is preferred but needs some more hardware and code, "automatic homing". Place a limit switch at the bottom of the lift so that the lift triggers it when it is down. Make a command called HomeLift that does the following:

1. Set the lift motor to lower the lift (moderately slowly, we don't want to introduce slack. You can experiment with this)
2. When the limit switch is triggered, stop the lift motor (it helps if the motor is in brake mode, eliminates slack)
3. Reset the encoder using encoder.reset()

This will automatically reset the encoder at the correct position. You can trigger this command in autonomousInit and teleopInit so that no matter how you enable the robot it will always zero itself. SOTABots did something similar in 2016; a catapault was automatically lowered after firing by triggering a sensor when the catapault was in the correct place.

Once the lift is homed, you shouldn't need to reset the encoder unless the robot powers down.

Moving to a distance

The final question to answer is how to move the robot/raise the lift to the correct distance using the encoder? This is a pretty classic problem in robotics. There are two relatively easy options: a bang-bang controller and a PID controller.

PID controllers are efficient, but hard to set up. I don't recommend messing with PID controllers until off-season, they require a lot of trial and error to perfect. Therefore, the easiest option is a bang-bang controller.

A bang-bang controller is extremely intuitive:

1. While the lift is lower than the target value, set the motor to raise the lift
2. While the lift is higher than the target value, set the motor to lower the lift

This can easily be integrated into a command (for example, MoveLiftToHeight). Using these two steps alone will result in the lift jittering at the desired height. This is because the lift can never reach exactly the distance you set it to, especially with a bang-bang controller. To combat this, a threshold is added. A threshold is "tolerance", it allows the lift to be within some amount of error to the desired height (say, ±1 inch).

Here is an example of a MoveLiftToHeight command. This command assumes that the lift is homed and assumes that there is a subsystem for the lift:

public class MoveLiftToHeight extends Command {
    private double m_height; // Height is in inches
    private double m_speed; // Speed is a _positive_ number, between 0 and 1

    private final double m_tolerance = 1.0f;

    public MoveLiftToHeight(double height, double speed) {
        m_height = height;
        m_speed = speed;
        
        // I'm assuming there is a subsystem for the lift called 'lift'
        requires(Robot.lift);
    }

    @Override
    protected void initialize() {
    }

    @Override
    protected void execute() {
        // Robot.lift.getHeight is the `encoder.getDistance` method
        if(Robot.lift.getHeight() < m_height) {
            // The lift is lower than it should be.
            // Raise the lift.
            Robot.lift.setLiftSpeed(Math.abs(m_speed));
        }
        if(Robot.lift.getHeight() > m_height) {
            // The lift is higher than it should be.
            // Lower the lift.
            Robot.lift.setLiftSpeed(-1 * Math.abs(m_speed));
        }
    }

    @Override
    protected boolean isFinished() {
        final double distanceToHeight = Robot.lift.getHeight() - m_height; // This can be negative depending on whether the lift is higher or lower than the desired height.
        final boolean isWithinTolerance = Math.abs(distanceToHeight) < m_tolerance;
        return isWithinTolerance;
    }

    @Override
    protected void end() {
        // Stop the lift
        Robot.lift.setLiftSpeed(0);
    }

    @Override
    protected void interrupted() {
        // Stop the lift
        Robot.lift.setLiftSpeed(0);
    }
}