Wrapper library for controlling servos using Designer Systems DS-SCX18.Shield servo driver. See http://www.designersystems.co.uk/SCX18.S_info.htm
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