Indrek Ardel / DS-SCX18S

Wrapper library for controlling servos using Designer Systems DS-SCX18.Shield servo driver. See http://www.designersystems.co.uk/SCX18.S_info.htm

SCX18S.cpp@0:bf06c3ca958b, 2016-04-27 (annotated)

Committer:
Ingram
Date:
Wed Apr 27 14:16:45 2016 +0000
Revision:
0:bf06c3ca958b
Child:
1:3e636ff825f2
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ingram 0:bf06c3ca958b 1 #include "mbed.h"
Ingram 0:bf06c3ca958b 2 #include "SCX18S.h"
Ingram 0:bf06c3ca958b 3
Ingram 0:bf06c3ca958b 4 SCX18S::SCX18S(PinName p_sda, PinName p_scl, bool j_a0, bool j_a1) : i2c(p_sda, p_scl)
Ingram 0:bf06c3ca958b 5 {
Ingram 0:bf06c3ca958b 6 i2c_address = (SCX18_I2C_BASE_ADDR | (j_a0 << 1) | j_a1) << 1;
Ingram 0:bf06c3ca958b 7 }
Ingram 0:bf06c3ca958b 8
Ingram 0:bf06c3ca958b 9 SCX18SServoStatusRegister SCX18S::readStatus(uint8_t channel)
Ingram 0:bf06c3ca958b 10 {
Ingram 0:bf06c3ca958b 11 SCX18SServoStatusRegister ssr;
Ingram 0:bf06c3ca958b 12 ssr.value = readRegister(channel);
Ingram 0:bf06c3ca958b 13 return ssr;
Ingram 0:bf06c3ca958b 14 }
Ingram 0:bf06c3ca958b 15
Ingram 0:bf06c3ca958b 16 void SCX18S::writePosition(uint8_t channel, uint8_t position)
Ingram 0:bf06c3ca958b 17 {
Ingram 0:bf06c3ca958b 18 writeRegister(2 * channel - 1, position);
Ingram 0:bf06c3ca958b 19 }
Ingram 0:bf06c3ca958b 20
Ingram 0:bf06c3ca958b 21 void SCX18S::writeControl(uint8_t channel, bool enabled, bool reversed, bool softstart, bool speedcontrol, uint8_t speed)
Ingram 0:bf06c3ca958b 22 {
Ingram 0:bf06c3ca958b 23 if (speed > 15) {
Ingram 0:bf06c3ca958b 24 error("%s speed specified greater than allowed (15)", __func__);
Ingram 0:bf06c3ca958b 25 }
Ingram 0:bf06c3ca958b 26
Ingram 0:bf06c3ca958b 27 if (enabled) {
Ingram 0:bf06c3ca958b 28 speed |= SCX18_SERVO_ENABLED;
Ingram 0:bf06c3ca958b 29 }
Ingram 0:bf06c3ca958b 30
Ingram 0:bf06c3ca958b 31 if (reversed) {
Ingram 0:bf06c3ca958b 32 speed |= SCX18_SERVO_REVERSED;
Ingram 0:bf06c3ca958b 33 }
Ingram 0:bf06c3ca958b 34
Ingram 0:bf06c3ca958b 35 if (softstart) {
Ingram 0:bf06c3ca958b 36 speed |= SCX18_SERVO_SOFT_START;
Ingram 0:bf06c3ca958b 37 }
Ingram 0:bf06c3ca958b 38
Ingram 0:bf06c3ca958b 39 if (speedcontrol) {
Ingram 0:bf06c3ca958b 40 speed |= SCX18_SERVO_SPEED_CONTROL;
Ingram 0:bf06c3ca958b 41 }
Ingram 0:bf06c3ca958b 42
Ingram 0:bf06c3ca958b 43 writeRegister(2 * channel, speed);
Ingram 0:bf06c3ca958b 44 }
Ingram 0:bf06c3ca958b 45
Ingram 0:bf06c3ca958b 46 void SCX18S::writeEnable()
Ingram 0:bf06c3ca958b 47 {
Ingram 0:bf06c3ca958b 48 writeRegister(37, 0);
Ingram 0:bf06c3ca958b 49 }
Ingram 0:bf06c3ca958b 50
Ingram 0:bf06c3ca958b 51 void SCX18S::writeRegister(uint8_t reg, uint8_t value)
Ingram 0:bf06c3ca958b 52 {
Ingram 0:bf06c3ca958b 53 char i2c_data[] = {reg, value};
Ingram 0:bf06c3ca958b 54 i2c.write(i2c_address, i2c_data, 2);
Ingram 0:bf06c3ca958b 55 }
Ingram 0:bf06c3ca958b 56
Ingram 0:bf06c3ca958b 57 uint8_t SCX18S::readRegister(uint8_t reg)
Ingram 0:bf06c3ca958b 58 {
Ingram 0:bf06c3ca958b 59 char tmp[] = {reg};
Ingram 0:bf06c3ca958b 60 i2c.write(i2c_address, tmp, 1);
Ingram 0:bf06c3ca958b 61 i2c.read(i2c_address, tmp, 1);
Ingram 0:bf06c3ca958b 62 return tmp[0];
Ingram 0:bf06c3ca958b 63 }