Diff: main.cpp

Revision:

5:a70c0bce770d

Parent:

4:02c7cd7b2183

Child:

6:cc35eb643e8f

diff -r 02c7cd7b2183 -r a70c0bce770d main.cpp
--- a/main.cpp	Thu Jul 24 05:50:36 2014 +0000
+++ b/main.cpp	Sun Jul 27 18:24:51 2014 +0000
@@ -1,3 +1,109 @@
+/* Copyright 2014 M J Roberts, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+//
+// Pinscape Controller
+//
+// "Pinscape" is the name of my custom-built virtual pinball cabinet.  I wrote this
+// software to perform a number of tasks that I needed for my cabinet.  It runs on a
+// Freescale KL25Z microcontroller, which is a small and inexpensive device that
+// attaches to the host PC via USB and can interface with numerous types of external
+// hardware.
+//
+// I designed the software and hardware in this project especially for Pinscape, but 
+// it uses standard interfaces in Windows and Visual Pinball, so it should be
+// readily usable in anyone else's VP-based cabinet.  I've tried to document the
+// hardware in enough detail for anyone else to duplicate the entire project, and
+// the full software is open source.
+//
+// The controller provides the following functions.  It should be possible to use
+// any subet of the features without using all of them.  External hardware for any
+// particular function can simply be omitted if that feature isn't needed.
+//
+//  - Nudge sensing via the KL25Z's on-board accelerometer.  Nudge accelerations are
+//    processed into a physics model of a rolling ball, and changes to the ball's
+//    motion are sent to the host computer via the joystick interface.  This is designed
+//    especially to work with Visuall Pinball's nudge handling to produce realistic 
+//    on-screen results in VP.  By doing some physics modeling right on the device, 
+//    rather than sending raw accelerometer data to VP, we can produce better results
+//    using our awareness of the real physical parameters of a pinball cabinet.
+//    VP's nudge handling has to be more generic, so it can't make the same sorts
+//    of assumptions that we can about the dynamics of a real cabinet.
+//
+//    The nudge data reports are compatible with the built-in Windows USB joystick 
+//    drivers and with VP's own joystick input scheme, so the nudge sensing is almost 
+//    plug-and-play.  There are no Windiows drivers to install, and the only VP work 
+//    needed is to customize a few global preference settings.
+//
+//  - Plunger position sensing via an attached TAOS TSL 1410R CCD linear array sensor.  
+//    The sensor must be wired to a particular set of I/O ports on the KL25Z, and must 
+//    be positioned adjacent to the plunger with proper lighting.  The physical and
+//    electronic installation details are desribed in the project documentation.  We read 
+//    the CCD to determine how far back the plunger is pulled, and report this to Visual 
+//    Pinball via the joystick interface.  As with the nudge data, this is all nearly
+//    plug-and-play, in that it works with the default Windows USB drivers and works 
+//    with the existing VP handling for analog plunger input.  A few VP settings are
+//    needed to tell VP to allow the plunger.
+//
+//    Unfortunately, analog plungers are not well supported by individual tables,
+//    so some work is required for each table to give it proper support.  I've tried
+//    to reduce this to a recipe and document it in the project documentation.
+//
+//  - In addition to the CCD sensor, a button should be attached (also described in 
+//    the project documentation) to activate calibration mode for the plunger.  When 
+//    calibration mode is activated, the software reads the plunger position for about 
+//    10 seconds when to note the limits of travel, and uses these limits to ensure
+//    accurate reports to VP that properly report the actual position of the physical
+//    plunger.  The calibration is stored in non-volatile memory on the KL25Z, so it's
+//    only necessary to calibrate once - the calibration will survive power cycling
+//    and reboots of the PC.  It's only necessary to recalibrate if the CCD sensor or
+//    the plunger are removed and reinstalled, since the relative alignment of the
+//    parts could cahnge slightly when reinstalling.
+//
+//  - LedWiz emulation.  The KL25Z can appear to the PC as an LedWiz device, and will
+//    accept and process LedWiz commands from the host.  The software can turn digital
+//    output ports on and off, and can set varying PWM intensitiy levels on a subset
+//    of ports.  (The KL25Z can only provide 6 PWM ports.  Intensity level settings on
+//    other ports is ignored, so non-PWM ports can only be used for simple on/off
+//    devices such as contactors and solenoids.)  The KL25Z can only supply 4mA on its
+//    output ports, so external hardware is required to take advantage of the LedWiz
+//    emulation.  Many different hardware designs are possible, but there's a simple
+//    reference design in the documentation that uses a Darlington array IC to
+//    increase the output from each port to 500mA (the same level as the LedWiz),
+//    plus an extended design that adds an optocoupler and MOSFET to provide very
+//    high power handling, up to about 45A or 150W, with voltages up to 100V.
+//    That will handle just about any DC device directly (wtihout relays or other
+//    amplifiers), and switches fast enough to support PWM devices.
+//
+//    The device can report any desired LedWiz unit number to the host, which makes
+//    it possible to use the LedWiz emulation on a machine that also has one or more
+//    actual LedWiz devices intalled.  The LedWiz design allows for up to 16 units
+//    to be installed in one machine - each one is invidually addressable by its
+//    distinct unit number.
+//
+//    The LedWiz emulation features are of course optional.  There's no need to 
+//    build any of the external port hardware (or attach anything to the output 
+//    ports at all) if the LedWiz features aren't needed.  Most people won't have
+//    any use for the LedWiz features.  I built them mostly as a learning exercise,
+//    but with a slight practical need for a handful of extra ports (I'm using the
+//    cutting-edge 10-contactor setup, so my real LedWiz is full!).
+
+
 #include "mbed.h"
 #include "USBJoystick.h"
 #include "MMA8451Q.h"
@@ -5,25 +111,35 @@
 #include "FreescaleIAP.h"
 #include "crc32.h"
 
-// customization of the joystick class to expose connect/suspend status
-class MyUSBJoystick: public USBJoystick
-{
-public:
-    MyUSBJoystick(uint16_t vendor_id, uint16_t product_id, uint16_t product_release) 
-        : USBJoystick(vendor_id, product_id, product_release, false)
-    {
-        suspended_ = false;
-    }
-    
-    int isConnected() { return configured(); }
-    int isSuspended() const { return suspended_; }
-    
-protected:
-    virtual void suspendStateChanged(unsigned int suspended)
-        { suspended_ = suspended; }
+
+// ---------------------------------------------------------------------------
+//
+// Configuration details
+//
 
-    int suspended_; 
-};
+// Our USB device vendor ID, product ID, and version.  
+// We use the vendor ID for the LedWiz, so that the PC-side software can
+// identify us as capable of performing LedWiz commands.  The LedWiz uses
+// a product ID value from 0xF0 to 0xFF; the last four bits identify the
+// unit number (e.g., product ID 0xF7 means unit #7).  This allows multiple
+// LedWiz units to be installed in a single PC; the software on the PC side
+// uses the unit number to route commands to the devices attached to each
+// unit.  On the real LedWiz, the unit number must be set in the firmware
+// at the factory; it's not configurable by the end user.  Most LedWiz's
+// ship with the unit number set to 0, but the vendor will set different
+// unit numbers if requested at the time of purchase.  So if you have a
+// single LedWiz already installed in your cabinet, and you didn't ask for
+// a non-default unit number, your existing LedWiz will be unit 0.
+//
+// We use unit #7 by default.  There doesn't seem to be a requirement that
+// unit numbers be contiguous (DirectOutput Framework and other software
+// seem happy to have units 0 and 7 installed, without 1-6 existing).
+// Marking this unit as #7 should work for almost everybody out of the box;
+// the most common case seems to be to have a single LedWiz installed, and
+// it's probably extremely rare to more than two.
+const uint16_t USB_VENDOR_ID = 0xFAFA;
+const uint16_t USB_PRODUCT_ID = 0x00F7;
+const uint16_t USB_VERSION_NO = 0x0004;
 
 // On-board RGB LED elements - we use these for diagnostic displays.
 DigitalOut ledR(LED1), ledG(LED2), ledB(LED3);
@@ -32,6 +148,24 @@
 DigitalIn calBtn(PTE29);
 DigitalOut calBtnLed(PTE23);
 
+// I2C address of the accelerometer (this is a constant of the KL25Z)
+const int MMA8451_I2C_ADDRESS = (0x1d<<1);
+
+// SCL and SDA pins for the accelerometer (constant for the KL25Z)
+#define MMA8451_SCL_PIN   PTE25
+#define MMA8451_SDA_PIN   PTE24
+
+// Digital in pin to use for the accelerometer interrupt.  For the KL25Z,
+// this can be either PTA14 or PTA15, since those are the pins physically
+// wired on this board to the MMA8451 interrupt controller.
+#define MMA8451_INT_PIN   PTA15
+
+
+// ---------------------------------------------------------------------------
+//
+// LedWiz emulation
+//
+
 static int pbaIdx = 0;
 
 // on/off state for each LedWiz output
@@ -70,22 +204,14 @@
     ledB = wizState(2);
 }
 
-struct AccPrv
-{
-    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);
-    }
-};
+// ---------------------------------------------------------------------------
+//
+// Non-volatile memory (NVM)
+//
 
-// Non-volatile memory structure.  We store persistent a small
+// Structure defining our NVM storage layout.  We store a small
 // amount of persistent data in flash memory to retain calibration
-// data between sessions.
+// data when powered off.
 struct NVM
 {
     // checksum - we use this to determine if the flash record
@@ -113,33 +239,211 @@
     } d;
 };
 
-// Accelerometer handler
-const int MMA8451_I2C_ADDRESS = (0x1d<<1);
+
+// ---------------------------------------------------------------------------
+//
+// Customization joystick subbclass
+//
+
+class MyUSBJoystick: public USBJoystick
+{
+public:
+    MyUSBJoystick(uint16_t vendor_id, uint16_t product_id, uint16_t product_release) 
+        : USBJoystick(vendor_id, product_id, product_release, true)
+    {
+        suspended_ = false;
+    }
+    
+    // are we connected?
+    int isConnected()  { return configured(); }
+    
+    // Are we in suspend mode?
+    int isSuspended() const { return suspended_; }
+    
+protected:
+    virtual void suspendStateChanged(unsigned int suspended)
+        { suspended_ = suspended; }
+
+    // are we suspended?
+    int suspended_; 
+};
+
+// ---------------------------------------------------------------------------
+// 
+// Accelerometer (MMA8451Q)
+//
+
+// The MMA8451Q is the KL25Z's on-board 3-axis accelerometer.
+//
+// This is a custom wrapper for the library code to interface to the
+// MMA8451Q.  This class encapsulates an interrupt handler and some
+// special data processing to produce more realistic results in
+// Visual Pinball.
+//
+// We install an interrupt handler on the accelerometer "data ready" 
+// interrupt in order to ensure that we fetch each sample immediately
+// when it becomes available.  Since our main program loop is busy
+// reading the CCD virtually all of the time, it wouldn't be practical
+// to keep up with the accelerometer data stream by polling.
+//
+// Visual Pinball is nominally designed to accept raw accelerometer
+// data as nudge input, but in practice, this doesn't produce
+// very realistic results.  VP simply applies accelerations from a
+// physical accelerometer directly to its modeled ball(s), but the
+// data stream coming from a real accelerometer isn't as clean as
+// an idealized physics simulation.  The problem seems to be that the
+// accelerometer samples capture instantaneous accelerations, not
+// integrated acceleration over time.  In other words, adding samples 
+// over time doesn't accurately reflect the actual net acceleration
+// experienced.  The longer the sampling period, the greater the
+// divergence between the sum of a series of samples and the actual
+// net acceleration.  The effect in VP is to leave the ball with
+// an unrealistically high residual velocity over the course of a
+// nudge event.
+//
+// This is where our custom data processing comes into play.  Rather
+// than sending raw accelerometer samples, we apply the samples to
+// our own virtual model ball.  What we send VP is the accelerations
+// experienced by the ball in our model, not the actual accelerations
+// we read from the MMA8451Q.  Now, that might seem like an unnecessary
+// middleman, because VP is just going to apply the accelerations to
+// its own model ball.  But it's a useful middleman: what we can do
+// in our model that VP can't do in its model is take into account
+// our special knowledge of the physical cabinet configuration.  VP
+// has to work generically with any sort of nudge input device, but
+// we can make assumptions about what kind of physical environment
+// we're operating in.
+//
+// The key assumption we make about our physical environment is that
+// accelerations from nudges should net out to zero over intervals on
+// the order of a couple of seconds.  Nudging a pinball cabinet makes
+// the cabinet accelerate briefly in the nudge direction, then rebound,
+// then re-rebound, and so on until the swaying motion damps out and
+// the table returns roughly to rest.  The table doesn't actually go
+// anywhere in these transactions, so the net acceleration experienced
+// is zero by the time the motion has damped out.  The damping time
+// depends on the degree of force of the nudge, but is a second or
+// two in most cases.
+//
+// We can't just assume that all motion and/or acceleration must stop 
+// in a second or two, though.  For one thing, the player can nudge
+// the table repeatedly for long periods.  (Doing this too aggressivly
+// will trigger a tilt, so there are limits, but a skillful player
+// can keep nudging a table almost continuously without tilting it.)
+// For another, a player could actually pick up one end of the table
+// for an extended period, applying a continuous acceleration the
+// whole time.
+//
+// The strategy we use to cope with these possibilities is to model a
+// ball, rather like VP does, but with damping that scales with the
+// current speed.  We'll choose a damping function that will bring
+// the ball to rest from any reasonable speed within a second or two
+// if there are no ongoing accelerations.  The damping function must
+// also be weak enough that new accelerations dominate - that is,
+// the damping function must not be so strong that it cancels out
+// ongoing physical acceleration input, such as when the player
+// lifts one end of the table and holds it up for a while.
+//
+// What we report to VP is the acceleration experienced by our model
+// ball between samples.  Our model ball starts at rest, and our damping
+// function ensures that when it's in motion, it will return to rest in
+// a short time in the absence of further physical accelerations.  The
+// sum or our reports to VP from a rest state to a subsequent rest state
+// will thus necessarily equal exactly zero.  This will ensure that we 
+// don't leave VP's model ball with any residual velocity after an 
+// isolated nudge.
+//
+// We do one more bit of data processing: automatic calibration.  When
+// we observe the accelerometer input staying constant (within a noise
+// window) for a few seconds continously, we'll assume that the cabinet
+// is at rest.  It's safe to assume that the accelerometer isn't
+// installed in such a way that it's perfectly level, so at the
+// cabinet's neutral rest position, we can expect to read non-zero
+// accelerations on the x and y axes from the component along that
+// axis of the Earth's gravity.  By watching for constant acceleration
+// values over time, we can infer the reseting position of the device
+// and take that as our zero point.  By doing this continuously, we
+// don't have to assume that the machine is perfectly motionless when
+// initially powered on - we'll organically find the zero point as soon
+// as the machine is undisturbed for a few moments.  We'll also deal
+// gracefully with situations where the machine is jolted so much in
+// the course of play that its position is changed slightly.  The result
+// should be to make the zeroing process reliable and completely 
+// transparent to the user.
+//
+
+// point structure
+struct FPoint
+{
+    float x, y;
+    
+    FPoint() { }
+    FPoint(float x, float y) { this->x = x; this->y = y; }
+    
+    void set(float x, float y) { this->x = x; this->y = y; }
+    void zero() { this->x = this->y = 0; }
+    
+    FPoint &operator=(FPoint &pt) { this->x = pt.x; this->y = pt.y; return *this; }
+    FPoint &operator-=(FPoint &pt) { this->x -= pt.x; this->y -= pt.y; return *this; }
+    FPoint &operator+=(FPoint &pt) { this->x += pt.x; this->y += pt.y; return *this; }
+    FPoint &operator*=(float f) { this->x *= f; this->y *= f; return *this; }
+    FPoint &operator/=(float f) { this->x /= f; this->y /= f; return *this; }
+    float magnitude() const { return sqrt(x*x + y*y); }
+    
+    float distance(FPoint &b)
+    {
+        float dx = x - b.x;
+        float dy = y - b.y;
+        return sqrt(dx*dx + dy*dy);
+    }
+};
+
+
+// accelerometer wrapper class
 class Accel
 {
 public:
     Accel(PinName sda, PinName scl, int i2cAddr, PinName irqPin)
         : mma_(sda, scl, i2cAddr), intIn_(irqPin)
     {
+        // remember the interrupt pin assignment
+        irqPin_ = irqPin;
+
+        // reset and initialize
+        reset();
+    }
+    
+    void reset()
+    {
+        // assume initially that the device is perfectly level
+        center_.zero();
+        tCenter_.start();
+        iAccPrv_ = nAccPrv_ = 0;
+
+        // reset and initialize the MMA8451Q
+        mma_.init();
+        
         // set the initial ball velocity to zero
-        vx_ = vy_ = 0;
+        v_.zero();
         
         // set the initial raw acceleration reading to zero
-        xRaw_ = yRaw_ = 0;
+        araw_.zero();
+        vsum_.zero();
 
         // enable the interrupt
-        mma_.setInterruptMode(irqPin == PTA14 ? 1 : 2);
+        mma_.setInterruptMode(irqPin_ == PTA14 ? 1 : 2);
         
         // set up the interrupt handler
         intIn_.rise(this, &Accel::isr);
         
         // read the current registers to clear the data ready flag
         float z;
-        mma_.getAccXYZ(xRaw_, yRaw_, z);
+        mma_.getAccXYZ(araw_.x, araw_.y, z);
 
         // start our timers
         tGet_.start();
         tInt_.start();
+        tRest_.start();
     }
     
     void get(float &x, float &y, float &rx, float &ry) 
@@ -148,11 +452,11 @@
          __disable_irq();
          
          // read the shared data and store locally for calculations
-         float vx = vx_, vy = vy_, xRaw = xRaw_, yRaw = yRaw_;
+         FPoint vsum = vsum_, araw = araw_;
+         
+         // reset the velocity sum
+         vsum_.zero();
 
-         // reset the velocity
-         vx_ = vy_ = 0;
-         
          // get the time since the last get() sample
          float dt = tGet_.read_us()/1.0e6;
          tGet_.reset();
@@ -160,16 +464,178 @@
          // done manipulating the shared data
          __enable_irq();
          
-         // calculate the acceleration since the last get(): a = dv/dt
-         x = vx/dt;
-         y = vy/dt;         
+         // check for auto-centering every so often
+         if (tCenter_.read_ms() > 1000)
+         {
+             // add the latest raw sample to the history list
+             accPrv_[iAccPrv_] = araw_;
+             
+             // commit the history entry
+             iAccPrv_ = (iAccPrv_ + 1) % maxAccPrv;
+
+             // if we have a full complement, check for stability
+             if (nAccPrv_ >= maxAccPrv)
+             {
+                 // check if we've been stable for all recent samples
+                 static const float accTol = .005;
+                 if (accPrv_[0].distance(accPrv_[1]) < accTol
+                     && accPrv_[0].distance(accPrv_[2]) < accTol
+                     && accPrv_[0].distance(accPrv_[3]) < accTol
+                     && accPrv_[0].distance(accPrv_[4]) < accTol)
+                 {
+                     // figure the new center as the average of these samples
+                     center_.set(
+                        (accPrv_[0].x + accPrv_[1].x + accPrv_[2].x + accPrv_[3].x + accPrv_[4].x)/5.0,
+                        (accPrv_[0].y + accPrv_[1].y + accPrv_[2].y + accPrv_[3].y + accPrv_[4].y)/5.0);
+                 }
+             }
+             else
+             {
+                // not enough samples yet; just up the count
+                ++nAccPrv_;
+             }
+            
+             // reset the timer
+             tCenter_.reset();
+         }
+
+         // Calculate the velocity vector for the model ball.  Start
+         // with the accumulated velocity from the accelerations since
+         // the last reading.
+         FPoint dv = vsum;
+
+         // remember the previous velocity of the model ball
+         FPoint vprv = v_;
+         
+         // If we have residual motion, check for damping.
+         //
+         // The dmaping we model here isn't friction - we leave that sort of
+         // detail to the pinball simulator on the PC.  Instead, our form of
+         // damping is just an attempt to compensate for measurement errors
+         // from the accelerometer.  During a nudge event, we should see a
+         // series of accelerations back and forth, as the table sways in
+         // response to the push, rebounds from the sway, rebounds from the
+         // rebound, etc.  We know that in reality, the table itself doesn't
+         // actually go anywhere - it just sways, and when the swaying stops,
+         // it ends up where it started.  If we use the accelerometer input
+         // to do dead reckoning on the location of the table, we know that
+         // it has to end up where it started.  This means that the series of
+         // position changes over the course of the event should cancel out -
+         // the displacements should add up to zero.  
          
-         // return the raw accelerometer data in rx,ry
-         rx = xRaw;
-         ry = yRaw;
+          to model friction and other forces
+         // on the ball.  Instead, the damping we apply is to compensate for
+         // measurement errors in the accelerometer.  During a nudge event,
+         // a real pinball cabinet typically ends up at the same place it
+         // started - it sways in response to the nudge, but the swaying
+         // quickly damps out and leaves the table unmoved.  You don't
+         // typically apply enough force to actually pick up the cabinet
+         // and move it, or slide it across the floor - and doing so would
+         // trigger a tilt, in which case the ball goes out of play and we
+         // don't really have to worry about how realistically it behaves
+         // in response to the acceleration.
+         if (vprv.magnitude() != 0)
+         {
+             // The model ball is moving.  If the current motion has been
+             // going on for long enough, apply damping.  We wait a short
+             // time before we apply damping to allow small continuous
+             // accelerations (from tiling the table) to get the ball
+             // rolling.
+             if (tRest_.read_ms() > 100)
+             {
+             }
+         }
+         else
+         {
+             // the model ball is at rest; if the instantaneous acceleration
+             // is also near zero, reset the rest timer
+             if (dv.magnitude() < 0.025)
+                 tRest_.reset();
+         }
+         
+         // If the current velocity change is near zero, damp the ball's
+         // velocity.  The idea is that the total series of accelerations 
+         // from a nudge should net to zero, since a nudge doesn't
+         // actually move the table anywhere.  
+         // 
+         // Ideally, this wouldn't be necessary, because the raw
+         // accelerometer readings should organically add up to zero over
+         // the course of a nudge.  In practice, the accelerometer isn't
+         // perfect; it can only sample so fast, so it can't capture every
+         // instantaneous change; and each reading has some small measurement
+         // error, which becomes significant when many readings are added
+         // together.  The damping is an attempt to reconcile the imperfect
+         // measurements with what how expect the real physical system to
+         // behave - we know what the outcome of an event should be, so we
+         // adjust our measurements to get the expected outcome.
+         //
+         // If the ball's velocity is large at this point, assume that this
+         // wasn't a nudge event at all, but a sustained inclination - as
+         // though the player picked up one end of the table and held it
+         // up for a while, to accelerate the ball down the sloped table.
+         // In this case just reset the velocity to zero without doing
+         // any damping, so that we don't pass through any deceleration
+         // to the pinball simulation.  In this case we want to leave it
+         // to the pinball simulation to do its own modeling of friction
+         // or bouncing to decelerate the ball.  Our correction is only
+         // realistic for brief events that naturally net out to neutral
+         // accelerations.
+         if (dv.magnitude() < .025)
+         {
+            // check the ball's speed
+            if (v_.magnitude() < .25)
+            {
+                // apply the damping
+                FPoint damp(damping(v_.x), damping(v_.y));
+                dv -= damp;
+                ledB = 0;
+            }
+            else
+            {
+                // the ball is going too fast - simply reset it
+                v_ = dv;
+                vprv = dv;
+                ledB = 1;
+            }
+         }
+         else
+             ledB = 1;
+         
+         // apply the velocity change for this interval
+         v_ += dv;
+         
+         // return the acceleration since the last update (change in velocity
+         // over time) in x,y
+         dv /= dt;
+         x = (v_.x - vprv.x) / dt;
+         y = (v_.y - vprv.y) / dt;
+         
+         // report the calibrated instantaneous acceleration in rx,ry
+         rx = araw.x - center_.x;
+         ry = araw.y - center_.y;
      }    
     
 private:
+    // velocity damping function
+    float damping(float v)
+    {
+        // scale to -2048..2048 range, and get the absolute value
+        float a = fabs(v*2048.0);
+        
+        // damp out small velocities immediately
+        if (a < 20)
+            return v;
+        
+        // calculate the cube root of the scaled value
+        float r = exp(log(a)/3.0);
+        
+        // rescale
+        r /= 2048.0;
+        
+        // apply the sign and return the result
+        return (v < 0 ? -r : r);
+    }
+
     // interrupt handler
     void isr()
     {
@@ -178,39 +644,101 @@
         // the "data ready" status bit in the accelerometer.  The
         // interrupt only occurs when the "ready" bit transitions from
         // off to on, so we have to make sure it's off.
-        float z;
-        mma_.getAccXYZ(xRaw_, yRaw_, z);
+        float x, y, z;
+        mma_.getAccXYZ(x, y, z);
+
+        // store the raw results
+        araw_.set(x, y);
+        zraw_ = z;
         
         // calculate the time since the last interrupt
         float dt = tInt_.read_us()/1.0e6;
         tInt_.reset();
         
-        // Accelerate the model ball: v = a*dt.  Assume that the raw
-        // data from the accelerometer reflects the average physical
-        // acceleration over the interval since the last sample.
-        vx_ += xRaw_ * dt;
-        vy_ += yRaw_ * dt;
+        // Add the velocity to the running total.  First, calibrate the
+        // raw acceleration to our centerpoint, then multiply by the time
+        // since the last sample to get the velocity resulting from
+        // applying this acceleration for the sample time.
+        FPoint rdt((x - center_.x)*dt, (y - center_.y)*dt);
+        vsum_ += rdt;
     }
     
-    // current modeled ball velocity
-    float vx_, vy_;
-    
-    // last raw axis readings
-    float xRaw_, yRaw_;
-    
     // underlying accelerometer object
     MMA8451Q mma_;
     
-    // interrupt router
-    InterruptIn intIn_;
+    // last raw acceleration readings
+    FPoint araw_;
+    float zraw_;
+    
+    // total velocity change since the last get() sample
+    FPoint vsum_;
+    
+    // current modeled ball velocity
+    FPoint v_;
     
     // timer for measuring time between get() samples
     Timer tGet_;
     
     // timer for measuring time between interrupts
     Timer tInt_;
+    
+    // time since last rest
+    Timer tRest_;
+
+    // calibrated center point - this is the position where we observe
+    // constant input for a few seconds, telling us the orientation of
+    // the accelerometer device when at rest
+    FPoint center_;
+
+    // timer for atuo-centering
+    Timer tCenter_;
+    
+    // recent accelerometer readings, for auto centering
+    int iAccPrv_, nAccPrv_;
+    static const int maxAccPrv = 5;
+    FPoint accPrv_[maxAccPrv];
+
+    // interurupt pin name
+    PinName irqPin_;
+    
+    // interrupt router
+    InterruptIn intIn_;
 };
 
+
+// ---------------------------------------------------------------------------
+//
+// Clear the I2C bus for the MMA8451!.  This seems necessary some of the time
+// for reasons that aren't clear to me.  Doing a hard power cycle has the same
+// effect, but when we do a soft reset, the hardware sometimes seems to leave
+// the MMA's SDA line stuck low.  Forcing a series of 9 clock pulses through
+// the SCL line is supposed to clear this conidtion.
+//
+void clear_i2c()
+{
+    // assume a general-purpose output pin to the I2C clock
+    DigitalOut scl(MMA8451_SCL_PIN);
+    DigitalIn sda(MMA8451_SDA_PIN);
+    
+    // clock the SCL 9 times
+    for (int i = 0 ; i < 9 ; ++i)
+    {
+        scl = 1;
+        wait_us(20);
+        scl = 0;
+        wait_us(20);
+    }
+}
+
+// ---------------------------------------------------------------------------
+//
+// Main program loop.  This is invoked on startup and runs forever.  Our
+// main work is to read our devices (the accelerometer and the CCD), process
+// the readings into nudge and plunger position data, and send the results
+// to the host computer via the USB joystick interface.  We also monitor
+// the USB connection for incoming LedWiz commands and process those into
+// port outputs.
+//
 int main(void)
 {
     // turn off our on-board indicator LED
@@ -218,6 +746,12 @@
     ledG = 1;
     ledB = 1;
     
+    // clear the I2C bus for the accelerometer
+    clear_i2c();
+    
+    // Create the joystick USB client
+    MyUSBJoystick js(USB_VENDOR_ID, USB_PRODUCT_ID, USB_VERSION_NO);
+
     // set up a flash memory controller
     FreescaleIAP iap;
     
@@ -234,11 +768,17 @@
     // Number of pixels we read from the sensor on each frame.  This can be
     // less than the physical pixel count if desired; we'll read every nth
     // piexl if so.  E.g., with a 1280-pixel physical sensor, if npix is 320,
-    // we'll read every 4th pixel.  VP doesn't seem to have very high
-    // resolution internally for the plunger, so it's probably not necessary
-    // to use the full resolution of the sensor - about 160 pixels seems
-    // perfectly adequate.  We can read the sensor faster (and thus provide
-    // a higher refresh rate) if we read fewer pixels in each frame.
+    // we'll read every 4th pixel.  It takes time to read each pixel, so the
+    // fewer pixels we read, the higher the refresh rate we can achieve.
+    // It's therefore better not to read more pixels than we have to.
+    //
+    // VP seems to have an internal resolution in the 8-bit range, so there's
+    // no apparent benefit to reading more than 128-256 pixels when using VP.
+    // Empirically, 160 pixels seems about right.  The overall travel of a
+    // standard pinball plunger is about 3", so 160 pixels gives us resolution
+    // of about 1/50".  This seems to take full advantage of VP's modeling
+    // ability, and is probably also more precise than a human player's
+    // perception of the plunger position.
     const int npix = 160;
 
     // if the flash is valid, load it; otherwise initialize to defaults
@@ -271,34 +811,22 @@
     // set up a timer for our heartbeat indicator
     Timer hbTimer;
     hbTimer.start();
-    int t0Hb = hbTimer.read_ms();
     int hb = 0;
+    uint16_t hbcnt = 0;
     
     // set a timer for accelerometer auto-centering
     Timer acTimer;
     acTimer.start();
-    int t0ac = acTimer.read_ms();
     
-    // Create the joystick USB client
-    MyUSBJoystick js(0xFAFA, 0x00F7, 0x0003);
-
     // create the accelerometer object
-    Accel accel(PTE25, PTE24, MMA8451_I2C_ADDRESS, PTA15);
+    Accel accel(MMA8451_SCL_PIN, MMA8451_SDA_PIN, MMA8451_I2C_ADDRESS, MMA8451_INT_PIN);
     
     // create the CCD array object
     TSL1410R ccd(PTE20, PTE21, PTB0);
     
-    // recent accelerometer readings, for auto centering
-    int iAccPrv = 0, nAccPrv = 0;
-    const int maxAccPrv = 5;
-    AccPrv accPrv[maxAccPrv];
-
     // 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;    
-    
     // start the first CCD integration cycle
     ccd.clear();
 
@@ -542,116 +1070,55 @@
         float xa, ya, rxa, rya;
         accel.get(xa, ya, rxa, rya);
         
-        // 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
+        // confine the accelerometer results 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;
 
-        // figure the new mouse report data
-        int xnew = (int)(127 * xa);
-        int ynew = (int)(127 * ya);
+        // scale to our -127..127 reporting range
+        int xnew = int(127 * xa);
+        int ynew = int(127 * ya);
 
         // store the updated joystick coordinates
         x = xnew;
         y = ynew;
         z = znew;
         
-        // if we're in USB suspend or disconnect mode, spin
-        if (js.isSuspended() || !js.isConnected())
-        {
-            // go dark (turn off the indicator LEDs)
-            ledG = 1;
-            ledB = 1;
-            ledR = 1;
-            
-            // wait until we're connected and come out of suspend mode
-            for (uint32_t n = 0 ; js.isSuspended() || !js.isConnected() ; ++n)
-            {
-                // spin for a bit
-                wait(1);
-                
-                // if we're suspended, do a brief red flash; otherwise do a long red flash
-                if (js.isSuspended())
-                {
-                    // suspended - flash briefly ever few seconds
-                    if (n % 3 == 0)
-                    {
-                        ledR = 0;
-                        wait(0.05);
-                        ledR = 1;
-                    }
-                }
-                else
-                {
-                    // running, not connected - flash red
-                    ledR = !ledR;
-                }
-            }
-        }
-
         // Send the status report.  It doesn't really matter what
         // coordinate system we use, since Visual Pinball has config
         // options for rotations and axis reversals, but reversing y
         // at the device level seems to produce the most intuitive 
         // results for the Windows joystick control panel view, which
         // is an easy way to check that the device is working.
-        js.update(x, -y, z, int(rxa*127), int(rya*127), 0);
+        //
+        // $$$ button updates are for diagnostics, so we can see that the
+        // device is sending data properly if the accelerometer gets stuck
+        js.update(x, -y, z, int(rxa*127), int(rya*127), hb ? 0x5500 : 0xAA00);
         
         // show a heartbeat flash in blue every so often if not in 
         // calibration mode
-        if (calBtnState < 2 && hbTimer.read_ms() - t0Hb > 1000) 
+        if (calBtnState < 2 && hbTimer.read_ms() > 1000) 
         {
-            if (js.isSuspended())
+            if (js.isSuspended() || !js.isConnected())
             {
-                // suspended - turn off the LEDs entirely
+                // suspended - turn off the LED
                 ledR = 1;
                 ledG = 1;
                 ledB = 1;
-            }
-            else if (!js.isConnected())
-            {
-                // not connected - flash red
-                hb = !hb;
-                ledR = (hb ? 0 : 1);
-                ledG = 1;
-                ledB = 1;
+
+                // show a status flash every so often                
+                if (hbcnt % 3 == 0)
+                {
+                    // disconnected = red flash; suspended = red-red
+                    for (int n = js.isConnected() ? 1 : 2 ; n > 0 ; --n)
+                    {
+                        ledR = 0;
+                        wait(0.05);
+                        ledR = 1;
+                        wait(0.25);
+                    }
+                }
             }
             else if (flash_valid)
             {
@@ -665,14 +1132,14 @@
             {
                 // connected, factory reset - flash yellow/green
                 hb = !hb;
-                ledR = (hb ? 0 : 1);
-                ledG = 0;
+                //ledR = (hb ? 0 : 1);
+                //ledG = 0;
                 ledB = 1;
             }
             
             // reset the heartbeat timer
             hbTimer.reset();
-            t0Hb = hbTimer.read_ms();
+            ++hbcnt;
         }
     }
 }