Diff: main.cpp

Revision:

1:d913e0afb2ac

Parent:

0:5acbbe3f4cf4

Child:

2:c174f9ee414a

--- a/main.cpp	Fri Jul 11 03:26:11 2014 +0000
+++ b/main.cpp	Wed Jul 16 23:33:12 2014 +0000
@@ -1,31 +1,34 @@
 #include "mbed.h"
 #include "USBJoystick.h"
 #include "MMA8451Q.h"
-#include "tls1410r.h"
+#include "tsl1410r.h"
+#include "FreescaleIAP.h"
 
+// on-board RGB LED elements - we use these for diagnostics
 PwmOut led1(LED1), led2(LED2), led3(LED3);
-DigitalOut out1(PTE29);
 
-
+// calibration button - switch input and LED output
+DigitalIn calBtn(PTE29);
+DigitalOut calBtnLed(PTE23);
 
 static int pbaIdx = 0;
 
 // on/off state for each LedWiz output
-static uint8_t ledOn[32];
+static uint8_t wizOn[32];
 
 // profile (brightness/blink) state for each LedWiz output
-static uint8_t ledVal[32] = {
+static uint8_t wizVal[32] = {
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0
 };
 
-static double ledState(int idx)
+static float wizState(int idx)
 {
-    if (ledOn[idx]) {
+    if (wizOn[idx]) {
         // on - map profile brightness state to PWM level
-        uint8_t val = ledVal[idx];
+        uint8_t val = wizVal[idx];
         if (val >= 1 && val <= 48)
             return 1.0 - val/48.0;
         else if (val >= 129 && val <= 132)
@@ -39,27 +42,75 @@
     }
 }
 
-static void updateLeds()
+static void updateWizOuts()
+{
+    led1 = wizState(0);
+    led2 = wizState(1);
+    led3 = wizState(2);
+}
+
+struct AccPrv
 {
-    led1 = ledState(0);
-    led2 = ledState(1);
-    led3 = ledState(2);
-}
+    AccPrv() : x(0), y(0) { }
+    float x;
+    float y;
+    
+    double dist(AccPrv &b)
+    {
+        float dx = x - b.x, dy = y - b.y;
+        return sqrt(dx*dx + dy*dy);
+    }
+};
 
 int main(void)
 {
+    // turn off our on-board indicator LED
     led1 = 1;
     led2 = 1;
     led3 = 1;
-    Timer timer;
+    
+    // plunger calibration data
+    const int npix = 320;
+    int plungerMin = 0, plungerMax = npix;
+    
+    // plunger calibration button debounce timer
+    Timer calBtnTimer;
+    calBtnTimer.start();
+    int calBtnDownTime = 0;
+    int calBtnLit = false;
+    
+    // Calibration button state:
+    //  0 = not pushed
+    //  1 = pushed, not yet debounced
+    //  2 = pushed, debounced, waiting for hold time
+    //  3 = pushed, hold time completed - in calibration mode
+    int calBtnState = 0;
+    
+    // set up a timer for our heartbeat indicator
+    Timer hbTimer;
+    hbTimer.start();
+    int t0Hb = hbTimer.read_ms();
+    int hb = 0;
+    
+    // set a timer for accelerometer auto-centering
+    Timer acTimer;
+    acTimer.start();
+    int t0ac = acTimer.read_ms();
+    
+    // set up a timer for reading the plunger sensor
+    Timer ccdTimer;
+    ccdTimer.start();
+    int t0ccd = ccdTimer.read_ms();
+    
+#if 0
+    // DEBUG
+    Timer ccdDbgTimer;
+    ccdDbgTimer.start();
+    int t0ccdDbg = ccdDbgTimer.read_ms();
+#endif
 
-    // set up a timer for spacing USB reports   
-    timer.start();
-    float t0 = timer.read_ms();    
-    float tout1 = timer.read_ms();
-
-    // Create the joystick USB client.  Show a read LED while connecting, and
-    // change to green when connected.
+    // Create the joystick USB client.  Light the on-board indicator LED
+    // red while connecting, and change to green after we connect.
     led1 = 0.75;
     USBJoystick js(0xFAFA, 0x00F7, 0x0001);
     led1 = 1;
@@ -68,13 +119,23 @@
     // create the accelerometer object
     const int MMA8451_I2C_ADDRESS = (0x1d<<1);
     MMA8451Q accel(PTE25, PTE24, MMA8451_I2C_ADDRESS);
-    printf("MMA8451 ID: %d\r\n", accel.getWhoAmI());
     
     // create the CCD array object
-    TLS1410R ccd(PTE20, PTE21, PTB0);
+    TSL1410R ccd(PTE20, PTE21, PTB0);
+
+    // recent accelerometer readings, for auto centering
+    int iAccPrv = 0, nAccPrv = 0;
+    const int maxAccPrv = 5;
+    AccPrv accPrv[maxAccPrv];
 
-    // process sensor reports and LedWiz requests forever
-    int x = 0, y = 127, z = 0;
+    // last accelerometer report, in mouse coordinates
+    int x = 127, y = 127, z = 0;
+    
+    // raw accelerator centerpoint, on the unit interval (-1.0 .. +1.0)
+    float xCenter = 0.0, yCenter = 0.0;    
+
+    // we're all set up - now just loop, processing sensor reports and 
+    // host requests
     for (;;)
     {
         // Look for an incoming report.  Continue processing input as
@@ -85,7 +146,8 @@
         while (js.readNB(&report) && report.length == 8)
         {
             uint8_t *data = report.data;
-            if (data[0] == 64) {
+            if (data[0] == 64) 
+            {
                 // LWZ-SBA - first four bytes are bit-packed on/off flags
                 // for the outputs; 5th byte is the pulse speed (0-7)
                 //printf("LWZ-SBA %02x %02x %02x %02x ; %02x\r\n",
@@ -98,16 +160,17 @@
                         bit = 1;
                         ++ri;
                     }
-                    ledOn[i] = ((data[ri] & bit) != 0);
+                    wizOn[i] = ((data[ri] & bit) != 0);
                 }
     
-                // update the physical LED state
-                updateLeds();
+                // update the physical outputs
+                updateWizOuts();
                 
                 // reset the PBA counter
                 pbaIdx = 0;
             }
-            else {
+            else 
+            {
                 // LWZ-PBA - full state dump; each byte is one output
                 // in the current bank.  pbaIdx keeps track of the bank;
                 // this is incremented implicitly by each PBA message.
@@ -116,73 +179,242 @@
 
                 // update all output profile settings
                 for (int i = 0 ; i < 8 ; ++i)
-                    ledVal[pbaIdx + i] = data[i];
+                    wizVal[pbaIdx + i] = data[i];
 
                 // update the physical LED state if this is the last bank                    
                 if (pbaIdx == 24)
-                    updateLeds();
+                    updateWizOuts();
 
                 // advance to the next bank
                 pbaIdx = (pbaIdx + 8) & 31;
             }
         }
-        
-#if 1
-        // check the accelerometer
+       
+        // check for plunger calibration
+        if (!calBtn)
         {
-            // read the accelerometer
-            float xa = accel.getAccX();
-            float ya = accel.getAccY();
-            
-            // figure the new joystick position
-            int xnew = (int)(127 * xa);
-            int ynew = (int)(127 * ya);
-            
-            // send an update if the position has changed
-            if (xnew != x || ynew != y)
+            // check the state
+            switch (calBtnState)
             {
-                x = xnew;
-                y = ynew;
-
-                // send the status report
-                js.update(x, y, z, 0);
+            case 0: 
+                // button not yet pushed - start debouncing
+                calBtnTimer.reset();
+                calBtnDownTime = calBtnTimer.read_ms();
+                calBtnState = 1;
+                break;
+                
+            case 1:
+                // pushed, not yet debounced - if the debounce time has
+                // passed, start the hold period
+                if (calBtnTimer.read_ms() - calBtnDownTime > 50)
+                    calBtnState = 2;
+                break;
+                
+            case 2:
+                // in the hold period - if the button has been held down
+                // for the entire hold period, move to calibration mode
+                if (calBtnTimer.read_ms() - calBtnDownTime > 2050)
+                {
+                    // enter calibration mode
+                    calBtnState = 3;
+                    
+                    // reset the calibration limits
+                    plungerMax = 0;
+                    plungerMin = npix;
+                }
+                break;
             }
         }
-#else
-        // Send a joystick report if it's been long enough since the
-        // last report        
-        if (timer.read_ms() - t0 > 250)
+        else
+        {
+            // Button released.  If we're not already in calibration mode,
+            // reset the button state.  Once calibration mode starts, it sticks
+            // until the calibration time elapses.
+            if (calBtnState != 3)
+                calBtnState = 0;
+            else if (calBtnTimer.read_ms() - calBtnDownTime > 32500)
+                calBtnState = 0;
+        }       
+        
+        // light/flash the calibration button light, if applicable
+        int newCalBtnLit = calBtnLit;
+        switch (calBtnState)
         {
-            // send the current joystick status report
-            js.update(x, y, z, 0);
-
-            // update our internal joystick position record
-            x += dx;
-            y += dy;
-            z += dz;
-            if (x > xmax || x < xmin) {
-                dx = -dx;
-                x += 2*dx;
+        case 2:
+            // in the hold period - flash the light
+            newCalBtnLit = (((calBtnTimer.read_ms() - calBtnDownTime)/250) & 1);
+            break;
+            
+        case 3:
+            // calibration mode - show steady on
+            newCalBtnLit = true;
+            break;
+            
+        default:
+            // not calibrating/holding - show steady off
+            newCalBtnLit = false;
+            break;
+        }
+        if (calBtnLit != newCalBtnLit)
+        {
+            calBtnLit = newCalBtnLit;
+            calBtnLed = (calBtnLit ? 1 : 0);
+        }
+        
+        // read the plunger sensor
+        int znew = z;
+        /* if (ccdTimer.read_ms() - t0ccd > 33) */
+        {
+            // read the sensor at reduced resolution
+            uint16_t pix[npix];
+            ccd.read(pix, npix, 0);
+            
+#if 0
+            // debug - send samples every 5 seconds
+            if (ccdDbgTimer.read_ms() - t0ccdDbg > 5000)
+            {
+                for (int i = 0 ; i < npix ; ++i)
+                    printf("%x ", pix[i]);
+                printf("\r\n\r\n");
+            
+                ccdDbgTimer.reset();
+                t0ccdDbg = ccdDbgTimer.read_ms();
             }
-            if (y > ymax || y < ymin) {
-                dy = -dy;
-                y += 2*dy;
-            }
-            if (z > zmax || z < zmin) {
-                dz = -dz;
-                z += 2*dz;
-            }
+#endif
+    
+            // check which end is the brighter - this is the "tip" end
+            // of the plunger
+            long avg1 = (long(pix[0]) + long(pix[1]) + long(pix[2]) + long(pix[3]) + long(pix[4]))/5;
+            long avg2 = (long(pix[npix-1]) + long(pix[npix-2]) + long(pix[npix-3]) + long(pix[npix-4]) + long(pix[npix-5]))/5;
+            
+            // figure the midpoint in the brightness
+            long midpt = (avg1 + avg2)/2 * 3;
+            
+            // Work from the bright end to the dark end.  VP interprets the
+            // Z axis value as the amount the plunger is pulled: the minimum
+            // is the rest position, the maximum is fully pulled.  So we 
+            // essentially want to report how much of the sensor is lit,
+            // since this increases as the plunger is pulled back.
+            int si = 1, di = 1;
+            if (avg1 < avg2)
+                si = npix - 1, di = -1;
 
-            // note the time of the last report
-            t0 = timer.read_ms();
-        }            
-#endif
+            // scan for the midpoint                
+            for (int n = 1, i = si ; n < npix - 1 ; ++n, i += di)
+            {
+                // if we've crossed the midpoint, report this position
+                if (long(pix[i-1]) + long(pix[i]) + long(pix[i+1]) < midpt)
+                {
+                    // note the new position
+                    int pos = abs(i - si);
+                    
+                    // Calibrate, or apply calibration, depending on the mode.
+                    // In either case, normalize to a 0-127 range.  VP appears to
+                    // ignore negative Z axis values.
+                    if (calBtnState == 3)
+                    {
+                        // calibrating - note if we're expanding the calibration envelope
+                        if (pos < plungerMin)
+                            plungerMin = pos;   
+                        if (pos > plungerMax)
+                            plungerMax = pos;
+                            
+                        // normalize to the full physical range while calibrating
+                        znew = int(float(pos)/npix * 127);
+                    }
+                    else
+                    {
+                        // running normally - normalize to the calibration range
+                        if (pos < plungerMin)
+                            pos = plungerMin;
+                        if (pos > plungerMax)
+                            pos = plungerMax;
+                        znew = int(float(pos - plungerMin)/(plungerMax - plungerMin + 1) * 127);
+                    }
+                    
+                    // done
+                    break;
+                }
+            }
+            
+            // reset the timer
+            ccdTimer.reset();
+            t0ccd = ccdTimer.read_ms();
+        } 
+        
+        // read the accelerometer
+        float xa, ya;
+        accel.getAccXY(xa, ya);
+        
+        // check for auto-centering every so often
+        if (acTimer.read_ms() - t0ac > 1000) 
+        {
+            // add the sample to the history list
+            accPrv[iAccPrv].x = xa;
+            accPrv[iAccPrv].y = ya;
+            
+            // store the slot
+            iAccPrv += 1;
+            iAccPrv %= maxAccPrv;
+            nAccPrv += 1;
+            
+            // If we have a full complement, check for stability.  The
+            // raw accelerometer input is in the rnage -4096 to 4096, but
+            // the class cover normalizes to a unit interval (-1.0 .. +1.0).
+            const float accTol = .005;
+            if (nAccPrv >= maxAccPrv
+                && accPrv[0].dist(accPrv[1]) < accTol
+                && accPrv[0].dist(accPrv[2]) < accTol
+                && accPrv[0].dist(accPrv[3]) < accTol
+                && accPrv[0].dist(accPrv[4]) < accTol)
+            {
+                // figure the new center
+                xCenter = (accPrv[0].x + accPrv[1].x + accPrv[2].x + accPrv[3].x + accPrv[4].x)/5.0;
+                yCenter = (accPrv[0].y + accPrv[1].y + accPrv[2].y + accPrv[3].y + accPrv[4].y)/5.0;
+            }
+            
+            // reset the auto-center timer
+            acTimer.reset();
+            t0ac = acTimer.read_ms();
+        }
+        
+        // adjust for our auto centering
+        xa -= xCenter;
+        ya -= yCenter;
+        
+        // confine to the unit interval
+        if (xa < -1.0) xa = -1.0;
+        if (xa > 1.0) xa = 1.0;
+        if (ya < -1.0) ya = -1.0;
+        if (ya > 1.0) ya = 1.0;
 
-        // pulse E29
-        if (timer.read_ms() - tout1 > 2000)
+        // figure the new mouse report data
+        int xnew = (int)(127 * xa);
+        int ynew = (int)(127 * ya);
+        
+        // send an update if the position has changed
+        // if (xnew != x || ynew != y || znew != z)
         {
-            out1 = !out1;
-            tout1 = timer.read_ms();
+            x = xnew;
+            y = ynew;
+            z = znew;
+
+            // Send the status report.  Note that the X axis needs to be
+            // reversed, becasue the native accelerometer reports seem to
+            // assume that the card is component side down.
+            js.update(x, -y, z, 0);
         }
-    }    
+        
+        // show a heartbeat flash in blue every so often
+        if (hbTimer.read_ms() - t0Hb > 1000) 
+        {
+            // invert the blue LED state
+            hb = !hb;
+            led3 = (hb ? .5 : 1);
+            
+            // reset the heartbeat timer
+            hbTimer.reset();
+            t0Hb = hbTimer.read_ms();
+        }
+    }
 }