Diff: SCX18S.cpp

Revision:

0:bf06c3ca958b

Child:

1:3e636ff825f2

diff -r 000000000000 -r bf06c3ca958b SCX18S.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SCX18S.cpp	Wed Apr 27 14:16:45 2016 +0000
@@ -0,0 +1,63 @@
+#include "mbed.h"
+#include "SCX18S.h"
+
+SCX18S::SCX18S(PinName p_sda, PinName p_scl, bool j_a0, bool j_a1) : i2c(p_sda, p_scl)
+{
+    i2c_address = (SCX18_I2C_BASE_ADDR | (j_a0 << 1) | j_a1) << 1;
+}
+
+SCX18SServoStatusRegister SCX18S::readStatus(uint8_t channel)
+{
+    SCX18SServoStatusRegister ssr;
+    ssr.value = readRegister(channel);
+    return ssr;
+}
+
+void SCX18S::writePosition(uint8_t channel, uint8_t position)
+{
+    writeRegister(2 * channel - 1, position);
+}
+
+void SCX18S::writeControl(uint8_t channel, bool enabled, bool reversed, bool softstart, bool speedcontrol, uint8_t speed)
+{
+    if (speed > 15) {
+        error("%s speed specified greater than allowed (15)", __func__);
+    }
+
+    if (enabled) {
+        speed |= SCX18_SERVO_ENABLED;
+    }
+
+    if (reversed) {
+        speed |= SCX18_SERVO_REVERSED;
+    }
+
+    if (softstart) {
+        speed |= SCX18_SERVO_SOFT_START;
+    }
+
+    if (speedcontrol) {
+        speed |= SCX18_SERVO_SPEED_CONTROL;
+    }
+
+    writeRegister(2 * channel, speed);
+}
+
+void SCX18S::writeEnable()
+{
+    writeRegister(37, 0);
+}
+
+void SCX18S::writeRegister(uint8_t reg, uint8_t value)
+{
+    char i2c_data[] = {reg, value};
+    i2c.write(i2c_address, i2c_data, 2);
+}
+
+uint8_t SCX18S::readRegister(uint8_t reg)
+{
+    char tmp[] = {reg};
+    i2c.write(i2c_address, tmp, 1);
+    i2c.read(i2c_address, tmp, 1);
+    return tmp[0];
+}
\ No newline at end of file