LineSensorArray + Srf10 added.

1 files changed, 60 insertions, 0 deletions

diff --git a/Srf10.cpp b/Srf10.cpp
new file mode 100644
index 0000000..329fc8a
--- /dev/null
+++ b/Srf10.cpp
@@ -0,0 +1,60 @@
+#include "Srf10.h"
+
+#include "i2c.h"
+
+
+Srf10::Srf10(uint8_t id)
+{
+	this->id = id;
+	
+	gain = 16;
+	range = 255;
+	
+	unit = Inches;
+	
+	distance = 0;
+	
+	firmware = readFirmware();
+}
+
+int Srf10::readFirmware() {
+	uint8_t byte;
+	
+	if(!I2CSendByte(id, 0)) return -1;
+	if(!I2CRecvByte(id, &byte)) return -1;
+	
+	return byte;
+}
+
+bool Srf10::setGain(uint8_t gain) {
+	uint8_t data[2] = {1, gain};
+	
+	if(!I2CSend(id, data, 2)) return false;
+	
+	this->gain = gain;
+	return true;
+}
+
+bool Srf10::setRange(uint8_t range) {
+	uint8_t data[2] = {2, range};
+	
+	if(!I2CSend(id, data, 2)) return false;
+	
+	this->range = range;
+	return true;
+}
+
+long Srf10::updateDistance() {
+	uint8_t data[2] = {0, unit};
+	
+	if(!I2CSend(id, data, 2)) return -1;
+	
+	while(readFirmware() != 0xFF);
+	
+	if(!I2CSendByte(id, 2)) return -1;
+	if(!I2CRecv(id, data, 2)) return -1;
+	
+	distance = (((uint16_t)data[0]) << 8) | data[1];
+	
+	return distance;
+}