Energy harvesting mobile robot. Developed at Institute of Systems and Robotics — University of Coimbra.
Dependents: Mapping VirtualForces_debug OneFileToRuleThemAll VirtualForces_with_class ... more
Encoder.cpp
- Committer:
- ISR
- Date:
- 2018-02-20
- Revision:
- 1:8569ac717e68
File content as of revision 1:8569ac717e68:
#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; }