1 files changed, 89 insertions, 0 deletions
diff --git a/source/qFix/qfixServoBoard4.h b/source/qFix/qfixServoBoard4.h
new file mode 100644
index 0000000..baaa2f9
--- /dev/null
+++ b/source/qFix/qfixServoBoard4.h
@@ -0,0 +1,89 @@
+//------------------------------------------------------------------
+// qfixServoBoard4.h
+//
+// This file contains the class ServoBoard4 which represents
+// four servos attached to the ServoBoard4.
+//
+// Copyright 2006 by KTB mechatronics GmbH
+// Author: Stefan Enderle
+//------------------------------------------------------------------
+
+#ifndef qfixServoBoard4_h
+#define qfixServoBoard4_h
+
+
+#include "qfixI2C.h"
+#include "qfixI2CDefs.h"
+#include "string.h"
+
+static const uint8_t I2C_ADDR_SERVO = 3;
+
+static const uint8_t SERVO_CMD_CHANGEID = 0;
+static const uint8_t SERVO_CMD_SET      = 1;
+
+
+class ServoBoard4
+{
+private:
+  I2C i2c;
+  uint8_t id;
+
+public:
+  /** Constructor with default ID 0
+  */
+  ServoBoard4();
+
+  /** Constructor with given ID
+  */
+  ServoBoard4(uint8_t ID);
+
+  /** Change logical device ID
+  */
+  void changeID(uint8_t newID);
+
+  /** 
+  */
+  void set(uint8_t index, uint8_t pwm);
+};
+
+
+
+ServoBoard4::ServoBoard4() : i2c(), id(0)
+{
+}
+
+
+ServoBoard4::ServoBoard4(uint8_t ID) : i2c(), id(ID)
+{
+}
+
+
+void ServoBoard4::set(uint8_t index, uint8_t pwm)
+{
+  uint8_t buf[5];
+
+  buf[0] = id;
+  buf[1] = SERVO_CMD_SET;
+  buf[2] = index;
+  buf[3] = pwm;
+  i2c.send(I2C_ADDR_SERVO, buf, 4);
+}
+
+
+void ServoBoard4::changeID(uint8_t newID)
+{
+  uint8_t buf[3];
+
+  buf[0] = id;
+  buf[1] = SERVO_CMD_CHANGEID;
+  buf[2] = newID;
+  i2c.send(I2C_ADDR_SERVO, buf, 3);
+
+  id = newID;
+}
+
+
+#endif
+
+
+