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
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 }
Generated on Fri Jul 15 2022 00:36:05 by 1.7.2