Andrew Duda / TLC5940Servo

Library for using the TLC5940 as a servo controller.

Dependencies:   FastPWM

Dependents:   TLC5940ServoTest

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?

UserRevisionLine numberNew 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 }