Mike R / Mbed 2 deprecated Pinscape_Controller_v1

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

main.cpp@5:a70c0bce770d, 2014-07-27 (annotated)

Committer:
mjr
Date:
Sun Jul 27 18:24:51 2014 +0000
Revision:
5:a70c0bce770d
Parent:
4:02c7cd7b2183
Child:
6:cc35eb643e8f
Somewhat working with ball-model damping. About to change to cabinet model.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mjr 5:a70c0bce770d 1 /* Copyright 2014 M J Roberts, MIT License
mjr 5:a70c0bce770d 2 *
mjr 5:a70c0bce770d 3 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
mjr 5:a70c0bce770d 4 * and associated documentation files (the "Software"), to deal in the Software without
mjr 5:a70c0bce770d 5 * restriction, including without limitation the rights to use, copy, modify, merge, publish,
mjr 5:a70c0bce770d 6 * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
mjr 5:a70c0bce770d 7 * Software is furnished to do so, subject to the following conditions:
mjr 5:a70c0bce770d 8 *
mjr 5:a70c0bce770d 9 * The above copyright notice and this permission notice shall be included in all copies or
mjr 5:a70c0bce770d 10 * substantial portions of the Software.
mjr 5:a70c0bce770d 11 *
mjr 5:a70c0bce770d 12 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
mjr 5:a70c0bce770d 13 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
mjr 5:a70c0bce770d 14 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
mjr 5:a70c0bce770d 15 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
mjr 5:a70c0bce770d 16 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
mjr 5:a70c0bce770d 17 */
mjr 5:a70c0bce770d 18
mjr 5:a70c0bce770d 19 //
mjr 5:a70c0bce770d 20 // Pinscape Controller
mjr 5:a70c0bce770d 21 //
mjr 5:a70c0bce770d 22 // "Pinscape" is the name of my custom-built virtual pinball cabinet. I wrote this
mjr 5:a70c0bce770d 23 // software to perform a number of tasks that I needed for my cabinet. It runs on a
mjr 5:a70c0bce770d 24 // Freescale KL25Z microcontroller, which is a small and inexpensive device that
mjr 5:a70c0bce770d 25 // attaches to the host PC via USB and can interface with numerous types of external
mjr 5:a70c0bce770d 26 // hardware.
mjr 5:a70c0bce770d 27 //
mjr 5:a70c0bce770d 28 // I designed the software and hardware in this project especially for Pinscape, but
mjr 5:a70c0bce770d 29 // it uses standard interfaces in Windows and Visual Pinball, so it should be
mjr 5:a70c0bce770d 30 // readily usable in anyone else's VP-based cabinet. I've tried to document the
mjr 5:a70c0bce770d 31 // hardware in enough detail for anyone else to duplicate the entire project, and
mjr 5:a70c0bce770d 32 // the full software is open source.
mjr 5:a70c0bce770d 33 //
mjr 5:a70c0bce770d 34 // The controller provides the following functions. It should be possible to use
mjr 5:a70c0bce770d 35 // any subet of the features without using all of them. External hardware for any
mjr 5:a70c0bce770d 36 // particular function can simply be omitted if that feature isn't needed.
mjr 5:a70c0bce770d 37 //
mjr 5:a70c0bce770d 38 // - Nudge sensing via the KL25Z's on-board accelerometer. Nudge accelerations are
mjr 5:a70c0bce770d 39 // processed into a physics model of a rolling ball, and changes to the ball's
mjr 5:a70c0bce770d 40 // motion are sent to the host computer via the joystick interface. This is designed
mjr 5:a70c0bce770d 41 // especially to work with Visuall Pinball's nudge handling to produce realistic
mjr 5:a70c0bce770d 42 // on-screen results in VP. By doing some physics modeling right on the device,
mjr 5:a70c0bce770d 43 // rather than sending raw accelerometer data to VP, we can produce better results
mjr 5:a70c0bce770d 44 // using our awareness of the real physical parameters of a pinball cabinet.
mjr 5:a70c0bce770d 45 // VP's nudge handling has to be more generic, so it can't make the same sorts
mjr 5:a70c0bce770d 46 // of assumptions that we can about the dynamics of a real cabinet.
mjr 5:a70c0bce770d 47 //
mjr 5:a70c0bce770d 48 // The nudge data reports are compatible with the built-in Windows USB joystick
mjr 5:a70c0bce770d 49 // drivers and with VP's own joystick input scheme, so the nudge sensing is almost
mjr 5:a70c0bce770d 50 // plug-and-play. There are no Windiows drivers to install, and the only VP work
mjr 5:a70c0bce770d 51 // needed is to customize a few global preference settings.
mjr 5:a70c0bce770d 52 //
mjr 5:a70c0bce770d 53 // - Plunger position sensing via an attached TAOS TSL 1410R CCD linear array sensor.
mjr 5:a70c0bce770d 54 // The sensor must be wired to a particular set of I/O ports on the KL25Z, and must
mjr 5:a70c0bce770d 55 // be positioned adjacent to the plunger with proper lighting. The physical and
mjr 5:a70c0bce770d 56 // electronic installation details are desribed in the project documentation. We read
mjr 5:a70c0bce770d 57 // the CCD to determine how far back the plunger is pulled, and report this to Visual
mjr 5:a70c0bce770d 58 // Pinball via the joystick interface. As with the nudge data, this is all nearly
mjr 5:a70c0bce770d 59 // plug-and-play, in that it works with the default Windows USB drivers and works
mjr 5:a70c0bce770d 60 // with the existing VP handling for analog plunger input. A few VP settings are
mjr 5:a70c0bce770d 61 // needed to tell VP to allow the plunger.
mjr 5:a70c0bce770d 62 //
mjr 5:a70c0bce770d 63 // Unfortunately, analog plungers are not well supported by individual tables,
mjr 5:a70c0bce770d 64 // so some work is required for each table to give it proper support. I've tried
mjr 5:a70c0bce770d 65 // to reduce this to a recipe and document it in the project documentation.
mjr 5:a70c0bce770d 66 //
mjr 5:a70c0bce770d 67 // - In addition to the CCD sensor, a button should be attached (also described in
mjr 5:a70c0bce770d 68 // the project documentation) to activate calibration mode for the plunger. When
mjr 5:a70c0bce770d 69 // calibration mode is activated, the software reads the plunger position for about
mjr 5:a70c0bce770d 70 // 10 seconds when to note the limits of travel, and uses these limits to ensure
mjr 5:a70c0bce770d 71 // accurate reports to VP that properly report the actual position of the physical
mjr 5:a70c0bce770d 72 // plunger. The calibration is stored in non-volatile memory on the KL25Z, so it's
mjr 5:a70c0bce770d 73 // only necessary to calibrate once - the calibration will survive power cycling
mjr 5:a70c0bce770d 74 // and reboots of the PC. It's only necessary to recalibrate if the CCD sensor or
mjr 5:a70c0bce770d 75 // the plunger are removed and reinstalled, since the relative alignment of the
mjr 5:a70c0bce770d 76 // parts could cahnge slightly when reinstalling.
mjr 5:a70c0bce770d 77 //
mjr 5:a70c0bce770d 78 // - LedWiz emulation. The KL25Z can appear to the PC as an LedWiz device, and will
mjr 5:a70c0bce770d 79 // accept and process LedWiz commands from the host. The software can turn digital
mjr 5:a70c0bce770d 80 // output ports on and off, and can set varying PWM intensitiy levels on a subset
mjr 5:a70c0bce770d 81 // of ports. (The KL25Z can only provide 6 PWM ports. Intensity level settings on
mjr 5:a70c0bce770d 82 // other ports is ignored, so non-PWM ports can only be used for simple on/off
mjr 5:a70c0bce770d 83 // devices such as contactors and solenoids.) The KL25Z can only supply 4mA on its
mjr 5:a70c0bce770d 84 // output ports, so external hardware is required to take advantage of the LedWiz
mjr 5:a70c0bce770d 85 // emulation. Many different hardware designs are possible, but there's a simple
mjr 5:a70c0bce770d 86 // reference design in the documentation that uses a Darlington array IC to
mjr 5:a70c0bce770d 87 // increase the output from each port to 500mA (the same level as the LedWiz),
mjr 5:a70c0bce770d 88 // plus an extended design that adds an optocoupler and MOSFET to provide very
mjr 5:a70c0bce770d 89 // high power handling, up to about 45A or 150W, with voltages up to 100V.
mjr 5:a70c0bce770d 90 // That will handle just about any DC device directly (wtihout relays or other
mjr 5:a70c0bce770d 91 // amplifiers), and switches fast enough to support PWM devices.
mjr 5:a70c0bce770d 92 //
mjr 5:a70c0bce770d 93 // The device can report any desired LedWiz unit number to the host, which makes
mjr 5:a70c0bce770d 94 // it possible to use the LedWiz emulation on a machine that also has one or more
mjr 5:a70c0bce770d 95 // actual LedWiz devices intalled. The LedWiz design allows for up to 16 units
mjr 5:a70c0bce770d 96 // to be installed in one machine - each one is invidually addressable by its
mjr 5:a70c0bce770d 97 // distinct unit number.
mjr 5:a70c0bce770d 98 //
mjr 5:a70c0bce770d 99 // The LedWiz emulation features are of course optional. There's no need to
mjr 5:a70c0bce770d 100 // build any of the external port hardware (or attach anything to the output
mjr 5:a70c0bce770d 101 // ports at all) if the LedWiz features aren't needed. Most people won't have
mjr 5:a70c0bce770d 102 // any use for the LedWiz features. I built them mostly as a learning exercise,
mjr 5:a70c0bce770d 103 // but with a slight practical need for a handful of extra ports (I'm using the
mjr 5:a70c0bce770d 104 // cutting-edge 10-contactor setup, so my real LedWiz is full!).
mjr 5:a70c0bce770d 105
mjr 5:a70c0bce770d 106
mjr 0:5acbbe3f4cf4 107 #include "mbed.h"
mjr 0:5acbbe3f4cf4 108 #include "USBJoystick.h"
mjr 0:5acbbe3f4cf4 109 #include "MMA8451Q.h"
mjr 1:d913e0afb2ac 110 #include "tsl1410r.h"
mjr 1:d913e0afb2ac 111 #include "FreescaleIAP.h"
mjr 2:c174f9ee414a 112 #include "crc32.h"
mjr 2:c174f9ee414a 113
mjr 5:a70c0bce770d 114
mjr 5:a70c0bce770d 115 // ---------------------------------------------------------------------------
mjr 5:a70c0bce770d 116 //
mjr 5:a70c0bce770d 117 // Configuration details
mjr 5:a70c0bce770d 118 //
mjr 2:c174f9ee414a 119
mjr 5:a70c0bce770d 120 // Our USB device vendor ID, product ID, and version.
mjr 5:a70c0bce770d 121 // We use the vendor ID for the LedWiz, so that the PC-side software can
mjr 5:a70c0bce770d 122 // identify us as capable of performing LedWiz commands. The LedWiz uses
mjr 5:a70c0bce770d 123 // a product ID value from 0xF0 to 0xFF; the last four bits identify the
mjr 5:a70c0bce770d 124 // unit number (e.g., product ID 0xF7 means unit #7). This allows multiple
mjr 5:a70c0bce770d 125 // LedWiz units to be installed in a single PC; the software on the PC side
mjr 5:a70c0bce770d 126 // uses the unit number to route commands to the devices attached to each
mjr 5:a70c0bce770d 127 // unit. On the real LedWiz, the unit number must be set in the firmware
mjr 5:a70c0bce770d 128 // at the factory; it's not configurable by the end user. Most LedWiz's
mjr 5:a70c0bce770d 129 // ship with the unit number set to 0, but the vendor will set different
mjr 5:a70c0bce770d 130 // unit numbers if requested at the time of purchase. So if you have a
mjr 5:a70c0bce770d 131 // single LedWiz already installed in your cabinet, and you didn't ask for
mjr 5:a70c0bce770d 132 // a non-default unit number, your existing LedWiz will be unit 0.
mjr 5:a70c0bce770d 133 //
mjr 5:a70c0bce770d 134 // We use unit #7 by default. There doesn't seem to be a requirement that
mjr 5:a70c0bce770d 135 // unit numbers be contiguous (DirectOutput Framework and other software
mjr 5:a70c0bce770d 136 // seem happy to have units 0 and 7 installed, without 1-6 existing).
mjr 5:a70c0bce770d 137 // Marking this unit as #7 should work for almost everybody out of the box;
mjr 5:a70c0bce770d 138 // the most common case seems to be to have a single LedWiz installed, and
mjr 5:a70c0bce770d 139 // it's probably extremely rare to more than two.
mjr 5:a70c0bce770d 140 const uint16_t USB_VENDOR_ID = 0xFAFA;
mjr 5:a70c0bce770d 141 const uint16_t USB_PRODUCT_ID = 0x00F7;
mjr 5:a70c0bce770d 142 const uint16_t USB_VERSION_NO = 0x0004;
mjr 0:5acbbe3f4cf4 143
mjr 4:02c7cd7b2183 144 // On-board RGB LED elements - we use these for diagnostic displays.
mjr 4:02c7cd7b2183 145 DigitalOut ledR(LED1), ledG(LED2), ledB(LED3);
mjr 0:5acbbe3f4cf4 146
mjr 1:d913e0afb2ac 147 // calibration button - switch input and LED output
mjr 1:d913e0afb2ac 148 DigitalIn calBtn(PTE29);
mjr 1:d913e0afb2ac 149 DigitalOut calBtnLed(PTE23);
mjr 0:5acbbe3f4cf4 150
mjr 5:a70c0bce770d 151 // I2C address of the accelerometer (this is a constant of the KL25Z)
mjr 5:a70c0bce770d 152 const int MMA8451_I2C_ADDRESS = (0x1d<<1);
mjr 5:a70c0bce770d 153
mjr 5:a70c0bce770d 154 // SCL and SDA pins for the accelerometer (constant for the KL25Z)
mjr 5:a70c0bce770d 155 #define MMA8451_SCL_PIN PTE25
mjr 5:a70c0bce770d 156 #define MMA8451_SDA_PIN PTE24
mjr 5:a70c0bce770d 157
mjr 5:a70c0bce770d 158 // Digital in pin to use for the accelerometer interrupt. For the KL25Z,
mjr 5:a70c0bce770d 159 // this can be either PTA14 or PTA15, since those are the pins physically
mjr 5:a70c0bce770d 160 // wired on this board to the MMA8451 interrupt controller.
mjr 5:a70c0bce770d 161 #define MMA8451_INT_PIN PTA15
mjr 5:a70c0bce770d 162
mjr 5:a70c0bce770d 163
mjr 5:a70c0bce770d 164 // ---------------------------------------------------------------------------
mjr 5:a70c0bce770d 165 //
mjr 5:a70c0bce770d 166 // LedWiz emulation
mjr 5:a70c0bce770d 167 //
mjr 5:a70c0bce770d 168
mjr 0:5acbbe3f4cf4 169 static int pbaIdx = 0;
mjr 0:5acbbe3f4cf4 170
mjr 0:5acbbe3f4cf4 171 // on/off state for each LedWiz output
mjr 1:d913e0afb2ac 172 static uint8_t wizOn[32];
mjr 0:5acbbe3f4cf4 173
mjr 0:5acbbe3f4cf4 174 // profile (brightness/blink) state for each LedWiz output
mjr 1:d913e0afb2ac 175 static uint8_t wizVal[32] = {
mjr 0:5acbbe3f4cf4 176 0, 0, 0, 0, 0, 0, 0, 0,
mjr 0:5acbbe3f4cf4 177 0, 0, 0, 0, 0, 0, 0, 0,
mjr 0:5acbbe3f4cf4 178 0, 0, 0, 0, 0, 0, 0, 0,
mjr 0:5acbbe3f4cf4 179 0, 0, 0, 0, 0, 0, 0, 0
mjr 0:5acbbe3f4cf4 180 };
mjr 0:5acbbe3f4cf4 181
mjr 1:d913e0afb2ac 182 static float wizState(int idx)
mjr 0:5acbbe3f4cf4 183 {
mjr 1:d913e0afb2ac 184 if (wizOn[idx]) {
mjr 0:5acbbe3f4cf4 185 // on - map profile brightness state to PWM level
mjr 1:d913e0afb2ac 186 uint8_t val = wizVal[idx];
mjr 0:5acbbe3f4cf4 187 if (val >= 1 && val <= 48)
mjr 0:5acbbe3f4cf4 188 return 1.0 - val/48.0;
mjr 0:5acbbe3f4cf4 189 else if (val >= 129 && val <= 132)
mjr 0:5acbbe3f4cf4 190 return 0.0;
mjr 0:5acbbe3f4cf4 191 else
mjr 0:5acbbe3f4cf4 192 return 1.0;
mjr 0:5acbbe3f4cf4 193 }
mjr 0:5acbbe3f4cf4 194 else {
mjr 0:5acbbe3f4cf4 195 // off
mjr 0:5acbbe3f4cf4 196 return 1.0;
mjr 0:5acbbe3f4cf4 197 }
mjr 0:5acbbe3f4cf4 198 }
mjr 0:5acbbe3f4cf4 199
mjr 1:d913e0afb2ac 200 static void updateWizOuts()
mjr 1:d913e0afb2ac 201 {
mjr 4:02c7cd7b2183 202 ledR = wizState(0);
mjr 4:02c7cd7b2183 203 ledG = wizState(1);
mjr 4:02c7cd7b2183 204 ledB = wizState(2);
mjr 1:d913e0afb2ac 205 }
mjr 1:d913e0afb2ac 206
mjr 5:a70c0bce770d 207 // ---------------------------------------------------------------------------
mjr 5:a70c0bce770d 208 //
mjr 5:a70c0bce770d 209 // Non-volatile memory (NVM)
mjr 5:a70c0bce770d 210 //
mjr 0:5acbbe3f4cf4 211
mjr 5:a70c0bce770d 212 // Structure defining our NVM storage layout. We store a small
mjr 2:c174f9ee414a 213 // amount of persistent data in flash memory to retain calibration
mjr 5:a70c0bce770d 214 // data when powered off.
mjr 2:c174f9ee414a 215 struct NVM
mjr 2:c174f9ee414a 216 {
mjr 2:c174f9ee414a 217 // checksum - we use this to determine if the flash record
mjr 2:c174f9ee414a 218 // has been initialized
mjr 2:c174f9ee414a 219 uint32_t checksum;
mjr 2:c174f9ee414a 220
mjr 2:c174f9ee414a 221 // signature value
mjr 2:c174f9ee414a 222 static const uint32_t SIGNATURE = 0x4D4A522A;
mjr 2:c174f9ee414a 223 static const uint16_t VERSION = 0x0002;
mjr 2:c174f9ee414a 224
mjr 2:c174f9ee414a 225 // stored data (excluding the checksum)
mjr 2:c174f9ee414a 226 struct
mjr 2:c174f9ee414a 227 {
mjr 2:c174f9ee414a 228 // signature and version - further verification that we have valid
mjr 2:c174f9ee414a 229 // initialized data
mjr 2:c174f9ee414a 230 uint32_t sig;
mjr 2:c174f9ee414a 231 uint16_t vsn;
mjr 2:c174f9ee414a 232
mjr 2:c174f9ee414a 233 // direction - 0 means unknown, 1 means bright end is pixel 0, 2 means reversed
mjr 2:c174f9ee414a 234 uint8_t dir;
mjr 2:c174f9ee414a 235
mjr 2:c174f9ee414a 236 // plunger calibration min and max
mjr 2:c174f9ee414a 237 int plungerMin;
mjr 2:c174f9ee414a 238 int plungerMax;
mjr 2:c174f9ee414a 239 } d;
mjr 2:c174f9ee414a 240 };
mjr 2:c174f9ee414a 241
mjr 5:a70c0bce770d 242
mjr 5:a70c0bce770d 243 // ---------------------------------------------------------------------------
mjr 5:a70c0bce770d 244 //
mjr 5:a70c0bce770d 245 // Customization joystick subbclass
mjr 5:a70c0bce770d 246 //
mjr 5:a70c0bce770d 247
mjr 5:a70c0bce770d 248 class MyUSBJoystick: public USBJoystick
mjr 5:a70c0bce770d 249 {
mjr 5:a70c0bce770d 250 public:
mjr 5:a70c0bce770d 251 MyUSBJoystick(uint16_t vendor_id, uint16_t product_id, uint16_t product_release)
mjr 5:a70c0bce770d 252 : USBJoystick(vendor_id, product_id, product_release, true)
mjr 5:a70c0bce770d 253 {
mjr 5:a70c0bce770d 254 suspended_ = false;
mjr 5:a70c0bce770d 255 }
mjr 5:a70c0bce770d 256
mjr 5:a70c0bce770d 257 // are we connected?
mjr 5:a70c0bce770d 258 int isConnected() { return configured(); }
mjr 5:a70c0bce770d 259
mjr 5:a70c0bce770d 260 // Are we in suspend mode?
mjr 5:a70c0bce770d 261 int isSuspended() const { return suspended_; }
mjr 5:a70c0bce770d 262
mjr 5:a70c0bce770d 263 protected:
mjr 5:a70c0bce770d 264 virtual void suspendStateChanged(unsigned int suspended)
mjr 5:a70c0bce770d 265 { suspended_ = suspended; }
mjr 5:a70c0bce770d 266
mjr 5:a70c0bce770d 267 // are we suspended?
mjr 5:a70c0bce770d 268 int suspended_;
mjr 5:a70c0bce770d 269 };
mjr 5:a70c0bce770d 270
mjr 5:a70c0bce770d 271 // ---------------------------------------------------------------------------
mjr 5:a70c0bce770d 272 //
mjr 5:a70c0bce770d 273 // Accelerometer (MMA8451Q)
mjr 5:a70c0bce770d 274 //
mjr 5:a70c0bce770d 275
mjr 5:a70c0bce770d 276 // The MMA8451Q is the KL25Z's on-board 3-axis accelerometer.
mjr 5:a70c0bce770d 277 //
mjr 5:a70c0bce770d 278 // This is a custom wrapper for the library code to interface to the
mjr 5:a70c0bce770d 279 // MMA8451Q. This class encapsulates an interrupt handler and some
mjr 5:a70c0bce770d 280 // special data processing to produce more realistic results in
mjr 5:a70c0bce770d 281 // Visual Pinball.
mjr 5:a70c0bce770d 282 //
mjr 5:a70c0bce770d 283 // We install an interrupt handler on the accelerometer "data ready"
mjr 5:a70c0bce770d 284 // interrupt in order to ensure that we fetch each sample immediately
mjr 5:a70c0bce770d 285 // when it becomes available. Since our main program loop is busy
mjr 5:a70c0bce770d 286 // reading the CCD virtually all of the time, it wouldn't be practical
mjr 5:a70c0bce770d 287 // to keep up with the accelerometer data stream by polling.
mjr 5:a70c0bce770d 288 //
mjr 5:a70c0bce770d 289 // Visual Pinball is nominally designed to accept raw accelerometer
mjr 5:a70c0bce770d 290 // data as nudge input, but in practice, this doesn't produce
mjr 5:a70c0bce770d 291 // very realistic results. VP simply applies accelerations from a
mjr 5:a70c0bce770d 292 // physical accelerometer directly to its modeled ball(s), but the
mjr 5:a70c0bce770d 293 // data stream coming from a real accelerometer isn't as clean as
mjr 5:a70c0bce770d 294 // an idealized physics simulation. The problem seems to be that the
mjr 5:a70c0bce770d 295 // accelerometer samples capture instantaneous accelerations, not
mjr 5:a70c0bce770d 296 // integrated acceleration over time. In other words, adding samples
mjr 5:a70c0bce770d 297 // over time doesn't accurately reflect the actual net acceleration
mjr 5:a70c0bce770d 298 // experienced. The longer the sampling period, the greater the
mjr 5:a70c0bce770d 299 // divergence between the sum of a series of samples and the actual
mjr 5:a70c0bce770d 300 // net acceleration. The effect in VP is to leave the ball with
mjr 5:a70c0bce770d 301 // an unrealistically high residual velocity over the course of a
mjr 5:a70c0bce770d 302 // nudge event.
mjr 5:a70c0bce770d 303 //
mjr 5:a70c0bce770d 304 // This is where our custom data processing comes into play. Rather
mjr 5:a70c0bce770d 305 // than sending raw accelerometer samples, we apply the samples to
mjr 5:a70c0bce770d 306 // our own virtual model ball. What we send VP is the accelerations
mjr 5:a70c0bce770d 307 // experienced by the ball in our model, not the actual accelerations
mjr 5:a70c0bce770d 308 // we read from the MMA8451Q. Now, that might seem like an unnecessary
mjr 5:a70c0bce770d 309 // middleman, because VP is just going to apply the accelerations to
mjr 5:a70c0bce770d 310 // its own model ball. But it's a useful middleman: what we can do
mjr 5:a70c0bce770d 311 // in our model that VP can't do in its model is take into account
mjr 5:a70c0bce770d 312 // our special knowledge of the physical cabinet configuration. VP
mjr 5:a70c0bce770d 313 // has to work generically with any sort of nudge input device, but
mjr 5:a70c0bce770d 314 // we can make assumptions about what kind of physical environment
mjr 5:a70c0bce770d 315 // we're operating in.
mjr 5:a70c0bce770d 316 //
mjr 5:a70c0bce770d 317 // The key assumption we make about our physical environment is that
mjr 5:a70c0bce770d 318 // accelerations from nudges should net out to zero over intervals on
mjr 5:a70c0bce770d 319 // the order of a couple of seconds. Nudging a pinball cabinet makes
mjr 5:a70c0bce770d 320 // the cabinet accelerate briefly in the nudge direction, then rebound,
mjr 5:a70c0bce770d 321 // then re-rebound, and so on until the swaying motion damps out and
mjr 5:a70c0bce770d 322 // the table returns roughly to rest. The table doesn't actually go
mjr 5:a70c0bce770d 323 // anywhere in these transactions, so the net acceleration experienced
mjr 5:a70c0bce770d 324 // is zero by the time the motion has damped out. The damping time
mjr 5:a70c0bce770d 325 // depends on the degree of force of the nudge, but is a second or
mjr 5:a70c0bce770d 326 // two in most cases.
mjr 5:a70c0bce770d 327 //
mjr 5:a70c0bce770d 328 // We can't just assume that all motion and/or acceleration must stop
mjr 5:a70c0bce770d 329 // in a second or two, though. For one thing, the player can nudge
mjr 5:a70c0bce770d 330 // the table repeatedly for long periods. (Doing this too aggressivly
mjr 5:a70c0bce770d 331 // will trigger a tilt, so there are limits, but a skillful player
mjr 5:a70c0bce770d 332 // can keep nudging a table almost continuously without tilting it.)
mjr 5:a70c0bce770d 333 // For another, a player could actually pick up one end of the table
mjr 5:a70c0bce770d 334 // for an extended period, applying a continuous acceleration the
mjr 5:a70c0bce770d 335 // whole time.
mjr 5:a70c0bce770d 336 //
mjr 5:a70c0bce770d 337 // The strategy we use to cope with these possibilities is to model a
mjr 5:a70c0bce770d 338 // ball, rather like VP does, but with damping that scales with the
mjr 5:a70c0bce770d 339 // current speed. We'll choose a damping function that will bring
mjr 5:a70c0bce770d 340 // the ball to rest from any reasonable speed within a second or two
mjr 5:a70c0bce770d 341 // if there are no ongoing accelerations. The damping function must
mjr 5:a70c0bce770d 342 // also be weak enough that new accelerations dominate - that is,
mjr 5:a70c0bce770d 343 // the damping function must not be so strong that it cancels out
mjr 5:a70c0bce770d 344 // ongoing physical acceleration input, such as when the player
mjr 5:a70c0bce770d 345 // lifts one end of the table and holds it up for a while.
mjr 5:a70c0bce770d 346 //
mjr 5:a70c0bce770d 347 // What we report to VP is the acceleration experienced by our model
mjr 5:a70c0bce770d 348 // ball between samples. Our model ball starts at rest, and our damping
mjr 5:a70c0bce770d 349 // function ensures that when it's in motion, it will return to rest in
mjr 5:a70c0bce770d 350 // a short time in the absence of further physical accelerations. The
mjr 5:a70c0bce770d 351 // sum or our reports to VP from a rest state to a subsequent rest state
mjr 5:a70c0bce770d 352 // will thus necessarily equal exactly zero. This will ensure that we
mjr 5:a70c0bce770d 353 // don't leave VP's model ball with any residual velocity after an
mjr 5:a70c0bce770d 354 // isolated nudge.
mjr 5:a70c0bce770d 355 //
mjr 5:a70c0bce770d 356 // We do one more bit of data processing: automatic calibration. When
mjr 5:a70c0bce770d 357 // we observe the accelerometer input staying constant (within a noise
mjr 5:a70c0bce770d 358 // window) for a few seconds continously, we'll assume that the cabinet
mjr 5:a70c0bce770d 359 // is at rest. It's safe to assume that the accelerometer isn't
mjr 5:a70c0bce770d 360 // installed in such a way that it's perfectly level, so at the
mjr 5:a70c0bce770d 361 // cabinet's neutral rest position, we can expect to read non-zero
mjr 5:a70c0bce770d 362 // accelerations on the x and y axes from the component along that
mjr 5:a70c0bce770d 363 // axis of the Earth's gravity. By watching for constant acceleration
mjr 5:a70c0bce770d 364 // values over time, we can infer the reseting position of the device
mjr 5:a70c0bce770d 365 // and take that as our zero point. By doing this continuously, we
mjr 5:a70c0bce770d 366 // don't have to assume that the machine is perfectly motionless when
mjr 5:a70c0bce770d 367 // initially powered on - we'll organically find the zero point as soon
mjr 5:a70c0bce770d 368 // as the machine is undisturbed for a few moments. We'll also deal
mjr 5:a70c0bce770d 369 // gracefully with situations where the machine is jolted so much in
mjr 5:a70c0bce770d 370 // the course of play that its position is changed slightly. The result
mjr 5:a70c0bce770d 371 // should be to make the zeroing process reliable and completely
mjr 5:a70c0bce770d 372 // transparent to the user.
mjr 5:a70c0bce770d 373 //
mjr 5:a70c0bce770d 374
mjr 5:a70c0bce770d 375 // point structure
mjr 5:a70c0bce770d 376 struct FPoint
mjr 5:a70c0bce770d 377 {
mjr 5:a70c0bce770d 378 float x, y;
mjr 5:a70c0bce770d 379
mjr 5:a70c0bce770d 380 FPoint() { }
mjr 5:a70c0bce770d 381 FPoint(float x, float y) { this->x = x; this->y = y; }
mjr 5:a70c0bce770d 382
mjr 5:a70c0bce770d 383 void set(float x, float y) { this->x = x; this->y = y; }
mjr 5:a70c0bce770d 384 void zero() { this->x = this->y = 0; }
mjr 5:a70c0bce770d 385
mjr 5:a70c0bce770d 386 FPoint &operator=(FPoint &pt) { this->x = pt.x; this->y = pt.y; return *this; }
mjr 5:a70c0bce770d 387 FPoint &operator-=(FPoint &pt) { this->x -= pt.x; this->y -= pt.y; return *this; }
mjr 5:a70c0bce770d 388 FPoint &operator+=(FPoint &pt) { this->x += pt.x; this->y += pt.y; return *this; }
mjr 5:a70c0bce770d 389 FPoint &operator*=(float f) { this->x *= f; this->y *= f; return *this; }
mjr 5:a70c0bce770d 390 FPoint &operator/=(float f) { this->x /= f; this->y /= f; return *this; }
mjr 5:a70c0bce770d 391 float magnitude() const { return sqrt(x*x + y*y); }
mjr 5:a70c0bce770d 392
mjr 5:a70c0bce770d 393 float distance(FPoint &b)
mjr 5:a70c0bce770d 394 {
mjr 5:a70c0bce770d 395 float dx = x - b.x;
mjr 5:a70c0bce770d 396 float dy = y - b.y;
mjr 5:a70c0bce770d 397 return sqrt(dx*dx + dy*dy);
mjr 5:a70c0bce770d 398 }
mjr 5:a70c0bce770d 399 };
mjr 5:a70c0bce770d 400
mjr 5:a70c0bce770d 401
mjr 5:a70c0bce770d 402 // accelerometer wrapper class
mjr 3:3514575d4f86 403 class Accel
mjr 3:3514575d4f86 404 {
mjr 3:3514575d4f86 405 public:
mjr 3:3514575d4f86 406 Accel(PinName sda, PinName scl, int i2cAddr, PinName irqPin)
mjr 3:3514575d4f86 407 : mma_(sda, scl, i2cAddr), intIn_(irqPin)
mjr 3:3514575d4f86 408 {
mjr 5:a70c0bce770d 409 // remember the interrupt pin assignment
mjr 5:a70c0bce770d 410 irqPin_ = irqPin;
mjr 5:a70c0bce770d 411
mjr 5:a70c0bce770d 412 // reset and initialize
mjr 5:a70c0bce770d 413 reset();
mjr 5:a70c0bce770d 414 }
mjr 5:a70c0bce770d 415
mjr 5:a70c0bce770d 416 void reset()
mjr 5:a70c0bce770d 417 {
mjr 5:a70c0bce770d 418 // assume initially that the device is perfectly level
mjr 5:a70c0bce770d 419 center_.zero();
mjr 5:a70c0bce770d 420 tCenter_.start();
mjr 5:a70c0bce770d 421 iAccPrv_ = nAccPrv_ = 0;
mjr 5:a70c0bce770d 422
mjr 5:a70c0bce770d 423 // reset and initialize the MMA8451Q
mjr 5:a70c0bce770d 424 mma_.init();
mjr 5:a70c0bce770d 425
mjr 3:3514575d4f86 426 // set the initial ball velocity to zero
mjr 5:a70c0bce770d 427 v_.zero();
mjr 3:3514575d4f86 428
mjr 3:3514575d4f86 429 // set the initial raw acceleration reading to zero
mjr 5:a70c0bce770d 430 araw_.zero();
mjr 5:a70c0bce770d 431 vsum_.zero();
mjr 3:3514575d4f86 432
mjr 3:3514575d4f86 433 // enable the interrupt
mjr 5:a70c0bce770d 434 mma_.setInterruptMode(irqPin_ == PTA14 ? 1 : 2);
mjr 3:3514575d4f86 435
mjr 3:3514575d4f86 436 // set up the interrupt handler
mjr 3:3514575d4f86 437 intIn_.rise(this, &Accel::isr);
mjr 3:3514575d4f86 438
mjr 3:3514575d4f86 439 // read the current registers to clear the data ready flag
mjr 3:3514575d4f86 440 float z;
mjr 5:a70c0bce770d 441 mma_.getAccXYZ(araw_.x, araw_.y, z);
mjr 3:3514575d4f86 442
mjr 3:3514575d4f86 443 // start our timers
mjr 3:3514575d4f86 444 tGet_.start();
mjr 3:3514575d4f86 445 tInt_.start();
mjr 5:a70c0bce770d 446 tRest_.start();
mjr 3:3514575d4f86 447 }
mjr 3:3514575d4f86 448
mjr 3:3514575d4f86 449 void get(float &x, float &y, float &rx, float &ry)
mjr 3:3514575d4f86 450 {
mjr 3:3514575d4f86 451 // disable interrupts while manipulating the shared data
mjr 3:3514575d4f86 452 __disable_irq();
mjr 3:3514575d4f86 453
mjr 3:3514575d4f86 454 // read the shared data and store locally for calculations
mjr 5:a70c0bce770d 455 FPoint vsum = vsum_, araw = araw_;
mjr 5:a70c0bce770d 456
mjr 5:a70c0bce770d 457 // reset the velocity sum
mjr 5:a70c0bce770d 458 vsum_.zero();
mjr 3:3514575d4f86 459
mjr 3:3514575d4f86 460 // get the time since the last get() sample
mjr 3:3514575d4f86 461 float dt = tGet_.read_us()/1.0e6;
mjr 3:3514575d4f86 462 tGet_.reset();
mjr 3:3514575d4f86 463
mjr 3:3514575d4f86 464 // done manipulating the shared data
mjr 3:3514575d4f86 465 __enable_irq();
mjr 3:3514575d4f86 466
mjr 5:a70c0bce770d 467 // check for auto-centering every so often
mjr 5:a70c0bce770d 468 if (tCenter_.read_ms() > 1000)
mjr 5:a70c0bce770d 469 {
mjr 5:a70c0bce770d 470 // add the latest raw sample to the history list
mjr 5:a70c0bce770d 471 accPrv_[iAccPrv_] = araw_;
mjr 5:a70c0bce770d 472
mjr 5:a70c0bce770d 473 // commit the history entry
mjr 5:a70c0bce770d 474 iAccPrv_ = (iAccPrv_ + 1) % maxAccPrv;
mjr 5:a70c0bce770d 475
mjr 5:a70c0bce770d 476 // if we have a full complement, check for stability
mjr 5:a70c0bce770d 477 if (nAccPrv_ >= maxAccPrv)
mjr 5:a70c0bce770d 478 {
mjr 5:a70c0bce770d 479 // check if we've been stable for all recent samples
mjr 5:a70c0bce770d 480 static const float accTol = .005;
mjr 5:a70c0bce770d 481 if (accPrv_[0].distance(accPrv_[1]) < accTol
mjr 5:a70c0bce770d 482 && accPrv_[0].distance(accPrv_[2]) < accTol
mjr 5:a70c0bce770d 483 && accPrv_[0].distance(accPrv_[3]) < accTol
mjr 5:a70c0bce770d 484 && accPrv_[0].distance(accPrv_[4]) < accTol)
mjr 5:a70c0bce770d 485 {
mjr 5:a70c0bce770d 486 // figure the new center as the average of these samples
mjr 5:a70c0bce770d 487 center_.set(
mjr 5:a70c0bce770d 488 (accPrv_[0].x + accPrv_[1].x + accPrv_[2].x + accPrv_[3].x + accPrv_[4].x)/5.0,
mjr 5:a70c0bce770d 489 (accPrv_[0].y + accPrv_[1].y + accPrv_[2].y + accPrv_[3].y + accPrv_[4].y)/5.0);
mjr 5:a70c0bce770d 490 }
mjr 5:a70c0bce770d 491 }
mjr 5:a70c0bce770d 492 else
mjr 5:a70c0bce770d 493 {
mjr 5:a70c0bce770d 494 // not enough samples yet; just up the count
mjr 5:a70c0bce770d 495 ++nAccPrv_;
mjr 5:a70c0bce770d 496 }
mjr 5:a70c0bce770d 497
mjr 5:a70c0bce770d 498 // reset the timer
mjr 5:a70c0bce770d 499 tCenter_.reset();
mjr 5:a70c0bce770d 500 }
mjr 5:a70c0bce770d 501
mjr 5:a70c0bce770d 502 // Calculate the velocity vector for the model ball. Start
mjr 5:a70c0bce770d 503 // with the accumulated velocity from the accelerations since
mjr 5:a70c0bce770d 504 // the last reading.
mjr 5:a70c0bce770d 505 FPoint dv = vsum;
mjr 5:a70c0bce770d 506
mjr 5:a70c0bce770d 507 // remember the previous velocity of the model ball
mjr 5:a70c0bce770d 508 FPoint vprv = v_;
mjr 5:a70c0bce770d 509
mjr 5:a70c0bce770d 510 // If we have residual motion, check for damping.
mjr 5:a70c0bce770d 511 //
mjr 5:a70c0bce770d 512 // The dmaping we model here isn't friction - we leave that sort of
mjr 5:a70c0bce770d 513 // detail to the pinball simulator on the PC. Instead, our form of
mjr 5:a70c0bce770d 514 // damping is just an attempt to compensate for measurement errors
mjr 5:a70c0bce770d 515 // from the accelerometer. During a nudge event, we should see a
mjr 5:a70c0bce770d 516 // series of accelerations back and forth, as the table sways in
mjr 5:a70c0bce770d 517 // response to the push, rebounds from the sway, rebounds from the
mjr 5:a70c0bce770d 518 // rebound, etc. We know that in reality, the table itself doesn't
mjr 5:a70c0bce770d 519 // actually go anywhere - it just sways, and when the swaying stops,
mjr 5:a70c0bce770d 520 // it ends up where it started. If we use the accelerometer input
mjr 5:a70c0bce770d 521 // to do dead reckoning on the location of the table, we know that
mjr 5:a70c0bce770d 522 // it has to end up where it started. This means that the series of
mjr 5:a70c0bce770d 523 // position changes over the course of the event should cancel out -
mjr 5:a70c0bce770d 524 // the displacements should add up to zero.
mjr 3:3514575d4f86 525
mjr 5:a70c0bce770d 526 to model friction and other forces
mjr 5:a70c0bce770d 527 // on the ball. Instead, the damping we apply is to compensate for
mjr 5:a70c0bce770d 528 // measurement errors in the accelerometer. During a nudge event,
mjr 5:a70c0bce770d 529 // a real pinball cabinet typically ends up at the same place it
mjr 5:a70c0bce770d 530 // started - it sways in response to the nudge, but the swaying
mjr 5:a70c0bce770d 531 // quickly damps out and leaves the table unmoved. You don't
mjr 5:a70c0bce770d 532 // typically apply enough force to actually pick up the cabinet
mjr 5:a70c0bce770d 533 // and move it, or slide it across the floor - and doing so would
mjr 5:a70c0bce770d 534 // trigger a tilt, in which case the ball goes out of play and we
mjr 5:a70c0bce770d 535 // don't really have to worry about how realistically it behaves
mjr 5:a70c0bce770d 536 // in response to the acceleration.
mjr 5:a70c0bce770d 537 if (vprv.magnitude() != 0)
mjr 5:a70c0bce770d 538 {
mjr 5:a70c0bce770d 539 // The model ball is moving. If the current motion has been
mjr 5:a70c0bce770d 540 // going on for long enough, apply damping. We wait a short
mjr 5:a70c0bce770d 541 // time before we apply damping to allow small continuous
mjr 5:a70c0bce770d 542 // accelerations (from tiling the table) to get the ball
mjr 5:a70c0bce770d 543 // rolling.
mjr 5:a70c0bce770d 544 if (tRest_.read_ms() > 100)
mjr 5:a70c0bce770d 545 {
mjr 5:a70c0bce770d 546 }
mjr 5:a70c0bce770d 547 }
mjr 5:a70c0bce770d 548 else
mjr 5:a70c0bce770d 549 {
mjr 5:a70c0bce770d 550 // the model ball is at rest; if the instantaneous acceleration
mjr 5:a70c0bce770d 551 // is also near zero, reset the rest timer
mjr 5:a70c0bce770d 552 if (dv.magnitude() < 0.025)
mjr 5:a70c0bce770d 553 tRest_.reset();
mjr 5:a70c0bce770d 554 }
mjr 5:a70c0bce770d 555
mjr 5:a70c0bce770d 556 // If the current velocity change is near zero, damp the ball's
mjr 5:a70c0bce770d 557 // velocity. The idea is that the total series of accelerations
mjr 5:a70c0bce770d 558 // from a nudge should net to zero, since a nudge doesn't
mjr 5:a70c0bce770d 559 // actually move the table anywhere.
mjr 5:a70c0bce770d 560 //
mjr 5:a70c0bce770d 561 // Ideally, this wouldn't be necessary, because the raw
mjr 5:a70c0bce770d 562 // accelerometer readings should organically add up to zero over
mjr 5:a70c0bce770d 563 // the course of a nudge. In practice, the accelerometer isn't
mjr 5:a70c0bce770d 564 // perfect; it can only sample so fast, so it can't capture every
mjr 5:a70c0bce770d 565 // instantaneous change; and each reading has some small measurement
mjr 5:a70c0bce770d 566 // error, which becomes significant when many readings are added
mjr 5:a70c0bce770d 567 // together. The damping is an attempt to reconcile the imperfect
mjr 5:a70c0bce770d 568 // measurements with what how expect the real physical system to
mjr 5:a70c0bce770d 569 // behave - we know what the outcome of an event should be, so we
mjr 5:a70c0bce770d 570 // adjust our measurements to get the expected outcome.
mjr 5:a70c0bce770d 571 //
mjr 5:a70c0bce770d 572 // If the ball's velocity is large at this point, assume that this
mjr 5:a70c0bce770d 573 // wasn't a nudge event at all, but a sustained inclination - as
mjr 5:a70c0bce770d 574 // though the player picked up one end of the table and held it
mjr 5:a70c0bce770d 575 // up for a while, to accelerate the ball down the sloped table.
mjr 5:a70c0bce770d 576 // In this case just reset the velocity to zero without doing
mjr 5:a70c0bce770d 577 // any damping, so that we don't pass through any deceleration
mjr 5:a70c0bce770d 578 // to the pinball simulation. In this case we want to leave it
mjr 5:a70c0bce770d 579 // to the pinball simulation to do its own modeling of friction
mjr 5:a70c0bce770d 580 // or bouncing to decelerate the ball. Our correction is only
mjr 5:a70c0bce770d 581 // realistic for brief events that naturally net out to neutral
mjr 5:a70c0bce770d 582 // accelerations.
mjr 5:a70c0bce770d 583 if (dv.magnitude() < .025)
mjr 5:a70c0bce770d 584 {
mjr 5:a70c0bce770d 585 // check the ball's speed
mjr 5:a70c0bce770d 586 if (v_.magnitude() < .25)
mjr 5:a70c0bce770d 587 {
mjr 5:a70c0bce770d 588 // apply the damping
mjr 5:a70c0bce770d 589 FPoint damp(damping(v_.x), damping(v_.y));
mjr 5:a70c0bce770d 590 dv -= damp;
mjr 5:a70c0bce770d 591 ledB = 0;
mjr 5:a70c0bce770d 592 }
mjr 5:a70c0bce770d 593 else
mjr 5:a70c0bce770d 594 {
mjr 5:a70c0bce770d 595 // the ball is going too fast - simply reset it
mjr 5:a70c0bce770d 596 v_ = dv;
mjr 5:a70c0bce770d 597 vprv = dv;
mjr 5:a70c0bce770d 598 ledB = 1;
mjr 5:a70c0bce770d 599 }
mjr 5:a70c0bce770d 600 }
mjr 5:a70c0bce770d 601 else
mjr 5:a70c0bce770d 602 ledB = 1;
mjr 5:a70c0bce770d 603
mjr 5:a70c0bce770d 604 // apply the velocity change for this interval
mjr 5:a70c0bce770d 605 v_ += dv;
mjr 5:a70c0bce770d 606
mjr 5:a70c0bce770d 607 // return the acceleration since the last update (change in velocity
mjr 5:a70c0bce770d 608 // over time) in x,y
mjr 5:a70c0bce770d 609 dv /= dt;
mjr 5:a70c0bce770d 610 x = (v_.x - vprv.x) / dt;
mjr 5:a70c0bce770d 611 y = (v_.y - vprv.y) / dt;
mjr 5:a70c0bce770d 612
mjr 5:a70c0bce770d 613 // report the calibrated instantaneous acceleration in rx,ry
mjr 5:a70c0bce770d 614 rx = araw.x - center_.x;
mjr 5:a70c0bce770d 615 ry = araw.y - center_.y;
mjr 3:3514575d4f86 616 }
mjr 3:3514575d4f86 617
mjr 3:3514575d4f86 618 private:
mjr 5:a70c0bce770d 619 // velocity damping function
mjr 5:a70c0bce770d 620 float damping(float v)
mjr 5:a70c0bce770d 621 {
mjr 5:a70c0bce770d 622 // scale to -2048..2048 range, and get the absolute value
mjr 5:a70c0bce770d 623 float a = fabs(v*2048.0);
mjr 5:a70c0bce770d 624
mjr 5:a70c0bce770d 625 // damp out small velocities immediately
mjr 5:a70c0bce770d 626 if (a < 20)
mjr 5:a70c0bce770d 627 return v;
mjr 5:a70c0bce770d 628
mjr 5:a70c0bce770d 629 // calculate the cube root of the scaled value
mjr 5:a70c0bce770d 630 float r = exp(log(a)/3.0);
mjr 5:a70c0bce770d 631
mjr 5:a70c0bce770d 632 // rescale
mjr 5:a70c0bce770d 633 r /= 2048.0;
mjr 5:a70c0bce770d 634
mjr 5:a70c0bce770d 635 // apply the sign and return the result
mjr 5:a70c0bce770d 636 return (v < 0 ? -r : r);
mjr 5:a70c0bce770d 637 }
mjr 5:a70c0bce770d 638
mjr 3:3514575d4f86 639 // interrupt handler
mjr 3:3514575d4f86 640 void isr()
mjr 3:3514575d4f86 641 {
mjr 3:3514575d4f86 642 // Read the axes. Note that we have to read all three axes
mjr 3:3514575d4f86 643 // (even though we only really use x and y) in order to clear
mjr 3:3514575d4f86 644 // the "data ready" status bit in the accelerometer. The
mjr 3:3514575d4f86 645 // interrupt only occurs when the "ready" bit transitions from
mjr 3:3514575d4f86 646 // off to on, so we have to make sure it's off.
mjr 5:a70c0bce770d 647 float x, y, z;
mjr 5:a70c0bce770d 648 mma_.getAccXYZ(x, y, z);
mjr 5:a70c0bce770d 649
mjr 5:a70c0bce770d 650 // store the raw results
mjr 5:a70c0bce770d 651 araw_.set(x, y);
mjr 5:a70c0bce770d 652 zraw_ = z;
mjr 3:3514575d4f86 653
mjr 3:3514575d4f86 654 // calculate the time since the last interrupt
mjr 3:3514575d4f86 655 float dt = tInt_.read_us()/1.0e6;
mjr 3:3514575d4f86 656 tInt_.reset();
mjr 3:3514575d4f86 657
mjr 5:a70c0bce770d 658 // Add the velocity to the running total. First, calibrate the
mjr 5:a70c0bce770d 659 // raw acceleration to our centerpoint, then multiply by the time
mjr 5:a70c0bce770d 660 // since the last sample to get the velocity resulting from
mjr 5:a70c0bce770d 661 // applying this acceleration for the sample time.
mjr 5:a70c0bce770d 662 FPoint rdt((x - center_.x)*dt, (y - center_.y)*dt);
mjr 5:a70c0bce770d 663 vsum_ += rdt;
mjr 3:3514575d4f86 664 }
mjr 3:3514575d4f86 665
mjr 3:3514575d4f86 666 // underlying accelerometer object
mjr 3:3514575d4f86 667 MMA8451Q mma_;
mjr 3:3514575d4f86 668
mjr 5:a70c0bce770d 669 // last raw acceleration readings
mjr 5:a70c0bce770d 670 FPoint araw_;
mjr 5:a70c0bce770d 671 float zraw_;
mjr 5:a70c0bce770d 672
mjr 5:a70c0bce770d 673 // total velocity change since the last get() sample
mjr 5:a70c0bce770d 674 FPoint vsum_;
mjr 5:a70c0bce770d 675
mjr 5:a70c0bce770d 676 // current modeled ball velocity
mjr 5:a70c0bce770d 677 FPoint v_;
mjr 3:3514575d4f86 678
mjr 3:3514575d4f86 679 // timer for measuring time between get() samples
mjr 3:3514575d4f86 680 Timer tGet_;
mjr 3:3514575d4f86 681
mjr 3:3514575d4f86 682 // timer for measuring time between interrupts
mjr 3:3514575d4f86 683 Timer tInt_;
mjr 5:a70c0bce770d 684
mjr 5:a70c0bce770d 685 // time since last rest
mjr 5:a70c0bce770d 686 Timer tRest_;
mjr 5:a70c0bce770d 687
mjr 5:a70c0bce770d 688 // calibrated center point - this is the position where we observe
mjr 5:a70c0bce770d 689 // constant input for a few seconds, telling us the orientation of
mjr 5:a70c0bce770d 690 // the accelerometer device when at rest
mjr 5:a70c0bce770d 691 FPoint center_;
mjr 5:a70c0bce770d 692
mjr 5:a70c0bce770d 693 // timer for atuo-centering
mjr 5:a70c0bce770d 694 Timer tCenter_;
mjr 5:a70c0bce770d 695
mjr 5:a70c0bce770d 696 // recent accelerometer readings, for auto centering
mjr 5:a70c0bce770d 697 int iAccPrv_, nAccPrv_;
mjr 5:a70c0bce770d 698 static const int maxAccPrv = 5;
mjr 5:a70c0bce770d 699 FPoint accPrv_[maxAccPrv];
mjr 5:a70c0bce770d 700
mjr 5:a70c0bce770d 701 // interurupt pin name
mjr 5:a70c0bce770d 702 PinName irqPin_;
mjr 5:a70c0bce770d 703
mjr 5:a70c0bce770d 704 // interrupt router
mjr 5:a70c0bce770d 705 InterruptIn intIn_;
mjr 3:3514575d4f86 706 };
mjr 3:3514575d4f86 707
mjr 5:a70c0bce770d 708
mjr 5:a70c0bce770d 709 // ---------------------------------------------------------------------------
mjr 5:a70c0bce770d 710 //
mjr 5:a70c0bce770d 711 // Clear the I2C bus for the MMA8451!. This seems necessary some of the time
mjr 5:a70c0bce770d 712 // for reasons that aren't clear to me. Doing a hard power cycle has the same
mjr 5:a70c0bce770d 713 // effect, but when we do a soft reset, the hardware sometimes seems to leave
mjr 5:a70c0bce770d 714 // the MMA's SDA line stuck low. Forcing a series of 9 clock pulses through
mjr 5:a70c0bce770d 715 // the SCL line is supposed to clear this conidtion.
mjr 5:a70c0bce770d 716 //
mjr 5:a70c0bce770d 717 void clear_i2c()
mjr 5:a70c0bce770d 718 {
mjr 5:a70c0bce770d 719 // assume a general-purpose output pin to the I2C clock
mjr 5:a70c0bce770d 720 DigitalOut scl(MMA8451_SCL_PIN);
mjr 5:a70c0bce770d 721 DigitalIn sda(MMA8451_SDA_PIN);
mjr 5:a70c0bce770d 722
mjr 5:a70c0bce770d 723 // clock the SCL 9 times
mjr 5:a70c0bce770d 724 for (int i = 0 ; i < 9 ; ++i)
mjr 5:a70c0bce770d 725 {
mjr 5:a70c0bce770d 726 scl = 1;
mjr 5:a70c0bce770d 727 wait_us(20);
mjr 5:a70c0bce770d 728 scl = 0;
mjr 5:a70c0bce770d 729 wait_us(20);
mjr 5:a70c0bce770d 730 }
mjr 5:a70c0bce770d 731 }
mjr 5:a70c0bce770d 732
mjr 5:a70c0bce770d 733 // ---------------------------------------------------------------------------
mjr 5:a70c0bce770d 734 //
mjr 5:a70c0bce770d 735 // Main program loop. This is invoked on startup and runs forever. Our
mjr 5:a70c0bce770d 736 // main work is to read our devices (the accelerometer and the CCD), process
mjr 5:a70c0bce770d 737 // the readings into nudge and plunger position data, and send the results
mjr 5:a70c0bce770d 738 // to the host computer via the USB joystick interface. We also monitor
mjr 5:a70c0bce770d 739 // the USB connection for incoming LedWiz commands and process those into
mjr 5:a70c0bce770d 740 // port outputs.
mjr 5:a70c0bce770d 741 //
mjr 0:5acbbe3f4cf4 742 int main(void)
mjr 0:5acbbe3f4cf4 743 {
mjr 1:d913e0afb2ac 744 // turn off our on-board indicator LED
mjr 4:02c7cd7b2183 745 ledR = 1;
mjr 4:02c7cd7b2183 746 ledG = 1;
mjr 4:02c7cd7b2183 747 ledB = 1;
mjr 1:d913e0afb2ac 748
mjr 5:a70c0bce770d 749 // clear the I2C bus for the accelerometer
mjr 5:a70c0bce770d 750 clear_i2c();
mjr 5:a70c0bce770d 751
mjr 5:a70c0bce770d 752 // Create the joystick USB client
mjr 5:a70c0bce770d 753 MyUSBJoystick js(USB_VENDOR_ID, USB_PRODUCT_ID, USB_VERSION_NO);
mjr 5:a70c0bce770d 754
mjr 2:c174f9ee414a 755 // set up a flash memory controller
mjr 2:c174f9ee414a 756 FreescaleIAP iap;
mjr 2:c174f9ee414a 757
mjr 2:c174f9ee414a 758 // use the last sector of flash for our non-volatile memory structure
mjr 2:c174f9ee414a 759 int flash_addr = (iap.flash_size() - SECTOR_SIZE);
mjr 2:c174f9ee414a 760 NVM *flash = (NVM *)flash_addr;
mjr 2:c174f9ee414a 761 NVM cfg;
mjr 2:c174f9ee414a 762
mjr 2:c174f9ee414a 763 // check for valid flash
mjr 2:c174f9ee414a 764 bool flash_valid = (flash->d.sig == flash->SIGNATURE
mjr 2:c174f9ee414a 765 && flash->d.vsn == flash->VERSION
mjr 2:c174f9ee414a 766 && flash->checksum == CRC32(&flash->d, sizeof(flash->d)));
mjr 2:c174f9ee414a 767
mjr 2:c174f9ee414a 768 // Number of pixels we read from the sensor on each frame. This can be
mjr 2:c174f9ee414a 769 // less than the physical pixel count if desired; we'll read every nth
mjr 2:c174f9ee414a 770 // piexl if so. E.g., with a 1280-pixel physical sensor, if npix is 320,
mjr 5:a70c0bce770d 771 // we'll read every 4th pixel. It takes time to read each pixel, so the
mjr 5:a70c0bce770d 772 // fewer pixels we read, the higher the refresh rate we can achieve.
mjr 5:a70c0bce770d 773 // It's therefore better not to read more pixels than we have to.
mjr 5:a70c0bce770d 774 //
mjr 5:a70c0bce770d 775 // VP seems to have an internal resolution in the 8-bit range, so there's
mjr 5:a70c0bce770d 776 // no apparent benefit to reading more than 128-256 pixels when using VP.
mjr 5:a70c0bce770d 777 // Empirically, 160 pixels seems about right. The overall travel of a
mjr 5:a70c0bce770d 778 // standard pinball plunger is about 3", so 160 pixels gives us resolution
mjr 5:a70c0bce770d 779 // of about 1/50". This seems to take full advantage of VP's modeling
mjr 5:a70c0bce770d 780 // ability, and is probably also more precise than a human player's
mjr 5:a70c0bce770d 781 // perception of the plunger position.
mjr 2:c174f9ee414a 782 const int npix = 160;
mjr 2:c174f9ee414a 783
mjr 2:c174f9ee414a 784 // if the flash is valid, load it; otherwise initialize to defaults
mjr 2:c174f9ee414a 785 if (flash_valid) {
mjr 2:c174f9ee414a 786 memcpy(&cfg, flash, sizeof(cfg));
mjr 2:c174f9ee414a 787 printf("Flash restored: plunger min=%d, max=%d\r\n",
mjr 2:c174f9ee414a 788 cfg.d.plungerMin, cfg.d.plungerMax);
mjr 2:c174f9ee414a 789 }
mjr 2:c174f9ee414a 790 else {
mjr 2:c174f9ee414a 791 printf("Factory reset\r\n");
mjr 2:c174f9ee414a 792 cfg.d.sig = cfg.SIGNATURE;
mjr 2:c174f9ee414a 793 cfg.d.vsn = cfg.VERSION;
mjr 2:c174f9ee414a 794 cfg.d.plungerMin = 0;
mjr 2:c174f9ee414a 795 cfg.d.plungerMax = npix;
mjr 2:c174f9ee414a 796 }
mjr 1:d913e0afb2ac 797
mjr 1:d913e0afb2ac 798 // plunger calibration button debounce timer
mjr 1:d913e0afb2ac 799 Timer calBtnTimer;
mjr 1:d913e0afb2ac 800 calBtnTimer.start();
mjr 1:d913e0afb2ac 801 int calBtnDownTime = 0;
mjr 1:d913e0afb2ac 802 int calBtnLit = false;
mjr 1:d913e0afb2ac 803
mjr 1:d913e0afb2ac 804 // Calibration button state:
mjr 1:d913e0afb2ac 805 // 0 = not pushed
mjr 1:d913e0afb2ac 806 // 1 = pushed, not yet debounced
mjr 1:d913e0afb2ac 807 // 2 = pushed, debounced, waiting for hold time
mjr 1:d913e0afb2ac 808 // 3 = pushed, hold time completed - in calibration mode
mjr 1:d913e0afb2ac 809 int calBtnState = 0;
mjr 1:d913e0afb2ac 810
mjr 1:d913e0afb2ac 811 // set up a timer for our heartbeat indicator
mjr 1:d913e0afb2ac 812 Timer hbTimer;
mjr 1:d913e0afb2ac 813 hbTimer.start();
mjr 1:d913e0afb2ac 814 int hb = 0;
mjr 5:a70c0bce770d 815 uint16_t hbcnt = 0;
mjr 1:d913e0afb2ac 816
mjr 1:d913e0afb2ac 817 // set a timer for accelerometer auto-centering
mjr 1:d913e0afb2ac 818 Timer acTimer;
mjr 1:d913e0afb2ac 819 acTimer.start();
mjr 1:d913e0afb2ac 820
mjr 0:5acbbe3f4cf4 821 // create the accelerometer object
mjr 5:a70c0bce770d 822 Accel accel(MMA8451_SCL_PIN, MMA8451_SDA_PIN, MMA8451_I2C_ADDRESS, MMA8451_INT_PIN);
mjr 0:5acbbe3f4cf4 823
mjr 0:5acbbe3f4cf4 824 // create the CCD array object
mjr 1:d913e0afb2ac 825 TSL1410R ccd(PTE20, PTE21, PTB0);
mjr 2:c174f9ee414a 826
mjr 1:d913e0afb2ac 827 // last accelerometer report, in mouse coordinates
mjr 1:d913e0afb2ac 828 int x = 127, y = 127, z = 0;
mjr 2:c174f9ee414a 829
mjr 2:c174f9ee414a 830 // start the first CCD integration cycle
mjr 2:c174f9ee414a 831 ccd.clear();
mjr 1:d913e0afb2ac 832
mjr 1:d913e0afb2ac 833 // we're all set up - now just loop, processing sensor reports and
mjr 1:d913e0afb2ac 834 // host requests
mjr 0:5acbbe3f4cf4 835 for (;;)
mjr 0:5acbbe3f4cf4 836 {
mjr 0:5acbbe3f4cf4 837 // Look for an incoming report. Continue processing input as
mjr 0:5acbbe3f4cf4 838 // long as there's anything pending - this ensures that we
mjr 0:5acbbe3f4cf4 839 // handle input in as timely a fashion as possible by deferring
mjr 0:5acbbe3f4cf4 840 // output tasks as long as there's input to process.
mjr 0:5acbbe3f4cf4 841 HID_REPORT report;
mjr 0:5acbbe3f4cf4 842 while (js.readNB(&report) && report.length == 8)
mjr 0:5acbbe3f4cf4 843 {
mjr 0:5acbbe3f4cf4 844 uint8_t *data = report.data;
mjr 1:d913e0afb2ac 845 if (data[0] == 64)
mjr 1:d913e0afb2ac 846 {
mjr 0:5acbbe3f4cf4 847 // LWZ-SBA - first four bytes are bit-packed on/off flags
mjr 0:5acbbe3f4cf4 848 // for the outputs; 5th byte is the pulse speed (0-7)
mjr 0:5acbbe3f4cf4 849 //printf("LWZ-SBA %02x %02x %02x %02x ; %02x\r\n",
mjr 0:5acbbe3f4cf4 850 // data[1], data[2], data[3], data[4], data[5]);
mjr 0:5acbbe3f4cf4 851
mjr 0:5acbbe3f4cf4 852 // update all on/off states
mjr 0:5acbbe3f4cf4 853 for (int i = 0, bit = 1, ri = 1 ; i < 32 ; ++i, bit <<= 1)
mjr 0:5acbbe3f4cf4 854 {
mjr 0:5acbbe3f4cf4 855 if (bit == 0x100) {
mjr 0:5acbbe3f4cf4 856 bit = 1;
mjr 0:5acbbe3f4cf4 857 ++ri;
mjr 0:5acbbe3f4cf4 858 }
mjr 1:d913e0afb2ac 859 wizOn[i] = ((data[ri] & bit) != 0);
mjr 0:5acbbe3f4cf4 860 }
mjr 0:5acbbe3f4cf4 861
mjr 1:d913e0afb2ac 862 // update the physical outputs
mjr 1:d913e0afb2ac 863 updateWizOuts();
mjr 0:5acbbe3f4cf4 864
mjr 0:5acbbe3f4cf4 865 // reset the PBA counter
mjr 0:5acbbe3f4cf4 866 pbaIdx = 0;
mjr 0:5acbbe3f4cf4 867 }
mjr 1:d913e0afb2ac 868 else
mjr 1:d913e0afb2ac 869 {
mjr 0:5acbbe3f4cf4 870 // LWZ-PBA - full state dump; each byte is one output
mjr 0:5acbbe3f4cf4 871 // in the current bank. pbaIdx keeps track of the bank;
mjr 0:5acbbe3f4cf4 872 // this is incremented implicitly by each PBA message.
mjr 0:5acbbe3f4cf4 873 //printf("LWZ-PBA[%d] %02x %02x %02x %02x %02x %02x %02x %02x\r\n",
mjr 0:5acbbe3f4cf4 874 // pbaIdx, data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7]);
mjr 0:5acbbe3f4cf4 875
mjr 0:5acbbe3f4cf4 876 // update all output profile settings
mjr 0:5acbbe3f4cf4 877 for (int i = 0 ; i < 8 ; ++i)
mjr 1:d913e0afb2ac 878 wizVal[pbaIdx + i] = data[i];
mjr 0:5acbbe3f4cf4 879
mjr 0:5acbbe3f4cf4 880 // update the physical LED state if this is the last bank
mjr 0:5acbbe3f4cf4 881 if (pbaIdx == 24)
mjr 1:d913e0afb2ac 882 updateWizOuts();
mjr 0:5acbbe3f4cf4 883
mjr 0:5acbbe3f4cf4 884 // advance to the next bank
mjr 0:5acbbe3f4cf4 885 pbaIdx = (pbaIdx + 8) & 31;
mjr 0:5acbbe3f4cf4 886 }
mjr 0:5acbbe3f4cf4 887 }
mjr 1:d913e0afb2ac 888
mjr 1:d913e0afb2ac 889 // check for plunger calibration
mjr 1:d913e0afb2ac 890 if (!calBtn)
mjr 0:5acbbe3f4cf4 891 {
mjr 1:d913e0afb2ac 892 // check the state
mjr 1:d913e0afb2ac 893 switch (calBtnState)
mjr 0:5acbbe3f4cf4 894 {
mjr 1:d913e0afb2ac 895 case 0:
mjr 1:d913e0afb2ac 896 // button not yet pushed - start debouncing
mjr 1:d913e0afb2ac 897 calBtnTimer.reset();
mjr 1:d913e0afb2ac 898 calBtnDownTime = calBtnTimer.read_ms();
mjr 1:d913e0afb2ac 899 calBtnState = 1;
mjr 1:d913e0afb2ac 900 break;
mjr 1:d913e0afb2ac 901
mjr 1:d913e0afb2ac 902 case 1:
mjr 1:d913e0afb2ac 903 // pushed, not yet debounced - if the debounce time has
mjr 1:d913e0afb2ac 904 // passed, start the hold period
mjr 1:d913e0afb2ac 905 if (calBtnTimer.read_ms() - calBtnDownTime > 50)
mjr 1:d913e0afb2ac 906 calBtnState = 2;
mjr 1:d913e0afb2ac 907 break;
mjr 1:d913e0afb2ac 908
mjr 1:d913e0afb2ac 909 case 2:
mjr 1:d913e0afb2ac 910 // in the hold period - if the button has been held down
mjr 1:d913e0afb2ac 911 // for the entire hold period, move to calibration mode
mjr 1:d913e0afb2ac 912 if (calBtnTimer.read_ms() - calBtnDownTime > 2050)
mjr 1:d913e0afb2ac 913 {
mjr 1:d913e0afb2ac 914 // enter calibration mode
mjr 1:d913e0afb2ac 915 calBtnState = 3;
mjr 1:d913e0afb2ac 916
mjr 1:d913e0afb2ac 917 // reset the calibration limits
mjr 2:c174f9ee414a 918 cfg.d.plungerMax = 0;
mjr 2:c174f9ee414a 919 cfg.d.plungerMin = npix;
mjr 1:d913e0afb2ac 920 }
mjr 1:d913e0afb2ac 921 break;
mjr 2:c174f9ee414a 922
mjr 2:c174f9ee414a 923 case 3:
mjr 2:c174f9ee414a 924 // Already in calibration mode - pushing the button in this
mjr 2:c174f9ee414a 925 // state doesn't change the current state, but we won't leave
mjr 2:c174f9ee414a 926 // this state as long as it's held down. We can simply do
mjr 2:c174f9ee414a 927 // nothing here.
mjr 2:c174f9ee414a 928 break;
mjr 0:5acbbe3f4cf4 929 }
mjr 0:5acbbe3f4cf4 930 }
mjr 1:d913e0afb2ac 931 else
mjr 1:d913e0afb2ac 932 {
mjr 2:c174f9ee414a 933 // Button released. If we're in calibration mode, and
mjr 2:c174f9ee414a 934 // the calibration time has elapsed, end the calibration
mjr 2:c174f9ee414a 935 // and save the results to flash.
mjr 2:c174f9ee414a 936 //
mjr 2:c174f9ee414a 937 // Otherwise, return to the base state without saving anything.
mjr 2:c174f9ee414a 938 // If the button is released before we make it to calibration
mjr 2:c174f9ee414a 939 // mode, it simply cancels the attempt.
mjr 2:c174f9ee414a 940 if (calBtnState == 3
mjr 2:c174f9ee414a 941 && calBtnTimer.read_ms() - calBtnDownTime > 17500)
mjr 2:c174f9ee414a 942 {
mjr 2:c174f9ee414a 943 // exit calibration mode
mjr 1:d913e0afb2ac 944 calBtnState = 0;
mjr 2:c174f9ee414a 945
mjr 2:c174f9ee414a 946 // Save the current configuration state to flash, so that it
mjr 2:c174f9ee414a 947 // will be preserved through power off. Update the checksum
mjr 2:c174f9ee414a 948 // first so that we recognize the flash record as valid.
mjr 2:c174f9ee414a 949 cfg.checksum = CRC32(&cfg.d, sizeof(cfg.d));
mjr 2:c174f9ee414a 950 iap.erase_sector(flash_addr);
mjr 2:c174f9ee414a 951 iap.program_flash(flash_addr, &cfg, sizeof(cfg));
mjr 2:c174f9ee414a 952
mjr 2:c174f9ee414a 953 // the flash state is now valid
mjr 2:c174f9ee414a 954 flash_valid = true;
mjr 2:c174f9ee414a 955 }
mjr 2:c174f9ee414a 956 else if (calBtnState != 3)
mjr 2:c174f9ee414a 957 {
mjr 2:c174f9ee414a 958 // didn't make it to calibration mode - cancel the operation
mjr 1:d913e0afb2ac 959 calBtnState = 0;
mjr 2:c174f9ee414a 960 }
mjr 1:d913e0afb2ac 961 }
mjr 1:d913e0afb2ac 962
mjr 1:d913e0afb2ac 963 // light/flash the calibration button light, if applicable
mjr 1:d913e0afb2ac 964 int newCalBtnLit = calBtnLit;
mjr 1:d913e0afb2ac 965 switch (calBtnState)
mjr 0:5acbbe3f4cf4 966 {
mjr 1:d913e0afb2ac 967 case 2:
mjr 1:d913e0afb2ac 968 // in the hold period - flash the light
mjr 1:d913e0afb2ac 969 newCalBtnLit = (((calBtnTimer.read_ms() - calBtnDownTime)/250) & 1);
mjr 1:d913e0afb2ac 970 break;
mjr 1:d913e0afb2ac 971
mjr 1:d913e0afb2ac 972 case 3:
mjr 1:d913e0afb2ac 973 // calibration mode - show steady on
mjr 1:d913e0afb2ac 974 newCalBtnLit = true;
mjr 1:d913e0afb2ac 975 break;
mjr 1:d913e0afb2ac 976
mjr 1:d913e0afb2ac 977 default:
mjr 1:d913e0afb2ac 978 // not calibrating/holding - show steady off
mjr 1:d913e0afb2ac 979 newCalBtnLit = false;
mjr 1:d913e0afb2ac 980 break;
mjr 1:d913e0afb2ac 981 }
mjr 3:3514575d4f86 982
mjr 3:3514575d4f86 983 // light or flash the external calibration button LED, and
mjr 3:3514575d4f86 984 // do the same with the on-board blue LED
mjr 1:d913e0afb2ac 985 if (calBtnLit != newCalBtnLit)
mjr 1:d913e0afb2ac 986 {
mjr 1:d913e0afb2ac 987 calBtnLit = newCalBtnLit;
mjr 2:c174f9ee414a 988 if (calBtnLit) {
mjr 2:c174f9ee414a 989 calBtnLed = 1;
mjr 4:02c7cd7b2183 990 ledR = 1;
mjr 4:02c7cd7b2183 991 ledG = 1;
mjr 4:02c7cd7b2183 992 ledB = 1;
mjr 2:c174f9ee414a 993 }
mjr 2:c174f9ee414a 994 else {
mjr 2:c174f9ee414a 995 calBtnLed = 0;
mjr 4:02c7cd7b2183 996 ledR = 1;
mjr 4:02c7cd7b2183 997 ledG = 1;
mjr 4:02c7cd7b2183 998 ledB = 0;
mjr 2:c174f9ee414a 999 }
mjr 1:d913e0afb2ac 1000 }
mjr 1:d913e0afb2ac 1001
mjr 1:d913e0afb2ac 1002 // read the plunger sensor
mjr 1:d913e0afb2ac 1003 int znew = z;
mjr 2:c174f9ee414a 1004 uint16_t pix[npix];
mjr 2:c174f9ee414a 1005 ccd.read(pix, npix);
mjr 2:c174f9ee414a 1006
mjr 2:c174f9ee414a 1007 // get the average brightness at each end of the sensor
mjr 2:c174f9ee414a 1008 long avg1 = (long(pix[0]) + long(pix[1]) + long(pix[2]) + long(pix[3]) + long(pix[4]))/5;
mjr 2:c174f9ee414a 1009 long avg2 = (long(pix[npix-1]) + long(pix[npix-2]) + long(pix[npix-3]) + long(pix[npix-4]) + long(pix[npix-5]))/5;
mjr 2:c174f9ee414a 1010
mjr 2:c174f9ee414a 1011 // figure the midpoint in the brightness; multiply by 3 so that we can
mjr 2:c174f9ee414a 1012 // compare sums of three pixels at a time to smooth out noise
mjr 2:c174f9ee414a 1013 long midpt = (avg1 + avg2)/2 * 3;
mjr 2:c174f9ee414a 1014
mjr 2:c174f9ee414a 1015 // Work from the bright end to the dark end. VP interprets the
mjr 2:c174f9ee414a 1016 // Z axis value as the amount the plunger is pulled: the minimum
mjr 2:c174f9ee414a 1017 // is the rest position, the maximum is fully pulled. So we
mjr 2:c174f9ee414a 1018 // essentially want to report how much of the sensor is lit,
mjr 2:c174f9ee414a 1019 // since this increases as the plunger is pulled back.
mjr 2:c174f9ee414a 1020 int si = 1, di = 1;
mjr 2:c174f9ee414a 1021 if (avg1 < avg2)
mjr 2:c174f9ee414a 1022 si = npix - 2, di = -1;
mjr 2:c174f9ee414a 1023
mjr 2:c174f9ee414a 1024 // scan for the midpoint
mjr 2:c174f9ee414a 1025 uint16_t *pixp = pix + si;
mjr 2:c174f9ee414a 1026 for (int n = 1 ; n < npix - 1 ; ++n, pixp += di)
mjr 1:d913e0afb2ac 1027 {
mjr 2:c174f9ee414a 1028 // if we've crossed the midpoint, report this position
mjr 2:c174f9ee414a 1029 if (long(pixp[-1]) + long(pixp[0]) + long(pixp[1]) < midpt)
mjr 1:d913e0afb2ac 1030 {
mjr 2:c174f9ee414a 1031 // note the new position
mjr 2:c174f9ee414a 1032 int pos = n;
mjr 2:c174f9ee414a 1033
mjr 2:c174f9ee414a 1034 // if the bright end and dark end don't differ by enough, skip this
mjr 2:c174f9ee414a 1035 // reading entirely - we must have an overexposed or underexposed frame
mjr 2:c174f9ee414a 1036 if (labs(avg1 - avg2) < 0x3333)
mjr 2:c174f9ee414a 1037 break;
mjr 2:c174f9ee414a 1038
mjr 2:c174f9ee414a 1039 // Calibrate, or apply calibration, depending on the mode.
mjr 2:c174f9ee414a 1040 // In either case, normalize to a 0-127 range. VP appears to
mjr 2:c174f9ee414a 1041 // ignore negative Z axis values.
mjr 2:c174f9ee414a 1042 if (calBtnState == 3)
mjr 1:d913e0afb2ac 1043 {
mjr 2:c174f9ee414a 1044 // calibrating - note if we're expanding the calibration envelope
mjr 2:c174f9ee414a 1045 if (pos < cfg.d.plungerMin)
mjr 2:c174f9ee414a 1046 cfg.d.plungerMin = pos;
mjr 2:c174f9ee414a 1047 if (pos > cfg.d.plungerMax)
mjr 2:c174f9ee414a 1048 cfg.d.plungerMax = pos;
mjr 2:c174f9ee414a 1049
mjr 2:c174f9ee414a 1050 // normalize to the full physical range while calibrating
mjr 2:c174f9ee414a 1051 znew = int(float(pos)/npix * 127);
mjr 1:d913e0afb2ac 1052 }
mjr 2:c174f9ee414a 1053 else
mjr 2:c174f9ee414a 1054 {
mjr 2:c174f9ee414a 1055 // running normally - normalize to the calibration range
mjr 2:c174f9ee414a 1056 if (pos < cfg.d.plungerMin)
mjr 2:c174f9ee414a 1057 pos = cfg.d.plungerMin;
mjr 2:c174f9ee414a 1058 if (pos > cfg.d.plungerMax)
mjr 2:c174f9ee414a 1059 pos = cfg.d.plungerMax;
mjr 2:c174f9ee414a 1060 znew = int(float(pos - cfg.d.plungerMin)
mjr 2:c174f9ee414a 1061 / (cfg.d.plungerMax - cfg.d.plungerMin + 1) * 127);
mjr 2:c174f9ee414a 1062 }
mjr 2:c174f9ee414a 1063
mjr 2:c174f9ee414a 1064 // done
mjr 2:c174f9ee414a 1065 break;
mjr 1:d913e0afb2ac 1066 }
mjr 2:c174f9ee414a 1067 }
mjr 1:d913e0afb2ac 1068
mjr 1:d913e0afb2ac 1069 // read the accelerometer
mjr 3:3514575d4f86 1070 float xa, ya, rxa, rya;
mjr 3:3514575d4f86 1071 accel.get(xa, ya, rxa, rya);
mjr 1:d913e0afb2ac 1072
mjr 5:a70c0bce770d 1073 // confine the accelerometer results to the unit interval
mjr 1:d913e0afb2ac 1074 if (xa < -1.0) xa = -1.0;
mjr 1:d913e0afb2ac 1075 if (xa > 1.0) xa = 1.0;
mjr 1:d913e0afb2ac 1076 if (ya < -1.0) ya = -1.0;
mjr 1:d913e0afb2ac 1077 if (ya > 1.0) ya = 1.0;
mjr 0:5acbbe3f4cf4 1078
mjr 5:a70c0bce770d 1079 // scale to our -127..127 reporting range
mjr 5:a70c0bce770d 1080 int xnew = int(127 * xa);
mjr 5:a70c0bce770d 1081 int ynew = int(127 * ya);
mjr 2:c174f9ee414a 1082
mjr 2:c174f9ee414a 1083 // store the updated joystick coordinates
mjr 2:c174f9ee414a 1084 x = xnew;
mjr 2:c174f9ee414a 1085 y = ynew;
mjr 2:c174f9ee414a 1086 z = znew;
mjr 1:d913e0afb2ac 1087
mjr 3:3514575d4f86 1088 // Send the status report. It doesn't really matter what
mjr 3:3514575d4f86 1089 // coordinate system we use, since Visual Pinball has config
mjr 3:3514575d4f86 1090 // options for rotations and axis reversals, but reversing y
mjr 3:3514575d4f86 1091 // at the device level seems to produce the most intuitive
mjr 3:3514575d4f86 1092 // results for the Windows joystick control panel view, which
mjr 3:3514575d4f86 1093 // is an easy way to check that the device is working.
mjr 5:a70c0bce770d 1094 //
mjr 5:a70c0bce770d 1095 // $$$ button updates are for diagnostics, so we can see that the
mjr 5:a70c0bce770d 1096 // device is sending data properly if the accelerometer gets stuck
mjr 5:a70c0bce770d 1097 js.update(x, -y, z, int(rxa*127), int(rya*127), hb ? 0x5500 : 0xAA00);
mjr 1:d913e0afb2ac 1098
mjr 2:c174f9ee414a 1099 // show a heartbeat flash in blue every so often if not in
mjr 2:c174f9ee414a 1100 // calibration mode
mjr 5:a70c0bce770d 1101 if (calBtnState < 2 && hbTimer.read_ms() > 1000)
mjr 1:d913e0afb2ac 1102 {
mjr 5:a70c0bce770d 1103 if (js.isSuspended() || !js.isConnected())
mjr 2:c174f9ee414a 1104 {
mjr 5:a70c0bce770d 1105 // suspended - turn off the LED
mjr 4:02c7cd7b2183 1106 ledR = 1;
mjr 4:02c7cd7b2183 1107 ledG = 1;
mjr 4:02c7cd7b2183 1108 ledB = 1;
mjr 5:a70c0bce770d 1109
mjr 5:a70c0bce770d 1110 // show a status flash every so often
mjr 5:a70c0bce770d 1111 if (hbcnt % 3 == 0)
mjr 5:a70c0bce770d 1112 {
mjr 5:a70c0bce770d 1113 // disconnected = red flash; suspended = red-red
mjr 5:a70c0bce770d 1114 for (int n = js.isConnected() ? 1 : 2 ; n > 0 ; --n)
mjr 5:a70c0bce770d 1115 {
mjr 5:a70c0bce770d 1116 ledR = 0;
mjr 5:a70c0bce770d 1117 wait(0.05);
mjr 5:a70c0bce770d 1118 ledR = 1;
mjr 5:a70c0bce770d 1119 wait(0.25);
mjr 5:a70c0bce770d 1120 }
mjr 5:a70c0bce770d 1121 }
mjr 2:c174f9ee414a 1122 }
mjr 2:c174f9ee414a 1123 else if (flash_valid)
mjr 2:c174f9ee414a 1124 {
mjr 2:c174f9ee414a 1125 // connected, NVM valid - flash blue/green
mjr 2:c174f9ee414a 1126 hb = !hb;
mjr 4:02c7cd7b2183 1127 ledR = 1;
mjr 4:02c7cd7b2183 1128 ledG = (hb ? 0 : 1);
mjr 4:02c7cd7b2183 1129 ledB = (hb ? 1 : 0);
mjr 2:c174f9ee414a 1130 }
mjr 2:c174f9ee414a 1131 else
mjr 2:c174f9ee414a 1132 {
mjr 2:c174f9ee414a 1133 // connected, factory reset - flash yellow/green
mjr 2:c174f9ee414a 1134 hb = !hb;
mjr 5:a70c0bce770d 1135 //ledR = (hb ? 0 : 1);
mjr 5:a70c0bce770d 1136 //ledG = 0;
mjr 4:02c7cd7b2183 1137 ledB = 1;
mjr 2:c174f9ee414a 1138 }
mjr 1:d913e0afb2ac 1139
mjr 1:d913e0afb2ac 1140 // reset the heartbeat timer
mjr 1:d913e0afb2ac 1141 hbTimer.reset();
mjr 5:a70c0bce770d 1142 ++hbcnt;
mjr 1:d913e0afb2ac 1143 }
mjr 1:d913e0afb2ac 1144 }
mjr 0:5acbbe3f4cf4 1145 }