Diff: Rangefinder.cpp

Revision:

4:f6fddeae358e

Child:

5:43215ff15665

diff -r 215c9544480d -r f6fddeae358e Rangefinder.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Rangefinder.cpp	Thu Apr 19 15:44:26 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,1,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