Fabio Faria / ISR_Mini-explorer_tmp

Energy harvesting mobile robot. Developed at Institute of Systems and Robotics — University of Coimbra.

Dependencies:   RF24

Fork of ISR_Mini-explorer by ISR UC

Encoder.cpp@4:560d24c0e5f8, 2018-04-19 (annotated)

Committer:
fabiofaria
Date:
Thu Apr 19 15:35:29 2018 +0000
Revision:
4:560d24c0e5f8
Parent:
1:8569ac717e68 
Initial commit.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ISR 1:8569ac717e68 1 #include "Encoder.h"
ISR 1:8569ac717e68 2 #include "mbed.h"
ISR 1:8569ac717e68 3
ISR 1:8569ac717e68 4 /** Create Encoder instance, initializing it. */
ISR 1:8569ac717e68 5 Encoder::Encoder(I2C* i2c_in, Mutex* mutex_in, char invert_in) :
ISR 1:8569ac717e68 6 _i2c(i2c_in),
ISR 1:8569ac717e68 7 _mutex(mutex_in)
ISR 1:8569ac717e68 8 {
ISR 1:8569ac717e68 9 prev_L = readAbsolute();
ISR 1:8569ac717e68 10 total_L = 0;
ISR 1:8569ac717e68 11 _invert = invert_in;
ISR 1:8569ac717e68 12 };
ISR 1:8569ac717e68 13
ISR 1:8569ac717e68 14 /** Read encoder raw data (absolute value) */
ISR 1:8569ac717e68 15 long int Encoder::readAbsolute()
ISR 1:8569ac717e68 16 {
ISR 1:8569ac717e68 17 int addr = 0x6C;
ISR 1:8569ac717e68 18 char send[5];
ISR 1:8569ac717e68 19 char receive[5];
ISR 1:8569ac717e68 20
ISR 1:8569ac717e68 21 send[0] = 0x0E;
ISR 1:8569ac717e68 22
ISR 1:8569ac717e68 23 _mutex->lock();
ISR 1:8569ac717e68 24 _i2c->write(addr, send, 1);
ISR 1:8569ac717e68 25 _i2c->read(addr, receive, 2);
ISR 1:8569ac717e68 26 _mutex->unlock();
ISR 1:8569ac717e68 27
ISR 1:8569ac717e68 28 short int val1 = receive[0];
ISR 1:8569ac717e68 29 short int val2 = receive[1];
ISR 1:8569ac717e68 30 val1 = (val1 & 0xf)*256;
ISR 1:8569ac717e68 31 long int result = (val2 + val1);
ISR 1:8569ac717e68 32 return result;
ISR 1:8569ac717e68 33 }
ISR 1:8569ac717e68 34
ISR 1:8569ac717e68 35 long int Encoder::incremental()
ISR 1:8569ac717e68 36 {
ISR 1:8569ac717e68 37 short int next_L;
ISR 1:8569ac717e68 38 short int dif;
ISR 1:8569ac717e68 39
ISR 1:8569ac717e68 40 next_L = readAbsolute(); // Reads curent value of the encoder
ISR 1:8569ac717e68 41
ISR 1:8569ac717e68 42 // !!! verificar: para ser correcto deve-se escrever a inversão ao contrário e trocar tb nos encoders.
ISR 1:8569ac717e68 43 if(_invert == 1)
ISR 1:8569ac717e68 44 dif = next_L-prev_L;
ISR 1:8569ac717e68 45 else
ISR 1:8569ac717e68 46 dif = -next_L + prev_L; // Calculates the diference from last reading
ISR 1:8569ac717e68 47
ISR 1:8569ac717e68 48 if(dif > 3000) { // Going back and pass zero
ISR 1:8569ac717e68 49 total_L = total_L - 4096 + dif;
ISR 1:8569ac717e68 50 }
ISR 1:8569ac717e68 51 if(dif < 3000 && dif > 0) { // Going front
ISR 1:8569ac717e68 52 total_L = total_L + dif;
ISR 1:8569ac717e68 53 }
ISR 1:8569ac717e68 54 if(dif < -3000) { // Going front and pass zero
ISR 1:8569ac717e68 55 total_L = total_L + 4096 + dif;
ISR 1:8569ac717e68 56 }
ISR 1:8569ac717e68 57 if(dif > -3000 && dif < 0) { // going back
ISR 1:8569ac717e68 58 total_L = total_L + dif;
ISR 1:8569ac717e68 59 }
ISR 1:8569ac717e68 60 prev_L = next_L; // Sets last reading for next iteration
ISR 1:8569ac717e68 61
ISR 1:8569ac717e68 62 return total_L;
ISR 1:8569ac717e68 63 }
ISR 1:8569ac717e68 64
ISR 1:8569ac717e68 65 long int Encoder::readIncrementalValue()
ISR 1:8569ac717e68 66 {
ISR 1:8569ac717e68 67 return total_L;
ISR 1:8569ac717e68 68 }