File content as of revision 3:1812150d7a4d:

#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 + 2, 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;
}