Encoder.cpp

00001 #include "Encoder.h"
00002 #include "mbed.h"
00003 
00004 /** Create Encoder instance, initializing it. */
00005 Encoder::Encoder(I2C* i2c_in, Mutex* mutex_in, char invert_in) :
00006     _i2c(i2c_in),
00007     _mutex(mutex_in)
00008 {
00009     prev_L = readAbsolute();
00010     total_L = 0;
00011     _invert = invert_in;
00012 };
00013 
00014 /** Read encoder raw data (absolute value) */
00015 long int Encoder::readAbsolute()
00016 {
00017     int addr = 0x6C;
00018     char send[5];
00019     char receive[5];
00020 
00021     send[0] = 0x0E;
00022 
00023     _mutex->lock();
00024     _i2c->write(addr, send, 1);
00025     _i2c->read(addr, receive, 2);
00026     _mutex->unlock();
00027 
00028     short int val1 = receive[0];
00029     short int val2 = receive[1];
00030     val1 = (val1 & 0xf)*256;
00031     long int result = (val2 + val1);
00032     return result;
00033 }
00034 
00035 long int Encoder::incremental()
00036 {
00037     short int next_L;
00038     short int dif;
00039 
00040     next_L = readAbsolute(); // Reads curent value of the encoder
00041 
00042     // !!! verificar: para ser correcto deve-se escrever a inversão ao contrário e trocar tb nos encoders.
00043     if(_invert == 1)
00044         dif = next_L-prev_L;
00045     else
00046         dif = -next_L + prev_L;      // Calculates the diference from last reading
00047 
00048     if(dif > 3000) {                     // Going back and pass zero
00049         total_L = total_L - 4096 + dif;
00050     }
00051     if(dif < 3000 && dif > 0) {              // Going front
00052         total_L = total_L + dif;
00053     }
00054     if(dif < -3000) {                    // Going front and pass zero
00055         total_L = total_L + 4096 + dif;
00056     }
00057     if(dif > -3000 && dif < 0) {             // going back
00058         total_L = total_L + dif;
00059     }
00060     prev_L = next_L;                     // Sets last reading for next iteration
00061 
00062     return total_L;
00063 }
00064 
00065 long int Encoder::readIncrementalValue()
00066 {
00067     return total_L;
00068 }