Robot.cpp

#include "WPILib.h"
#include "S5.h"

#define dashboard SmartDashboard::GetInstance()

static void PeriodicTask(S5 *encoder) {
    while (1) {
        encoder->Periodic();
        Wait(0.005);
    }
}

class Robot : public IterativeRobot {
public:
    Task periodic;
    S5 encoder;

    Robot():
        periodic("PeriodicTask", (FUNCPTR)PeriodicTask),
        encoder(1, 2, 3)
    {}

    virtual void RobotInit() {
        NetworkTable::Initialize();
    }

    virtual void TeleopInit() {
        periodic.Start((INT32)&encoder);
    }

    virtual void DisabledInit() {
        periodic.Stop();
    }

    virtual void TeleopPeriodic() {
        dashboard->PutInt("Raw Count", encoder.GetRawCount());
        dashboard->PutInt("Raw Position", encoder.GetRawPosition());
        dashboard->PutDouble("Position", encoder.GetPosition());
    }
};

START_ROBOT_CLASS(Robot);

S5.cpp

#include "S5.h"

S5::S5(int aChannel, int bChannel, int indexChannel):
    a(aChannel),
    b(bChannel),
    index(indexChannel),
    quadratureCounter(CounterBase::k2X, &a, &b, false),
    indexCounter(index),
    offset(0),
    previousCount(0)
{
}

void S5::Periodic() {
    if (indexCounter.Get() > 0) {
        indexCounter.Reset();
        offset = 0;
    } else {
        int newCount = quadratureCounter.Get();
        offset += newCount - previousCount;
        offset %= 360;
        previousCount = newCount;
    }
}

int S5::GetRawCount() {
    return quadratureCounter.Get();
}

int S5::GetRawPosition() {
    return offset;
}

float S5::GetPosition() {
    return offset / 512.0 * 360;
}

S5.h

#ifndef _S5_H_
#define _S5_H_

#include "WPILib.h"

class S5 {
public:
    S5(int aChannel, int bChannel, int index);
    void Periodic();
    int GetRawCount();
    int GetRawPosition();
    float GetPosition();

private:
    DigitalInput a;
    DigitalInput b;
    DigitalInput index;
    Counter quadratureCounter;
    Counter indexCounter;
    int offset;
    int previousCount;
};

#endif