Diff: Rangefinder.cpp

Revision:

4:560d24c0e5f8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Rangefinder.cpp	Thu Apr 19 15:35:29 2018 +0000
@@ -0,0 +1,33 @@
+#include "Rangefinder.h"
+#include "mbed.h"
+
+const int rgfAddr = 0x14;
+uint16_t rgfMeasurements[16];
+//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.
+//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.
+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.
+
+I2C* _i2c;
+Mutex* _mutex;
+
+void init_rangefinder(I2C* i2c_in, Mutex* mutex_in)
+{
+    _i2c = i2c_in;
+    _mutex = mutex_in;
+    wait(1);
+    _mutex->lock();
+    _i2c->write(rgfAddr, sensors_selection, 16);
+    _mutex->unlock();
+    wait(1);
+}
+
+int read_rangefinder()
+{
+    int i2c_result;
+
+    _mutex->lock();
+    i2c_result = _i2c->read(rgfAddr, (char*)rgfMeasurements, sizeof(rgfMeasurements));
+    _mutex->unlock();
+
+    return i2c_result;
+}
\ No newline at end of file