1 files changed, 107 insertions, 0 deletions
diff --git a/source/qFix/qfixSonar.h b/source/qFix/qfixSonar.h
new file mode 100644
index 0000000..96d26ce
--- /dev/null
+++ b/source/qFix/qfixSonar.h
@@ -0,0 +1,107 @@
+//------------------------------------------------------------------
+// qfixSonar.h
+//
+// This file contains the class Sonar which represents a
+// sonar modul SFH04/08.
+//
+// Copyright 2005 by KTB mechatronics GmbH
+// Author: Florian Schrapp, Stefan Enderle
+// Version 1.0
+//------------------------------------------------------------------
+
+
+#ifndef qfixSonar_h
+#define qfixSonar_h
+
+#include "qfix.h"
+#include "qfixI2C.h"
+
+
+class Sonar
+{
+private:
+  uint8_t id;
+  I2C i2c;
+
+public:
+	
+  /** Constructor with given ID
+  */
+  Sonar(uint8_t ID);
+	
+  /** Change logical device ID
+  */
+  void changeID(uint8_t newID);
+	
+  /** Returns the distance to the next object in millimeter
+  */
+  int distance();	
+};
+
+
+
+Sonar::Sonar(uint8_t ID) : i2c()
+{
+  id = 112+ID;
+	
+  uint8_t buf[10];
+  buf[0]=2;				// register 2 = select range register
+  buf[1]=48;			// set maximum range to 2107 mm (formula: range=(register*43)+43 mm)
+  i2c.send(id, buf, 2);	
+
+  buf[0]=1;				// register 1 = amplification
+  buf[1]=4;				// improve detection along center axis
+  i2c.send(id, buf, 2);	
+}
+
+
+void Sonar::changeID(uint8_t newID)
+{
+  if(newID>=0 && newID<=15){		// only 16 IDs allowed
+    uint8_t buf[10];
+
+    buf[0]=0;	
+    buf[1]=160;
+    i2c.send(id, buf, 2);	
+
+    buf[0]=0;	
+    buf[1]=170;
+    i2c.send(id, buf, 2);	
+
+    buf[0]=0;	
+    buf[1]=165;
+    i2c.send(id, buf, 2);	
+
+    buf[0]=0;	
+    buf[1]=(newID*2)+224;
+    i2c.send(id, buf, 2);	
+
+    id=112+newID;
+  }	//else error!
+}
+
+
+int Sonar::distance()
+{
+  uint8_t buf[10];
+
+  buf[0]=0;				// start measurement
+  buf[1]=81;		
+  i2c.send(id, buf, 2);	
+  msleep(70);			// wait for echo!!!  (about 65ms)
+
+  i2c.send(id, 3);		// select byte to be read
+
+  i2c.get(id, buf, 1);	// read first byte
+  uint8_t r1=buf[0];
+
+  i2c.send(id, 2);		// select byte to be read
+
+  i2c.get(id, buf, 1);	// read second byte
+  uint8_t r2=buf[0];
+	
+  return ((r1+(256*r2))*10);	// compute mm
+}
+
+
+#endif
+\ No newline at end of file