File content as of revision 1:6fa863d09d45:

#include "QEI.h"

const int8_t encodeTable[] = {0, -1,  1,  0, 1,  0,  0, -1, -1,  0,  0,  1, 0,  1, -1,  0 };

QEI::QEI(PinName A, PinName B, int ppr, Timer *t) : channelA(A), channelB(B)
{
    timer = t;
    channelA.rise(this, &QEI::encode);
    channelB.rise(this, &QEI::encode);
    channelA.fall(this, &QEI::encode);
    channelB.fall(this, &QEI::encode);
    _ppr = ppr;
    currState = 0;
    prevState = 0;
    position = 0;
    last_position = 0;
    time = timer->read();
}

float QEI::getAngle()
{
    return float(position) * 360.0/(_ppr*4.0);
}

float QEI::getSpeed()
{
    float dt = timer->read() - time;
    float dangle = float(position - last_position) * 360.0/(_ppr*4.0);
    float speed =  dangle / dt;
    time = timer->read();
    last_position = position;
    return speed;
}

float QEI::getDisAngle()
{
    float dangle = float(position - last_position) * 360.0/(_ppr*4.0);
    last_position = position;
    return dangle;
}

void QEI::reset()
{
    position = 0;
    last_position = 0;
}

void QEI::encode(void)
{
    int8_t chanA  = channelA.read();
    int8_t chanB  = channelB.read();
    currState = chanA | (chanB << 1);
    
    if (prevState != currState) {
        position += encodeTable[currState | (prevState<<2)];
        prevState = currState;
    }
}