SCX18S.cpp

00001 #include "mbed.h"
00002 #include "SCX18S.h"
00003 
00004 SCX18S::SCX18S(PinName p_sda, PinName p_scl, bool j_a0, bool j_a1) : i2c(p_sda, p_scl)
00005 {
00006     i2c_address = (SCX18_I2C_BASE_ADDR | ((!j_a0) << 1) | (!j_a1)) << 1;
00007 }
00008 
00009 SCX18SServoStatusRegister SCX18S::readStatus(uint8_t channel)
00010 {
00011     SCX18SServoStatusRegister ssr;
00012     ssr.value = readRegister(channel);
00013     return ssr;
00014 }
00015 
00016 void SCX18S::writePosition(uint8_t channel, uint8_t position)
00017 {
00018     writeRegister(2 * channel + 1, position);
00019 }
00020 
00021 void SCX18S::writeControl(uint8_t channel, bool enabled, bool reversed, bool softstart, bool speedcontrol, uint8_t speed)
00022 {
00023     if (speed > 15) {
00024         error("%s speed specified greater than allowed (15)", __func__);
00025     }
00026 
00027     if (enabled) {
00028         speed |= SCX18_SERVO_ENABLED;
00029     }
00030 
00031     if (reversed) {
00032         speed |= SCX18_SERVO_REVERSED;
00033     }
00034 
00035     if (softstart) {
00036         speed |= SCX18_SERVO_SOFT_START;
00037     }
00038 
00039     if (speedcontrol) {
00040         speed |= SCX18_SERVO_SPEED_CONTROL;
00041     }
00042 
00043     writeRegister(2 * channel + 2, speed);
00044 }
00045 
00046 void SCX18S::writeEnable()
00047 {
00048     writeRegister(37, 0);
00049 }
00050 
00051 void SCX18S::writeRegister(uint8_t reg, uint8_t value)
00052 {
00053     char i2c_data[] = {reg, value};
00054     i2c.write(i2c_address, i2c_data, 2);
00055 }
00056 
00057 uint8_t SCX18S::readRegister(uint8_t reg)
00058 {
00059     char tmp = reg;
00060     i2c.write(i2c_address, &tmp, 1);
00061     i2c.read(i2c_address, &tmp, 1);
00062     return tmp;
00063 }