![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
work in progress
Dependencies: FastAnalogIn FastIO USBDevice mbed FastPWM SimpleDMA
Fork of Pinscape_Controller by
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(); + } + } }