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;
+}
+