Diff: Encoder.cpp

Revision:

1:8569ac717e68

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder.cpp	Tue Feb 20 13:37:47 2018 +0000
@@ -0,0 +1,68 @@
+#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;
+}
\ No newline at end of file