Library for using the TLC5940 as a servo controller.
TLC5940Servo.cpp@6:b001282e967b, 2014-10-21 (annotated)
- Committer:
- dudanian
- Date:
- Tue Oct 21 15:05:16 2014 +0000
- Revision:
- 6:b001282e967b
- Parent:
- 5:56daa8c0697d
Removed indexed calibrate to make usage more like an array of mbed official Servos
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dudanian | 0:9fc434ff7a03 | 1 | #include "TLC5940Servo.h" |
dudanian | 0:9fc434ff7a03 | 2 | |
dudanian | 2:1d2251574d35 | 3 | TLC5940Servo::TLC5940Servo(PinName MOSI, PinName SCLK, PinName XLAT, PinName BLANK, |
dudanian | 2:1d2251574d35 | 4 | PinName GSCLK, const int number) : number(number), spi(MOSI, NC, SCLK), gsclk(GSCLK), |
dudanian | 2:1d2251574d35 | 5 | blank(BLANK), xlat(XLAT), newData(false), need_xlat(false) { |
dudanian | 0:9fc434ff7a03 | 6 | spi.format(12, 0); |
dudanian | 0:9fc434ff7a03 | 7 | spi.frequency(SPI_SPEED); |
dudanian | 2:1d2251574d35 | 8 | |
dudanian | 0:9fc434ff7a03 | 9 | xlat = 0; |
dudanian | 0:9fc434ff7a03 | 10 | blank = 1; |
dudanian | 2:1d2251574d35 | 11 | |
dudanian | 0:9fc434ff7a03 | 12 | reset_ticker.attach_us(this, &TLC5940Servo::reset, (1000000.0/GSCLK_SPEED) * 4096.0); |
dudanian | 2:1d2251574d35 | 13 | |
dudanian | 0:9fc434ff7a03 | 14 | gsclk.period_us(1000000.0/GSCLK_SPEED); |
dudanian | 0:9fc434ff7a03 | 15 | gsclk.write(.5); |
dudanian | 0:9fc434ff7a03 | 16 | |
dudanian | 2:1d2251574d35 | 17 | servos = new Servo[16 * number]; |
dudanian | 0:9fc434ff7a03 | 18 | } |
dudanian | 0:9fc434ff7a03 | 19 | |
dudanian | 2:1d2251574d35 | 20 | TLC5940Servo::~TLC5940Servo() { |
dudanian | 2:1d2251574d35 | 21 | delete[] servos; |
dudanian | 2:1d2251574d35 | 22 | } |
dudanian | 2:1d2251574d35 | 23 | |
dudanian | 2:1d2251574d35 | 24 | void TLC5940Servo::calibrate(float range, float degrees) { |
dudanian | 2:1d2251574d35 | 25 | for (int i = 0; i < 16 * number; i++) |
dudanian | 2:1d2251574d35 | 26 | servos[i].calibrate(range, degrees); |
dudanian | 0:9fc434ff7a03 | 27 | } |
dudanian | 0:9fc434ff7a03 | 28 | |
dudanian | 2:1d2251574d35 | 29 | TLC5940Servo::Servo& TLC5940Servo::operator[](int index) { |
dudanian | 2:1d2251574d35 | 30 | newData = true; |
dudanian | 2:1d2251574d35 | 31 | return servos[index]; |
dudanian | 2:1d2251574d35 | 32 | } |
dudanian | 2:1d2251574d35 | 33 | |
dudanian | 2:1d2251574d35 | 34 | // most complicated method, heavily commented |
dudanian | 2:1d2251574d35 | 35 | void TLC5940Servo::reset() { |
dudanian | 0:9fc434ff7a03 | 36 | gsclk.write(0); // turn off gsclk |
dudanian | 0:9fc434ff7a03 | 37 | blank = 1; // start reset |
dudanian | 2:1d2251574d35 | 38 | |
dudanian | 0:9fc434ff7a03 | 39 | // latch data if new data was written |
dudanian | 0:9fc434ff7a03 | 40 | if (need_xlat) { |
dudanian | 0:9fc434ff7a03 | 41 | xlat = 1; |
dudanian | 0:9fc434ff7a03 | 42 | xlat = 0; |
dudanian | 2:1d2251574d35 | 43 | |
dudanian | 0:9fc434ff7a03 | 44 | need_xlat = false; |
dudanian | 0:9fc434ff7a03 | 45 | } |
dudanian | 2:1d2251574d35 | 46 | |
dudanian | 0:9fc434ff7a03 | 47 | blank = 0; // turn off reset |
dudanian | 0:9fc434ff7a03 | 48 | gsclk.write(.5); // restart gsclk |
dudanian | 0:9fc434ff7a03 | 49 | |
dudanian | 0:9fc434ff7a03 | 50 | if (newData) { |
dudanian | 2:1d2251574d35 | 51 | |
dudanian | 2:1d2251574d35 | 52 | // Send data backwards - this makes servos[0] index correspond to OUT0 |
dudanian | 2:1d2251574d35 | 53 | for (int i = (16 * number) - 1; i >= 0; i--) { |
dudanian | 0:9fc434ff7a03 | 54 | // Get the lower 12 bits of the buffer and send |
dudanian | 2:1d2251574d35 | 55 | spi.write(servos[i].pulsewidth() & 0xFFF); |
dudanian | 0:9fc434ff7a03 | 56 | } |
dudanian | 2:1d2251574d35 | 57 | |
dudanian | 0:9fc434ff7a03 | 58 | // Latch after current GS data is done being displayed |
dudanian | 0:9fc434ff7a03 | 59 | need_xlat = true; |
dudanian | 2:1d2251574d35 | 60 | |
dudanian | 0:9fc434ff7a03 | 61 | // No new data to send (we just sent it!) |
dudanian | 0:9fc434ff7a03 | 62 | newData = false; |
dudanian | 0:9fc434ff7a03 | 63 | } |
dudanian | 2:1d2251574d35 | 64 | } |
dudanian | 2:1d2251574d35 | 65 | |
dudanian | 2:1d2251574d35 | 66 | /** |
dudanian | 2:1d2251574d35 | 67 | * TLC5940 Inner Servo class |
dudanian | 2:1d2251574d35 | 68 | * |
dudanian | 2:1d2251574d35 | 69 | * Helps to abstract some of the details away |
dudanian | 2:1d2251574d35 | 70 | */ |
dudanian | 2:1d2251574d35 | 71 | static float clamp(float value, float min, float max) { |
dudanian | 2:1d2251574d35 | 72 | if(value < min) { |
dudanian | 2:1d2251574d35 | 73 | return min; |
dudanian | 2:1d2251574d35 | 74 | } else if(value > max) { |
dudanian | 2:1d2251574d35 | 75 | return max; |
dudanian | 2:1d2251574d35 | 76 | } else { |
dudanian | 2:1d2251574d35 | 77 | return value; |
dudanian | 2:1d2251574d35 | 78 | } |
dudanian | 2:1d2251574d35 | 79 | } |
dudanian | 2:1d2251574d35 | 80 | |
dudanian | 2:1d2251574d35 | 81 | TLC5940Servo::Servo::Servo() { |
dudanian | 2:1d2251574d35 | 82 | calibrate(); |
dudanian | 2:1d2251574d35 | 83 | write(0.5); |
dudanian | 2:1d2251574d35 | 84 | } |
dudanian | 2:1d2251574d35 | 85 | |
dudanian | 2:1d2251574d35 | 86 | void TLC5940Servo::Servo::write(float percent) { |
dudanian | 2:1d2251574d35 | 87 | float offset = _range * 2.0 * (percent - 0.5); |
dudanian | 2:1d2251574d35 | 88 | _pw = (int)(4095 - ((0.0015 + clamp(offset, -_range, _range))/0.02 * 4096.0)); |
dudanian | 2:1d2251574d35 | 89 | _p = clamp(percent, 0.0, 1.0); |
dudanian | 2:1d2251574d35 | 90 | } |
dudanian | 2:1d2251574d35 | 91 | |
dudanian | 5:56daa8c0697d | 92 | void TLC5940Servo::Servo::position(float degrees) { |
dudanian | 5:56daa8c0697d | 93 | float offset = _range * (degrees / _degrees); |
dudanian | 5:56daa8c0697d | 94 | _pw = (int)(4095 - ((0.0015 + clamp(offset, -_range, _range))/0.02 * 4096.0)); |
dudanian | 5:56daa8c0697d | 95 | } |
dudanian | 5:56daa8c0697d | 96 | |
dudanian | 2:1d2251574d35 | 97 | void TLC5940Servo::Servo::calibrate(float range, float degrees) { |
dudanian | 2:1d2251574d35 | 98 | _range = range; |
dudanian | 2:1d2251574d35 | 99 | _degrees = degrees; |
dudanian | 2:1d2251574d35 | 100 | } |
dudanian | 2:1d2251574d35 | 101 | |
dudanian | 2:1d2251574d35 | 102 | float TLC5940Servo::Servo::read() { |
dudanian | 2:1d2251574d35 | 103 | return _p; |
dudanian | 2:1d2251574d35 | 104 | } |
dudanian | 2:1d2251574d35 | 105 | |
dudanian | 2:1d2251574d35 | 106 | int TLC5940Servo::Servo::pulsewidth() { |
dudanian | 2:1d2251574d35 | 107 | return _pw; |
dudanian | 2:1d2251574d35 | 108 | } |
dudanian | 2:1d2251574d35 | 109 | |
dudanian | 2:1d2251574d35 | 110 | TLC5940Servo::Servo& TLC5940Servo::Servo::operator= (float percent) { |
dudanian | 2:1d2251574d35 | 111 | write(percent); |
dudanian | 2:1d2251574d35 | 112 | return *this; |
dudanian | 2:1d2251574d35 | 113 | } |
dudanian | 2:1d2251574d35 | 114 | |
dudanian | 5:56daa8c0697d | 115 | TLC5940Servo::Servo& TLC5940Servo::Servo::operator= (TLC5940Servo::Servo& rhs) { |
dudanian | 5:56daa8c0697d | 116 | write(rhs.read()); |
dudanian | 5:56daa8c0697d | 117 | return *this; |
dudanian | 5:56daa8c0697d | 118 | } |
dudanian | 5:56daa8c0697d | 119 | |
dudanian | 2:1d2251574d35 | 120 | TLC5940Servo::Servo::operator float() { |
dudanian | 2:1d2251574d35 | 121 | return read(); |
dudanian | 2:1d2251574d35 | 122 | } |