Rangefinder.cpp

00001 #include "Rangefinder.h"
00002 #include "mbed.h"
00003 
00004 const int rgfAddr = 0x14;
00005 uint16_t rgfMeasurements[16];
00006 //char sensors_selection[] = {1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0}; // Only allows a maximum of 11 sensors simultaneously.
00007 //char sensors_selection[] = {1,0,0,0,1,0,1,0,1,0,1,0,1,0,1,0}; // Only allows a maximum of 11 sensors simultaneously.
00008 char sensors_selection[] = {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; // Only allows a maximum of 11 sensors simultaneously.
00009 
00010 I2C* _i2c;
00011 Mutex* _mutex;
00012 
00013 void init_rangefinder(I2C* i2c_in, Mutex* mutex_in)
00014 {
00015     _i2c = i2c_in;
00016     _mutex = mutex_in;
00017     wait(1);
00018     _mutex->lock();
00019     _i2c->write(rgfAddr, sensors_selection, 16);
00020     _mutex->unlock();
00021     wait(1);
00022 }
00023 
00024 int read_rangefinder()
00025 {
00026     int i2c_result;
00027 
00028     _mutex->lock();
00029     i2c_result = _i2c->read(rgfAddr, (char*)rgfMeasurements, sizeof(rgfMeasurements));
00030     _mutex->unlock();
00031 
00032     return i2c_result;
00033 }