Diff: TLC5940Servo.cpp

Revision:

2:1d2251574d35

Parent:

0:9fc434ff7a03

Child:

5:56daa8c0697d

--- a/TLC5940Servo.cpp	Tue Oct 21 02:32:57 2014 +0000
+++ b/TLC5940Servo.cpp	Tue Oct 21 06:04:24 2014 +0000
@@ -1,71 +1,118 @@
 #include "TLC5940Servo.h"
 
-TLC5940Servo::TLC5940Servo(PinName MOSI, PinName SCLK, PinName XLAT, PinName BLANK, 
-        PinName GSCLK, const int number) : number(number), spi(MOSI, NC, SCLK), gsclk(GSCLK),
-                                           blank(BLANK), xlat(XLAT), newData(false), need_xlat(false)
-{   
-    // Configure SPI to 12 bits and SPI_SPEED
+TLC5940Servo::TLC5940Servo(PinName MOSI, PinName SCLK, PinName XLAT, PinName BLANK,
+                           PinName GSCLK, const int number) : number(number), spi(MOSI, NC, SCLK), gsclk(GSCLK),
+    blank(BLANK), xlat(XLAT), newData(false), need_xlat(false) {
     spi.format(12, 0);
     spi.frequency(SPI_SPEED);
-    
-    // Set output pin states
+
     xlat = 0;
     blank = 1;
-    
-    // Call the reset function every 4096 PWM outputs
+
     reset_ticker.attach_us(this, &TLC5940Servo::reset, (1000000.0/GSCLK_SPEED) * 4096.0);
-    
-    // Configure FastPWM output for GSCLK frequency at 50% duty cycle
+
     gsclk.period_us(1000000.0/GSCLK_SPEED);
     gsclk.write(.5);
 
-    // Create a data buffer to store the current Servo states
-    dataBuffer = new int[16 * number]();
+    servos = new Servo[16 * number];
 }
 
-TLC5940Servo::~TLC5940Servo()
-{
-    // Delete the buffer
-    delete[] dataBuffer;
+TLC5940Servo::~TLC5940Servo() {
+    delete[] servos;
+}
+
+void TLC5940Servo::calibrate(int index, float range, float degrees) {
+    servos[index].calibrate(range, degrees);
 }
 
-
-int& TLC5940Servo::operator[](int index)
-{
-    // Return the start of the correct data chunk
-    newData = true;
-    return dataBuffer[index];
+void TLC5940Servo::calibrate(float range, float degrees) {
+    for (int i = 0; i < 16 * number; i++)
+        servos[i].calibrate(range, degrees);
 }
 
-void TLC5940Servo::reset()
-{
+TLC5940Servo::Servo& TLC5940Servo::operator[](int index) {
+    newData = true;
+    return servos[index];
+}
+
+// most complicated method, heavily commented
+void TLC5940Servo::reset() {
     gsclk.write(0); // turn off gsclk
     blank = 1; // start reset
-    
+
     // latch data if new data was written
     if (need_xlat) {
         xlat = 1;
         xlat = 0;
-        
+
         need_xlat = false;
     }
-    
+
     blank = 0; // turn off reset
     gsclk.write(.5); // restart gsclk
 
     if (newData) {
-        
-        // Send GS data backwards - this makes the GS_buffer[0] index correspond to OUT0 
-        for (int i = (16 * number) - 1; i >= 0; i--)
-        {
+
+        // Send data backwards - this makes servos[0] index correspond to OUT0
+        for (int i = (16 * number) - 1; i >= 0; i--) {
             // Get the lower 12 bits of the buffer and send
-            spi.write(dataBuffer[i] & 0xFFF);
+            spi.write(servos[i].pulsewidth() & 0xFFF);
         }
-        
+
         // Latch after current GS data is done being displayed
         need_xlat = true;
-        
+
         // No new data to send (we just sent it!)
         newData = false;
     }
-}
\ No newline at end of file
+}
+
+/**
+ * TLC5940 Inner Servo class
+ * 
+ * Helps to abstract some of the details away
+ */
+static float clamp(float value, float min, float max) {
+    if(value < min) {
+        return min;
+    } else if(value > max) {
+        return max;
+    } else {
+        return value;
+    }
+}
+
+TLC5940Servo::Servo::Servo() {
+    calibrate();
+    write(0.5);
+}
+
+// used source code, but converted pwm pulse to 12 bit value
+// assuming 20ms period, divide to get percentage and multiply to get value
+void TLC5940Servo::Servo::write(float percent) {
+    float offset = _range * 2.0 * (percent - 0.5);
+    _pw = (int)(4095 - ((0.0015 + clamp(offset, -_range, _range))/0.02 * 4096.0));
+    _p = clamp(percent, 0.0, 1.0);
+}
+
+void TLC5940Servo::Servo::calibrate(float range, float degrees) {
+    _range = range;
+    _degrees = degrees;
+}
+
+float TLC5940Servo::Servo::read() {
+    return _p;
+}
+
+int TLC5940Servo::Servo::pulsewidth() {
+    return _pw;
+}
+
+TLC5940Servo::Servo& TLC5940Servo::Servo::operator= (float percent) { 
+    write(percent);
+    return *this;
+}
+
+TLC5940Servo::Servo::operator float() {
+    return read();
+}