Energy harvesting mobile robot. Developed at Institute of Systems and Robotics — University of Coimbra.
Dependents: Mapping VirtualForces_debug OneFileToRuleThemAll VirtualForces_with_class ... more
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