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@3:1812150d7a4d, 2016-04-27 (annotated)
- Committer:
- Ingram
- Date:
- Wed Apr 27 17:11:38 2016 +0000
- Revision:
- 3:1812150d7a4d
- Parent:
- 2:e5bc45ecdaca
Fix last commit
Who changed what in which revision?
User | Revision | Line number | New 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 | 3:1812150d7a4d | 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 | 2:e5bc45ecdaca | 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 | 2:e5bc45ecdaca | 43 | writeRegister(2 * channel + 2, 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 | 2:e5bc45ecdaca | 59 | char tmp = reg; |
Ingram | 2:e5bc45ecdaca | 60 | i2c.write(i2c_address, &tmp, 1); |
Ingram | 2:e5bc45ecdaca | 61 | i2c.read(i2c_address, &tmp, 1); |
Ingram | 2:e5bc45ecdaca | 62 | return tmp; |
Ingram | 0:bf06c3ca958b | 63 | } |