File content as of revision 1:ac5eb2de660e:

#include "codeurs.h"

static const int16_t MAX = 16384;

Codeurs::Codeurs(PinName sda, PinName scl, int address) : _i2c(sda, scl), _address(address)
{
    _gauche = 0;
    _droit = 0;
    _g16 = 0;
    _d16 = 0;
}

bool Codeurs::test()
{
    char data = 5;
    _i2c.write(_address, &data, 1);
    _i2c.read(_address, &data, 1);
    return (data == 0x1F);
}

void Codeurs::read16(int16_t &gauche, int16_t &droit)
{
    char data[4];
    data[0] = 1;
    _i2c.write(_address, data, 1);
    _i2c.read(_address, data, 4);
    gauche = data[0]<<8 | (data[1]&0xFF);
    droit = data[2]<<8 | (data[3]&0xFF);;
}

void Codeurs::reset()
{
    char data[2] = {0, 1};
    _i2c.write(_address, data, 2);
    _gauche = 0;
    _droit = 0;
    _g16 = 0;
    _d16 = 0;    
}

void Codeurs::read(int32_t &gauche, int32_t &droit)
{
    int16_t ng, nd;
    read16(ng, nd);
    if ((ng > MAX) && (_g16 < -MAX)) {
        _gauche = _gauche - _g16 + ng - 65536;
    } else if ((ng < -MAX) && (_g16 > MAX)) {
        _gauche = _gauche - _g16 + ng + 65536;
    } else {
        _gauche = _gauche - _g16 + ng;
    }
    _g16 = ng;
    if ((nd > MAX) && (_d16 < -MAX)) {
        _droit = _droit - _d16 + nd - 65536;
    } else if ((nd < -MAX) && (_d16 > MAX)) {
        _droit = _droit - _d16 + nd + 65536;
    } else {
        _droit = _droit - _d16 + nd;
    }
    _d16 = nd;
    gauche = _gauche;
    droit = _droit;
}