Mike R
Pinscape Controller version 1 fork. This is a fork to allow for ongoing bug fixes to the original controller version, from before the major changes for the expansion board project.
Dependencies: FastIO FastPWM SimpleDMA mbed
Fork of
Pinscape_Controller
by 
Mike R
Diff: main.cpp
- Revision:
- 6:cc35eb643e8f
- Parent:
- 5:a70c0bce770d
- Child:
- 7:100a25f8bf56
--- a/main.cpp Sun Jul 27 18:24:51 2014 +0000
+++ b/main.cpp Wed Aug 06 23:08:07 2014 +0000
@@ -31,6 +31,16 @@
// hardware in enough detail for anyone else to duplicate the entire project, and
// the full software is open source.
//
+// The device appears to the host computer as a USB joystick. This works with the
+// standard Windows joystick device drivers, so there's no need to install any
+// software on the PC - Windows should recognize it as a joystick when you plug
+// it in and shouldn't ask you to install anything. If you bring up the control
+// panel for USB Game Controllers, this device will appear as "Pinscape Controller".
+// *Don't* do any calibration with the Windows control panel or third-part
+// calibration tools. The device calibrates itself automatically for the
+// accelerometer data, and has its own special calibration procedure for the
+// plunger (see below).
+//
// 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.
@@ -60,9 +70,34 @@
// 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.
+// For best results, the plunger sensor should be calibrated. The calibration
+// is stored in non-volatile memory on board the KL25Z, so it's only necessary
+// to do the calibration once, when you first install everything. (You might
+// also want to re-calibrate if you physically remove and reinstall the CCD
+// sensor or the mechanical plunger, since their alignment might change slightly
+// when you put everything back together.) To calibrate, you have to attach a
+// momentary switch (e.g., a push-button switch) between one of the KL25Z ground
+// pins (e.g., jumper J9 pin 12) and PTE29 (J10 pin 9). Press and hold the
+// button for about two seconds - the LED on the KL25Z wlil flash blue while
+// you hold the button, and will turn solid blue when you've held it down long
+// enough to enter calibration mode. This mode will last about 15 seconds.
+// Simply pull the plunger all the way back, hold it for a few moments, and
+// gradually return it to the starting position. *Don't* release it - we want
+// to measure the maximum retracted position and the rest position, but NOT
+// the maximum forward position when the outer barrel spring is compressed.
+// After about 15 seconds, the device will save the new calibration settings
+// to its flash memory, and the LED will return to the regular "heartbeat"
+// flashes. If this is the first time you calibrated, you should observe the
+// color of the flashes change from yellow/green to blue/green to indicate
+// that the plunger has been calibrated.
+//
+// Note that while Visual Pinball itself has good native support for analog
+// plungers, most of the VP tables in circulation don't implement the necessary
+// scripting features to make this work properly. Therefore, you'll have to do
+// a little scripting work for each table you download to add the required code
+// to that individual table. The work has to be customized for each table, so
+// I haven't been able to automate this process, but I have tried to reduce it
+// to a relatively simple recipe that I've documented separately.
//
// - 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
@@ -102,9 +137,44 @@
// 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!).
-
+//
+// The on-board LED on the KL25Z flashes to indicate the current device status:
+//
+// two short red flashes = the device is powered but hasn't successfully
+// connected to the host via USB (either it's not physically connected
+// to the USB port, or there was a problem with the software handshake
+// with the USB device driver on the computer)
+//
+// short red flash = the host computer is in sleep/suspend mode
+//
+// long red/green = the LedWiz unti number has been changed, so a reset
+// is needed. You can simply unplug the device and plug it back in,
+// or presss and hold the reset button on the device for a few seconds.
+//
+// long yellow/green = everything's working, but the plunger hasn't
+// been calibrated; follow the calibration procedure described above.
+// This flash mode won't appear if the CCD has been disabled. Note
+// that the device can't tell whether a CCD is physically attached,
+// so you should use the config command to disable the CCD software
+// features if you won't be attaching a CCD.
+//
+// alternating blue/green = everything's working
+//
+// Software configuration: you can change option settings by sending special
+// USB commands from the PC. I've provided a Windows program for this purpose;
+// refer to the documentation for details. For reference, here's the format
+// of the USB command for option changes:
+//
+// length of report = 8 bytes
+// byte 0 = 65 (0x41)
+// byte 1 = 1 (0x01)
+// byte 2 = new LedWiz unit number, 0x01 to 0x0f
+// byte 3 = feature enable bit mask:
+// 0x01 = enable CCD (default = on)
+
#include "mbed.h"
+#include "math.h"
#include "USBJoystick.h"
#include "MMA8451Q.h"
#include "tsl1410r.h"
@@ -137,9 +207,18 @@
// 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.
+//
+// Note that the USB_PRODUCT_ID value set here omits the unit number. We
+// take the unit number from the saved configuration. We provide a
+// configuration command that can be sent via the USB connection to change
+// the unit number, so that users can select the unit number without having
+// to install a different version of the software. We'll combine the base
+// product ID here with the unit number to get the actual product ID that
+// we send to the USB controller.
const uint16_t USB_VENDOR_ID = 0xFAFA;
-const uint16_t USB_PRODUCT_ID = 0x00F7;
-const uint16_t USB_VERSION_NO = 0x0004;
+const uint16_t USB_PRODUCT_ID = 0x00F0;
+const uint16_t USB_VERSION_NO = 0x0006;
+const uint8_t DEFAULT_LEDWIZ_UNIT_NUMBER = 0x07;
// On-board RGB LED elements - we use these for diagnostic displays.
DigitalOut ledR(LED1), ledG(LED2), ledB(LED3);
@@ -148,6 +227,105 @@
DigitalIn calBtn(PTE29);
DigitalOut calBtnLed(PTE23);
+// LED-Wiz emulation output pin assignments. The LED-Wiz protocol
+// can support up to 32 outputs. The KL25Z can physically provide
+// about 48 (in addition to the ports we're already using for the
+// CCD sensor and the calibration button), but to stay compatible
+// with the LED-Wiz protocol we'll stop at 32.
+//
+// The LED-Wiz protocol allows setting individual intensity levels
+// on all outputs, with 48 levels of intensity. This can be used
+// to control lamp brightness and motor speeds, among other things.
+// Unfortunately, the KL25Z only has 10 PWM channels, so while we
+// can support the full complement of 32 outputs, we can only provide
+// PWM dimming/speed control on 10 of them. The remaining outputs
+// can only be switched fully on and fully off - we can't support
+// dimming on these, so they'll ignore any intensity level setting
+// requested by the host. Use these for devices that don't have any
+// use for intensity settings anyway, such as contactors and knockers.
+//
+// The mapping between physical output pins on the KL25Z and the
+// assigned LED-Wiz port numbers is essentially arbitrary - you can
+// customize this by changing the entries in the array below if you
+// wish to rearrange the pins for any reason. Be aware that some
+// of the physical outputs are already used for other purposes
+// (e.g., some of the GPIO pins on header J10 are used for the
+// CCD sensor - but you can of course reassign those as well by
+// changing the corresponding declarations elsewhere in this module).
+// The assignments we make here have two main objectives: first,
+// to group the outputs on headers J1 and J2 (to facilitate neater
+// wiring by keeping the output pins together physically), and
+// second, to make the physical pin layout match the LED-Wiz port
+// numbering order to the extent possible. There's one big wrench
+// in the works, though, which is the limited number and discontiguous
+// placement of the KL25Z PWM-capable output pins. This prevents
+// us from doing the most obvious sequential ordering of the pins,
+// so we end up with the outputs arranged into several blocks.
+// Hopefully this isn't too confusing; for more detailed rationale,
+// read on...
+//
+// With the LED-Wiz, the host software configuration usually
+// assumes that each RGB LED is hooked up to three consecutive ports
+// (for the red, green, and blue components, which need to be
+// physically wired to separate outputs to allow each color to be
+// controlled independently). To facilitate this, we arrange the
+// PWM-enabled outputs so that they're grouped together in the
+// port numbering scheme. Unfortunately, these outputs aren't
+// together in a single group in the physical pin layout, so to
+// group them logically in the LED-Wiz port numbering scheme, we
+// have to break up the overall numbering scheme into several blocks.
+// So our port numbering goes sequentially down each column of
+// header pins, but there are several break points where we have
+// to interrupt the obvious sequence to keep the PWM pins grouped
+// logically.
+//
+// In the list below, "pin J1-2" refers to pin 2 on header J1 on
+// the KL25Z, using the standard pin numbering in the KL25Z
+// documentation - this is the physical pin that the port controls.
+// "LW port 1" means LED-Wiz port 1 - this is the LED-Wiz port
+// number that you use on the PC side (in the DirectOutput config
+// file, for example) to address the port. PWM-capable ports are
+// marked as such - we group the PWM-capable ports into the first
+// 10 LED-Wiz port numbers.
+//
+struct {
+ PinName pin;
+ bool isPWM;
+} ledWizPortMap[32] = {
+ { PTA1, true }, // pin J1-2, LW port 1 (PWM capable - TPM 2.0 = channel 9)
+ { PTA2, true }, // pin J1-4, LW port 2 (PWM capable - TPM 2.1 = channel 10)
+ { PTD4, true }, // pin J1-6, LW port 3 (PWM capable - TPM 0.4 = channel 5)
+ { PTA12, true }, // pin J1-8, LW port 4 (PWM capable - TPM 1.0 = channel 7)
+ { PTA4, true }, // pin J1-10, LW port 5 (PWM capable - TPM 0.1 = channel 2)
+ { PTA5, true }, // pin J1-12, LW port 6 (PWM capable - TPM 0.2 = channel 3)
+ { PTA13, true }, // pin J2-2, LW port 7 (PWM capable - TPM 1.1 = channel 13)
+ { PTD5, true }, // pin J2-4, LW port 8 (PWM capable - TPM 0.5 = channel 6)
+ { PTD0, true }, // pin J2-6, LW port 9 (PWM capable - TPM 0.0 = channel 1)
+ { PTD3, true }, // pin J2-10, LW port 10 (PWM capable - TPM 0.3 = channel 4)
+ { PTC8, false }, // pin J1-14, LW port 11
+ { PTC9, false }, // pin J1-16, LW port 12
+ { PTC7, false }, // pin J1-1, LW port 13
+ { PTC0, false }, // pin J1-3, LW port 14
+ { PTC3, false }, // pin J1-5, LW port 15
+ { PTC4, false }, // pin J1-7, LW port 16
+ { PTC5, false }, // pin J1-9, LW port 17
+ { PTC6, false }, // pin J1-11, LW port 18
+ { PTC10, false }, // pin J1-13, LW port 19
+ { PTC11, false }, // pin J1-15, LW port 20
+ { PTC12, false }, // pin J2-1, LW port 21
+ { PTC13, false }, // pin J2-3, LW port 22
+ { PTC16, false }, // pin J2-5, LW port 23
+ { PTC17, false }, // pin J2-7, LW port 24
+ { PTA16, false }, // pin J2-9, LW port 25
+ { PTA17, false }, // pin J2-11, LW port 26
+ { PTE31, false }, // pin J2-13, LW port 27
+ { PTD6, false }, // pin J2-17, LW port 29
+ { PTD7, false }, // pin J2-19, LW port 30
+ { PTE0, false }, // pin J2-18, LW port 31
+ { PTE1, false } // pin J2-20, LW port 32
+};
+
+
// I2C address of the accelerometer (this is a constant of the KL25Z)
const int MMA8451_I2C_ADDRESS = (0x1d<<1);
@@ -160,6 +338,9 @@
// wired on this board to the MMA8451 interrupt controller.
#define MMA8451_INT_PIN PTA15
+// Joystick axis report range - we report from -JOYMAX to +JOYMAX
+#define JOYMAX 4096
+
// ---------------------------------------------------------------------------
//
@@ -168,6 +349,47 @@
static int pbaIdx = 0;
+// LedWiz output pin interface. We create a cover class to virtualize
+// digital vs PWM outputs and give them a common interface. The KL25Z
+// unfortunately doesn't have enough hardware PWM channels to support
+// PWM on all 32 LedWiz outputs, so we provide as many PWM channels as
+// we can (10), and fill out the rest of the outputs with plain digital
+// outs.
+class LwOut
+{
+public:
+ virtual void set(float val) = 0;
+};
+class LwPwmOut: public LwOut
+{
+public:
+ LwPwmOut(PinName pin) : p(pin) { }
+ virtual void set(float val) { p = val; }
+ PwmOut p;
+};
+class LwDigOut: public LwOut
+{
+public:
+ LwDigOut(PinName pin) : p(pin) { }
+ virtual void set(float val) { p = val; }
+ DigitalOut p;
+};
+
+// output pin array
+static LwOut *lwPin[32];
+
+// initialize the output pin array
+void initLwOut()
+{
+ for (int i = 0 ; i < sizeof(lwPin) / sizeof(lwPin[0]) ; ++i)
+ {
+ PinName p = ledWizPortMap[i].pin;
+ lwPin[i] = (ledWizPortMap[i].isPWM
+ ? (LwOut *)new LwPwmOut(p)
+ : (LwOut *)new LwDigOut(p));
+ }
+}
+
// on/off state for each LedWiz output
static uint8_t wizOn[32];
@@ -199,9 +421,8 @@
static void updateWizOuts()
{
- ledR = wizState(0);
- ledG = wizState(1);
- ledB = wizState(2);
+ for (int i = 0 ; i < 32 ; ++i)
+ lwPin[i]->set(wizState(i));
}
// ---------------------------------------------------------------------------
@@ -215,27 +436,64 @@
struct NVM
{
// checksum - we use this to determine if the flash record
- // has been initialized
+ // has been properly initialized
uint32_t checksum;
// signature value
static const uint32_t SIGNATURE = 0x4D4A522A;
- static const uint16_t VERSION = 0x0002;
+ static const uint16_t VERSION = 0x0003;
+
+ // Is the data structure valid? We test the signature and
+ // checksum to determine if we've been properly stored.
+ int valid() const
+ {
+ return (d.sig == SIGNATURE
+ && d.vsn == VERSION
+ && d.sz == sizeof(NVM)
+ && checksum == CRC32(&d, sizeof(d)));
+ }
+
+ // save to non-volatile memory
+ void save(FreescaleIAP &iap, int addr)
+ {
+ // update the checksum and structure size
+ checksum = CRC32(&d, sizeof(d));
+ d.sz = sizeof(NVM);
+
+ // erase the sector
+ iap.erase_sector(addr);
+
+ // save the data
+ iap.program_flash(addr, this, sizeof(*this));
+ }
// stored data (excluding the checksum)
struct
{
- // signature and version - further verification that we have valid
- // initialized data
+ // Signature, structure version, and structure size - further verification
+ // that we have valid initialized data. The size is a simple proxy for a
+ // structure version, as the most common type of change to the structure as
+ // the software evolves will be the addition of new elements. We also
+ // provide an explicit version number that we can update manually if we
+ // make any changes that don't affect the structure size but would affect
+ // compatibility with a saved record (e.g., swapping two existing elements).
uint32_t sig;
uint16_t vsn;
+ int sz;
- // direction - 0 means unknown, 1 means bright end is pixel 0, 2 means reversed
- uint8_t dir;
-
+ // has the plunger been manually calibrated?
+ int plungerCal;
+
// plunger calibration min and max
int plungerMin;
+ int plungerZero;
int plungerMax;
+
+ // is the CCD enabled?
+ int ccdEnabled;
+
+ // LedWiz unit number
+ uint8_t ledWizUnitNo;
} d;
};
@@ -269,6 +527,14 @@
};
// ---------------------------------------------------------------------------
+//
+// Some simple math service routines
+//
+
+inline float square(float x) { return x*x; }
+inline float round(float x) { return x > 0 ? floor(x + 0.5) : ceil(x - 0.5); }
+
+// ---------------------------------------------------------------------------
//
// Accelerometer (MMA8451Q)
//
@@ -276,129 +542,63 @@
// 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.
+// MMA8451Q. This class encapsulates an interrupt handler and
+// automatic calibration.
//
// 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.
+// interrupt to ensure that we fetch each sample immediately when it
+// becomes available. The accelerometer data rate is fiarly high
+// (800 Hz), so it's not practical to keep up with it by polling.
+// Using an interrupt handler lets us respond quickly and read
+// every sample.
//
-// 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.
+// We automatically calibrate the accelerometer so that it's not
+// necessary to get it exactly level when installing it, and so
+// that it's also not necessary to calibrate it manually. There's
+// lots of experience that tells us that manual calibration is a
+// terrible solution, mostly because cabinets tend to shift slightly
+// during use, requiring frequent recalibration. Instead, we
+// calibrate automatically. We continuously monitor the acceleration
+// data, watching for periods of constant (or nearly constant) values.
+// Any time it appears that the machine has been at rest for a while
+// (about 5 seconds), we'll average the readings during that rest
+// period and use the result as the level rest position. This is
+// is ongoing, so we'll quickly find the center point again if the
+// machine is moved during play (by an especially aggressive bout
+// of nudging, say).
//
-// point structure
-struct FPoint
+// accelerometer input history item, for gathering calibration data
+struct AccHist
{
+ AccHist() { x = y = d = 0.0; xtot = ytot = 0.0; cnt = 0; }
+ void set(float x, float y, AccHist *prv)
+ {
+ // save the raw position
+ this->x = x;
+ this->y = y;
+ this->d = distance(prv);
+ }
+
+ // reading for this entry
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; }
+ // distance from previous entry
+ float d;
- 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); }
+ // total and count of samples averaged over this period
+ float xtot, ytot;
+ int cnt;
+
+ void clearAvg() { xtot = ytot = 0.0; cnt = 0; }
+ void addAvg(float x, float y) { xtot += x; ytot += y; ++cnt; }
+ float xAvg() const { return xtot/cnt; }
+ float yAvg() const { return ytot/cnt; }
- float distance(FPoint &b)
- {
- float dx = x - b.x;
- float dy = y - b.y;
- return sqrt(dx*dx + dy*dy);
- }
+ float distance(AccHist *p)
+ { return sqrt(square(p->x - x) + square(p->y - y)); }
};
-
// accelerometer wrapper class
class Accel
{
@@ -415,47 +615,42 @@
void reset()
{
- // assume initially that the device is perfectly level
- center_.zero();
+ // clear the center point
+ cx_ = cy_ = 0.0;
+
+ // start the calibration timer
tCenter_.start();
iAccPrv_ = nAccPrv_ = 0;
-
+
// reset and initialize the MMA8451Q
mma_.init();
-
- // set the initial ball velocity to zero
- v_.zero();
+
+ // set the initial integrated velocity reading to zero
+ vx_ = vy_ = 0;
- // set the initial raw acceleration reading to zero
- araw_.zero();
- vsum_.zero();
-
- // enable the interrupt
+ // set up our accelerometer interrupt handling
+ intIn_.rise(this, &Accel::isr);
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(araw_.x, araw_.y, z);
+ mma_.getAccXYZ(ax_, ay_, az_);
// start our timers
tGet_.start();
tInt_.start();
- tRest_.start();
}
- void get(float &x, float &y, float &rx, float &ry)
+ void get(int &x, int &y, int &rx, int &ry)
{
// disable interrupts while manipulating the shared data
__disable_irq();
// read the shared data and store locally for calculations
- FPoint vsum = vsum_, araw = araw_;
+ float ax = ax_, ay = ay_;
+ float vx = vx_, vy = vy_;
- // reset the velocity sum
- vsum_.zero();
+ // reset the velocity sum for the next run
+ vx_ = vy_ = 0;
// get the time since the last get() sample
float dt = tGet_.read_us()/1.0e6;
@@ -464,29 +659,39 @@
// done manipulating the shared data
__enable_irq();
+ // adjust the readings for the integration time
+ vx /= dt;
+ vy /= dt;
+
+ // add this sample to the current calibration interval's running total
+ AccHist *p = accPrv_ + iAccPrv_;
+ p->addAvg(ax, ay);
+
// 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
+ AccHist *prv = p;
iAccPrv_ = (iAccPrv_ + 1) % maxAccPrv;
+ p = accPrv_ + iAccPrv_;
+ p->set(ax, ay, prv);
// 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)
+ static const float accTol = .01;
+ AccHist *p0 = accPrv_;
+ if (p0[0].d < accTol
+ && p0[1].d < accTol
+ && p0[2].d < accTol
+ && p0[3].d < accTol
+ && p0[4].d < 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);
+ // Figure the new calibration point as the average of
+ // the samples over the rest period
+ cx_ = (p0[0].xAvg() + p0[1].xAvg() + p0[2].xAvg() + p0[3].xAvg() + p0[4].xAvg())/5.0;
+ cy_ = (p0[0].yAvg() + p0[1].yAvg() + p0[2].yAvg() + p0[3].yAvg() + p0[4].yAvg())/5.0;
}
}
else
@@ -494,146 +699,47 @@
// not enough samples yet; just up the count
++nAccPrv_;
}
+
+ // clear the new item's running totals
+ p->clearAvg();
// 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.
-
- 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;
+ // report our integrated velocity reading in x,y
+ x = rawToReport(vx);
+ y = rawToReport(vy);
- // 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;
+ // apply a small dead zone near the center
+ // if (abs(x) < 6) x = 0;
+ // if (abs(y) < 6) y = 0;
// report the calibrated instantaneous acceleration in rx,ry
- rx = araw.x - center_.x;
- ry = araw.y - center_.y;
+ rx = int(round((ax - cx_)*JOYMAX));
+ ry = int(round((ay - cy_)*JOYMAX));
+
+#ifdef DEBUG_PRINTF
+ if (x != 0 || y != 0)
+ printf("%f %f %d %d %f\r\n", vx, vy, x, y, dt);
+#endif
}
private:
- // velocity damping function
- float damping(float v)
+ // adjust a raw acceleration figure to a usb report value
+ int rawToReport(float v)
{
- // scale to -2048..2048 range, and get the absolute value
- float a = fabs(v*2048.0);
+ // scale to the joystick report range and round to integer
+ int i = int(round(v*JOYMAX));
- // 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);
+ // if it's near the center, scale it roughly as 20*(i/20)^2,
+ // to suppress noise near the rest position
+ static const int filter[] = {
+ -18, -16, -14, -13, -11, -10, -8, -7, -6, -5, -4, -3, -2, -2, -1, -1, 0, 0, 0, 0,
+ 0,
+ 0, 0, 0, 0, 1, 1, 2, 2, 3, 4, 5, 6, 7, 8, 10, 11, 13, 14, 16, 18
+ };
+ return (i > 20 || i < -20 ? i : filter[i+20]);
}
// interrupt handler
@@ -646,58 +752,60 @@
// off to on, so we have to make sure it's off.
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();
+
+ // integrate the time slice from the previous reading to this reading
+ vx_ += (x + ax_ - 2*cx_)*dt/2;
+ vy_ += (y + ay_ - 2*cy_)*dt/2;
- // 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;
+ // store the updates
+ ax_ = x;
+ ay_ = y;
+ az_ = z;
}
// underlying accelerometer object
MMA8451Q mma_;
// last raw acceleration readings
- FPoint araw_;
- float zraw_;
+ float ax_, ay_, az_;
- // total velocity change since the last get() sample
- FPoint vsum_;
-
- // current modeled ball velocity
- FPoint v_;
-
+ // integrated velocity reading since last get()
+ float vx_, vy_;
+
// 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_;
+ // Calibration reference point for accelerometer. This is the
+ // average reading on the accelerometer when in the neutral position
+ // at rest.
+ float cx_, cy_;
// timer for atuo-centering
Timer tCenter_;
-
- // recent accelerometer readings, for auto centering
+
+ // Auto-centering history. This is a separate history list that
+ // records results spaced out sparesely over time, so that we can
+ // watch for long-lasting periods of rest. When we observe nearly
+ // no motion for an extended period (on the order of 5 seconds), we
+ // take this to mean that the cabinet is at rest in its neutral
+ // position, so we take this as the calibration zero point for the
+ // accelerometer. We update this history continuously, which allows
+ // us to continuously re-calibrate the accelerometer. This ensures
+ // that we'll automatically adjust to any actual changes in the
+ // cabinet's orientation (e.g., if it gets moved slightly by an
+ // especially strong nudge) as well as any systematic drift in the
+ // accelerometer measurement bias (e.g., from temperature changes).
int iAccPrv_, nAccPrv_;
static const int maxAccPrv = 5;
- FPoint accPrv_[maxAccPrv];
-
+ AccHist accPrv_[maxAccPrv];
+
// interurupt pin name
PinName irqPin_;
@@ -746,12 +854,15 @@
ledG = 1;
ledB = 1;
+ // initialize the LedWiz ports
+ initLwOut();
+
+ // we don't need a reset yet
+ bool needReset = false;
+
// 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;
@@ -761,9 +872,7 @@
NVM cfg;
// check for valid flash
- bool flash_valid = (flash->d.sig == flash->SIGNATURE
- && flash->d.vsn == flash->VERSION
- && flash->checksum == CRC32(&flash->d, sizeof(flash->d)));
+ bool flash_valid = flash->valid();
// 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
@@ -784,17 +893,28 @@
// if the flash is valid, load it; otherwise initialize to defaults
if (flash_valid) {
memcpy(&cfg, flash, sizeof(cfg));
- printf("Flash restored: plunger min=%d, max=%d\r\n",
- cfg.d.plungerMin, cfg.d.plungerMax);
+ printf("Flash restored: plunger cal=%d, min=%d, zero=%d, max=%d\r\n",
+ cfg.d.plungerCal, cfg.d.plungerMin, cfg.d.plungerZero, cfg.d.plungerMax);
}
else {
printf("Factory reset\r\n");
cfg.d.sig = cfg.SIGNATURE;
cfg.d.vsn = cfg.VERSION;
+ cfg.d.plungerCal = 0;
+ cfg.d.plungerZero = 0;
cfg.d.plungerMin = 0;
cfg.d.plungerMax = npix;
+ cfg.d.ledWizUnitNo = DEFAULT_LEDWIZ_UNIT_NUMBER;
+ cfg.d.ccdEnabled = true;
}
+ // Create the joystick USB client. Note that we use the LedWiz unit
+ // number from the saved configuration.
+ MyUSBJoystick js(
+ USB_VENDOR_ID,
+ USB_PRODUCT_ID | cfg.d.ledWizUnitNo,
+ USB_VERSION_NO);
+
// plunger calibration button debounce timer
Timer calBtnTimer;
calBtnTimer.start();
@@ -825,7 +945,19 @@
TSL1410R ccd(PTE20, PTE21, PTB0);
// last accelerometer report, in mouse coordinates
- int x = 127, y = 127, z = 0;
+ int x = 0, y = 0, z = 0;
+
+ // previous two plunger readings, for "debouncing" the results (z0 is
+ // the most recent, z1 is the one before that)
+ int z0 = 0, z1 = 0, z2 = 0;
+
+ // Firing in progress: we set this when we detect the start of rapid
+ // plunger movement from a retracted position towards the rest position.
+ // The actual plunger spring return speed seems to be too slow for VP,
+ // so when we detect the start of this motion, we immediately tell VP
+ // to return the plunger to rest, then we monitor the real plunger
+ // until it atcually stops.
+ bool firing = false;
// start the first CCD integration cycle
ccd.clear();
@@ -839,50 +971,80 @@
// handle input in as timely a fashion as possible by deferring
// output tasks as long as there's input to process.
HID_REPORT report;
- while (js.readNB(&report) && report.length == 8)
+ while (js.readNB(&report))
{
- uint8_t *data = report.data;
- if (data[0] == 64)
+ // all Led-Wiz reports are 8 bytes exactly
+ if (report.length == 8)
{
- // 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",
- // data[1], data[2], data[3], data[4], data[5]);
-
- // update all on/off states
- for (int i = 0, bit = 1, ri = 1 ; i < 32 ; ++i, bit <<= 1)
+ uint8_t *data = report.data;
+ if (data[0] == 64)
{
- if (bit == 0x100) {
- bit = 1;
- ++ri;
- }
- wizOn[i] = ((data[ri] & bit) != 0);
- }
+ // 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",
+ // data[1], data[2], data[3], data[4], data[5]);
- // update the physical outputs
- updateWizOuts();
-
- // reset the PBA counter
- pbaIdx = 0;
- }
- 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.
- //printf("LWZ-PBA[%d] %02x %02x %02x %02x %02x %02x %02x %02x\r\n",
- // pbaIdx, data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7]);
-
- // update all output profile settings
- for (int i = 0 ; i < 8 ; ++i)
- wizVal[pbaIdx + i] = data[i];
-
- // update the physical LED state if this is the last bank
- if (pbaIdx == 24)
+ // update all on/off states
+ for (int i = 0, bit = 1, ri = 1 ; i < 32 ; ++i, bit <<= 1)
+ {
+ if (bit == 0x100) {
+ bit = 1;
+ ++ri;
+ }
+ wizOn[i] = ((data[ri] & bit) != 0);
+ }
+
+ // update the physical outputs
updateWizOuts();
-
- // advance to the next bank
- pbaIdx = (pbaIdx + 8) & 31;
+
+ // reset the PBA counter
+ pbaIdx = 0;
+ }
+ else if (data[0] == 65)
+ {
+ // Private control message. This isn't an LedWiz message - it's
+ // an extension for this device. 65 is an invalid PBA setting,
+ // and isn't used for any other LedWiz message, so we appropriate
+ // it for our own private use. The first byte specifies the
+ // message type.
+ if (data[1] == 1)
+ {
+ // Set Configuration:
+ // data[2] = LedWiz unit number (0x00 to 0x0f)
+ // data[3] = feature enable bit mask:
+ // 0x01 = enable CCD
+
+ // we'll need a reset if the LedWiz unit number is changing
+ uint8_t newUnitNo = data[2] & 0x0f;
+ needReset |= (newUnitNo != cfg.d.ledWizUnitNo);
+
+ // set the configuration parameters from the message
+ cfg.d.ledWizUnitNo = newUnitNo;
+ cfg.d.ccdEnabled = data[3] & 0x01;
+
+ // save the configuration
+ cfg.save(iap, flash_addr);
+ }
+ }
+ 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.
+ //printf("LWZ-PBA[%d] %02x %02x %02x %02x %02x %02x %02x %02x\r\n",
+ // pbaIdx, data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7]);
+
+ // update all output profile settings
+ for (int i = 0 ; i < 8 ; ++i)
+ wizVal[pbaIdx + i] = data[i];
+
+ // update the physical LED state if this is the last bank
+ if (pbaIdx == 24)
+ updateWizOuts();
+
+ // advance to the next bank
+ pbaIdx = (pbaIdx + 8) & 31;
+ }
}
}
@@ -914,8 +1076,10 @@
// enter calibration mode
calBtnState = 3;
- // reset the calibration limits
+ // set extremes for the calibration data, so that the actual
+ // values we read will set new high/low water marks on the fly
cfg.d.plungerMax = 0;
+ cfg.d.plungerZero = npix;
cfg.d.plungerMin = npix;
}
break;
@@ -943,12 +1107,9 @@
// exit calibration mode
calBtnState = 0;
- // Save the current configuration state to flash, so that it
- // will be preserved through power off. Update the checksum
- // first so that we recognize the flash record as valid.
- cfg.checksum = CRC32(&cfg.d, sizeof(cfg.d));
- iap.erase_sector(flash_addr);
- iap.program_flash(flash_addr, &cfg, sizeof(cfg));
+ // save the updated configuration
+ cfg.d.plungerCal = 1;
+ cfg.save(iap, flash_addr);
// the flash state is now valid
flash_valid = true;
@@ -999,105 +1160,297 @@
}
}
- // read the plunger sensor
- int znew = z;
- uint16_t pix[npix];
- ccd.read(pix, npix);
+ // read the plunger sensor, if it's enabled
+ if (cfg.d.ccdEnabled)
+ {
+ // start with the previous reading, in case we don't have a
+ // clear result on this frame
+ int znew = z;
- // get the average brightness at each end of the sensor
- 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; multiply by 3 so that we can
- // compare sums of three pixels at a time to smooth out noise
- long midpt = (avg1 + avg2)/2 * 3;
+ // read the array
+ uint16_t pix[npix];
+ ccd.read(pix, npix);
+
+ // get the average brightness at each end of the sensor
+ 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; multiply by 3 so that we can
+ // compare sums of three pixels at a time to smooth out noise
+ 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: zero is the
+ // rest position, and the axis 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 - 2, di = -1;
+
+ // If the bright end and dark end don't differ by enough, skip this
+ // reading entirely - we must have an overexposed or underexposed frame.
+ // Otherwise proceed with the scan.
+ if (labs(avg1 - avg2) > 0x1000)
+ {
+ uint16_t *pixp = pix + si;
+ for (int n = 1 ; n < npix - 1 ; ++n, pixp += di)
+ {
+ // if we've crossed the midpoint, report this position
+ if (long(pixp[-1]) + long(pixp[0]) + long(pixp[1]) < midpt)
+ {
+ // note the new position
+ int pos = n;
+
+ // Calibrate, or apply calibration, depending on the mode.
+ // In either case, normalize to our range. VP appears to
+ // ignore negative Z axis values.
+ if (calBtnState == 3)
+ {
+ // calibrating - note if we're expanding the calibration envelope
+ if (pos < cfg.d.plungerMin)
+ cfg.d.plungerMin = pos;
+ if (pos < cfg.d.plungerZero)
+ cfg.d.plungerZero = pos;
+ if (pos > cfg.d.plungerMax)
+ cfg.d.plungerMax = pos;
+
+ // normalize to the full physical range while calibrating
+ znew = int(round(float(pos)/npix * JOYMAX));
+ }
+ else
+ {
+ // Running normally - normalize to the calibration range. Note
+ // that values below the zero point are allowed - the zero point
+ // represents the park position, where the plunger sits when at
+ // rest, but a mechanical plunger has a smmall amount of travel
+ // in the "push" direction. We represent forward travel with
+ // negative z values.
+ if (pos > cfg.d.plungerMax)
+ pos = cfg.d.plungerMax;
+ znew = int(round(float(pos - cfg.d.plungerZero)
+ / (cfg.d.plungerMax - cfg.d.plungerZero + 1) * JOYMAX));
+ }
+
+ // done
+ break;
+ }
+ }
+ }
- // 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 - 2, di = -1;
+ // "Debounce" the plunger reading.
+ //
+ // It takes us about 25ms to read the CCD and calculate the new
+ // plunger position. That's not quite fast enough to keep up with
+ // very fast plunger motions. And the single most important motion
+ // the plunger makes - releasing from a retracted position it to
+ // launch the ball - is just such a fast motion. Our scan rate is
+ // fast enough to capture one or two intermediate frames in a release
+ // motion, but it's not nearly fast enough to get a clean reading on
+ // the instantaneous speed, let alone accelerations.
+ //
+ // Fortunately, we don't need to take speed readings at all. VP has
+ // its own internal simulated plunger model, which it uses to calculate
+ // the speed and force of the plunger movement. Our readings tell VP
+ // where the plunger should be at any given moment, and VP makes its
+ // model move in that direction, using the model parameters for speed
+ // and acceleration. So whatever speed we see physically is irrelevant;
+ // the VP model plunger can only move at the speed set in its model.
+ //
+ // This works out great for our relatively slow scan rate. We don't
+ // have to take readings quickly enough to get instantaneous velocities;
+ // we just need to know where the plunger is once in a while so that
+ // VP can move its model plunger in the right direction for the right
+ // distance, and VP figures out the appropriate speed for the required
+ // travel.
+ //
+ // But there is one complication. We do scan fast enough to see *some*
+ // intermediate positions during a fast motion. Suppose that on one
+ // scan, the plunger is fully retracted. Now suppose that the player
+ // releases the plunger just after that scan, such that our next scan
+ // catches the plunger *almost* back to the rest position, but not
+ // quite - just a hair short. If we send these two consecutive reports
+ // to VP, VP will set its model plunger in motion with the *almost*
+ // reading as the destination. VP will step its physics model with
+ // this new plunger destination until we send another reading.
+ // Ddpending on how the timing of our next scan works out, it's
+ // possible that the model plunger will have reached or almost reached
+ // the destination by the time we send our next report - so VP might
+ // be decelerating or stopping the model plunger as it approaches
+ // this position. Our next scan will probably find the plunger back
+ // at the rest position, so we'll tell VP to continue moving the
+ // plunger to the zero spot. The problem that just happened is that
+ // our intermediate *almost there* report might have robbed the
+ // motion in the model of some energy that should have been there,
+ // by causing it to decelerate briefly near the intermediate position.
+ //
+ // This is relatively easy to fix. Because VP does all of the fast
+ // motion modeling on its own anyway, there's no advantage to sending
+ // VP intermediate positions during rapid motions - and there's the
+ // disadvantage we just described. So all we need to do is skip
+ // reports while the plunger is moving rapidly - we just need to wait
+ // for it to settle at a new position before sending an update.
+ //
+ // So: only report the latest reading if it's relatively close to the
+ // previous reading, indicating we're moving slowly or at rest. One
+ // exception: if we see a reversal of direction, report the previous
+ // reading, which is the peak in the previous direction. This will
+ // catch cases where the player is moving the plunger very rapidly
+ // back and forth, as well as release motions where the plunger
+ // briefly overshoots the rest position.
+#if 1
+ // Check to see if plunger firing is in progress. If not, check
+ // to see if it looks like we just started firing.
+ const int restTol = JOYMAX/npix * 4;
+ const int fireTol = JOYMAX/npix * 12;
+ if (firing)
+ {
+ // Firing in progress - we've already told VP to send its
+ // model plunger all the way back to the rest position, so
+ // send no further reports until the mechanical plunger
+ // actually comes to rest somewhere.
+ if (abs(z0 - z2) < restTol && abs(znew - z2) < restTol)
+ {
+ // the plunger is back at rest - firing is done
+ firing = false;
+
+ // resume normal reporting
+ z = z2;
+ }
+ }
+ else if (z0 < z2 && z1 < z2 && znew < z2
+ && (z0 < z2 - fireTol
+ || z1 < z2 - fireTol
+ || znew < z2 - fireTol))
+ {
+ // Big jumps toward rest position in last two readings -
+ // firing has begun. Report an immediate return to the
+ // rest position, and send no further reports until the
+ // physical plunger has come to rest. This effectively
+ // detaches VP's model plunger from the real world for
+ // the duration of the spring return, letting VP evolve
+ // its model without trying to synchronize with the
+ // mechanical version. The release motion is too fast
+ // for that to work well; we can't take samples quickly
+ // enough to get prcise velocity or acceleration
+ // readings. It's better to let VP figure the speed
+ // and acceleration through modeling. Plus, that lets
+ // each virtual table set the desired parameters for its
+ // virtual plunger, rather than imposing the actual
+ // mechanical charateristics of the physical plunger on
+ // every table.
+ firing = true;
+ z = 0;
+ }
+ else
+ {
+ // everything normal; report the 3rd recent position on
+ // tape delay
+ z = z2;
+ }
+
+ // shift in the new reading
+ z2 = z1;
+ z1 = z0;
+ z0 = znew;
+#endif
- // scan for the midpoint
- uint16_t *pixp = pix + si;
- for (int n = 1 ; n < npix - 1 ; ++n, pixp += di)
- {
- // if we've crossed the midpoint, report this position
- if (long(pixp[-1]) + long(pixp[0]) + long(pixp[1]) < midpt)
+
+#if 0
+ // check for the anomalous fast return case, where we get two
+ // descending readings out of order
+ if (znew < z1
+ && z0 < z1
+ && znew > z0
+ && abs(znew - z1) > JOYMAX/npix*3
+ && abs(z0 - z1) > JOYMAX/npix*3)
{
- // note the new position
- int pos = n;
+ // drop the middle reading - report nothing this round
+ z0 = znew;
+ }
+ else
+ {
+ // report the previous reading
+ z = z0;
- // if the bright end and dark end don't differ by enough, skip this
- // reading entirely - we must have an overexposed or underexposed frame
- if (labs(avg1 - avg2) < 0x3333)
- break;
+ // shift in the new reading
+ z1 = z0;
+ z0 = znew;
+ }
+#endif
+#if 0
+ static int insertion = -1;
+ static int insertionList[] = { 0, 400, 800, 1200, 1600, 2000, 2400, 2800, 3200 };
+ static int overcnt = 0;
+ if (insertion >= 0)
+ z = insertionList[insertion--];
+ else if (znew > 3500 && z == 0)
+ z = 3500, overcnt = 1;
+ else if (znew > 3500)
+ ++overcnt;
+ else if (znew < 3500 && overcnt > 3)
+ insertion = sizeof(insertionList)/sizeof(insertionList[0]) - 1, z = 3500, overcnt = 0;
+ else
+ overcnt = 0, z = 0;
+#endif
+#if 0
+ if (znew != z) printf("%d\r\n", znew);
+ z = znew;
+#endif
+#if 0
+ // average the last three readings
+ z = int(round(0.0f + znew + z0 + z1)/3.0f);
+
+ // shift in the new reading
+ z1 = z0;
+ z0 = znew;
+#endif
+#if 0
+ const int zTol = JOYMAX/npix*5;
+ if (abs(znew - z0) < zTol && abs(z0 - z1) < zTol)
+ {
+ // slow or at rest - report the current reading
+ z = znew;
+ }
+ else if ((z0 < z1 && znew > z0) || (z0 > z1 && znew < z0))
+ {
+ // direction reveersal - report the peak reading
+ z = z0;
+ }
- // 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 < cfg.d.plungerMin)
- cfg.d.plungerMin = pos;
- if (pos > cfg.d.plungerMax)
- cfg.d.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 < cfg.d.plungerMin)
- pos = cfg.d.plungerMin;
- if (pos > cfg.d.plungerMax)
- pos = cfg.d.plungerMax;
- znew = int(float(pos - cfg.d.plungerMin)
- / (cfg.d.plungerMax - cfg.d.plungerMin + 1) * 127);
- }
-
- // done
- break;
- }
+ // in any case, remember this new reading, whether reporting it or not
+ z1 = z0;
+ z0 = znew;
+#endif
}
-
+
// read the accelerometer
- float xa, ya, rxa, rya;
+ int xa, ya, rxa, rya;
accel.get(xa, ya, rxa, rya);
- // 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;
-
- // 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;
+ // confine the results to our joystick axis range
+ if (xa < -JOYMAX) xa = -JOYMAX;
+ if (xa > JOYMAX) xa = JOYMAX;
+ if (ya < -JOYMAX) ya = -JOYMAX;
+ if (ya > JOYMAX) ya = JOYMAX;
- // 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.
+ // store the updated accelerometer coordinates
+ x = xa;
+ y = ya;
+
+ // Send the status report.
//
// $$$ 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);
+ uint16_t btns = hb ? 0x5500 : 0xAA00;
+ js.update(x, y, z, rxa, rya, btns);
- // show a heartbeat flash in blue every so often if not in
- // calibration mode
+#ifdef DEBUG_PRINTF
+ if (x != 0 || y != 0)
+ printf("%d,%d\r\n", x, y);
+#endif
+
+ // provide a visual status indication on the on-board LED
if (calBtnState < 2 && hbTimer.read_ms() > 1000)
{
if (js.isSuspended() || !js.isConnected())
@@ -1110,7 +1463,7 @@
// show a status flash every so often
if (hbcnt % 3 == 0)
{
- // disconnected = red flash; suspended = red-red
+ // disconnected = red/red flash; suspended = red
for (int n = js.isConnected() ? 1 : 2 ; n > 0 ; --n)
{
ledR = 0;
@@ -1120,22 +1473,31 @@
}
}
}
- else if (flash_valid)
+ else if (needReset)
{
- // connected, NVM valid - flash blue/green
+ // connected, need to reset due to changes in config parameters -
+ // flash red/green
+ hb = !hb;
+ ledR = (hb ? 0 : 1);
+ ledG = (hb ? 1 : 0);
+ ledB = 0;
+ }
+ else if (cfg.d.ccdEnabled && !cfg.d.plungerCal)
+ {
+ // connected, plunger calibration needed - flash yellow/green
+ hb = !hb;
+ ledR = (hb ? 0 : 1);
+ ledG = 0;
+ ledB = 1;
+ }
+ else
+ {
+ // connected - flash blue/green
hb = !hb;
ledR = 1;
ledG = (hb ? 0 : 1);
ledB = (hb ? 1 : 0);
}
- else
- {
- // connected, factory reset - flash yellow/green
- hb = !hb;
- //ledR = (hb ? 0 : 1);
- //ledG = 0;
- ledB = 1;
- }
// reset the heartbeat timer
hbTimer.reset();