File content as of revision 1:8569ac717e68:

#include "Encoder.h"
#include "mbed.h"

/** Create Encoder instance, initializing it. */
Encoder::Encoder(I2C* i2c_in, Mutex* mutex_in, char invert_in) :
    _i2c(i2c_in),
    _mutex(mutex_in)
{
    prev_L = readAbsolute();
    total_L = 0;
    _invert = invert_in;
};

/** Read encoder raw data (absolute value) */
long int Encoder::readAbsolute()
{
    int addr = 0x6C;
    char send[5];
    char receive[5];

    send[0] = 0x0E;

    _mutex->lock();
    _i2c->write(addr, send, 1);
    _i2c->read(addr, receive, 2);
    _mutex->unlock();

    short int val1 = receive[0];
    short int val2 = receive[1];
    val1 = (val1 & 0xf)*256;
    long int result = (val2 + val1);
    return result;
}

long int Encoder::incremental()
{
    short int next_L;
    short int dif;

    next_L = readAbsolute(); // Reads curent value of the encoder

    // !!! verificar: para ser correcto deve-se escrever a inversão ao contrário e trocar tb nos encoders.
    if(_invert == 1)
        dif = next_L-prev_L;
    else
        dif = -next_L + prev_L;      // Calculates the diference from last reading

    if(dif > 3000) {                     // Going back and pass zero
        total_L = total_L - 4096 + dif;
    }
    if(dif < 3000 && dif > 0) {              // Going front
        total_L = total_L + dif;
    }
    if(dif < -3000) {                    // Going front and pass zero
        total_L = total_L + 4096 + dif;
    }
    if(dif > -3000 && dif < 0) {             // going back
        total_L = total_L + dif;
    }
    prev_L = next_L;                     // Sets last reading for next iteration

    return total_L;
}

long int Encoder::readIncrementalValue()
{
    return total_L;
}