IUT CACHAN
/
codeursMecatro
Codeurs MECATRO
Diff: codeurs.cpp
- Revision:
- 0:f462be86dc4a
- Child:
- 1:ac5eb2de660e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/codeurs.cpp Wed Mar 24 15:20:01 2021 +0000 @@ -0,0 +1,60 @@ +#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); +} + +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; +} +