Markus Kalkbrenner / Mbed 2 deprecated Pinscape_Controller

work in progress

Dependencies:   FastAnalogIn FastIO USBDevice mbed FastPWM SimpleDMA

Fork of Pinscape_Controller by Mike R

main.cpp@3:3514575d4f86, 2014-07-23 (annotated)

Committer:
mjr
Date:
Wed Jul 23 17:53:28 2014 +0000
Revision:
3:3514575d4f86
Parent:
2:c174f9ee414a
Child:
4:02c7cd7b2183
Conversion to interrupt-based sampling of the accelerometer working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mjr 0:5acbbe3f4cf4 1 #include "mbed.h"
mjr 0:5acbbe3f4cf4 2 #include "USBJoystick.h"
mjr 0:5acbbe3f4cf4 3 #include "MMA8451Q.h"
mjr 1:d913e0afb2ac 4 #include "tsl1410r.h"
mjr 1:d913e0afb2ac 5 #include "FreescaleIAP.h"
mjr 2:c174f9ee414a 6 #include "crc32.h"
mjr 2:c174f9ee414a 7
mjr 2:c174f9ee414a 8 // customization of the joystick class to expose connect/suspend status
mjr 2:c174f9ee414a 9 class MyUSBJoystick: public USBJoystick
mjr 2:c174f9ee414a 10 {
mjr 2:c174f9ee414a 11 public:
mjr 2:c174f9ee414a 12 MyUSBJoystick(uint16_t vendor_id, uint16_t product_id, uint16_t product_release)
mjr 2:c174f9ee414a 13 : USBJoystick(vendor_id, product_id, product_release)
mjr 2:c174f9ee414a 14 {
mjr 2:c174f9ee414a 15 suspended_ = false;
mjr 2:c174f9ee414a 16 }
mjr 2:c174f9ee414a 17
mjr 3:3514575d4f86 18 int isConnected() { return configured(); }
mjr 2:c174f9ee414a 19 int isSuspended() const { return suspended_; }
mjr 2:c174f9ee414a 20
mjr 2:c174f9ee414a 21 protected:
mjr 2:c174f9ee414a 22 virtual void suspendStateChanged(unsigned int suspended)
mjr 2:c174f9ee414a 23 { suspended_ = suspended; }
mjr 2:c174f9ee414a 24
mjr 2:c174f9ee414a 25 int suspended_;
mjr 2:c174f9ee414a 26 };
mjr 0:5acbbe3f4cf4 27
mjr 1:d913e0afb2ac 28 // on-board RGB LED elements - we use these for diagnostics
mjr 0:5acbbe3f4cf4 29 PwmOut led1(LED1), led2(LED2), led3(LED3);
mjr 0:5acbbe3f4cf4 30
mjr 1:d913e0afb2ac 31 // calibration button - switch input and LED output
mjr 1:d913e0afb2ac 32 DigitalIn calBtn(PTE29);
mjr 1:d913e0afb2ac 33 DigitalOut calBtnLed(PTE23);
mjr 0:5acbbe3f4cf4 34
mjr 0:5acbbe3f4cf4 35 static int pbaIdx = 0;
mjr 0:5acbbe3f4cf4 36
mjr 0:5acbbe3f4cf4 37 // on/off state for each LedWiz output
mjr 1:d913e0afb2ac 38 static uint8_t wizOn[32];
mjr 0:5acbbe3f4cf4 39
mjr 0:5acbbe3f4cf4 40 // profile (brightness/blink) state for each LedWiz output
mjr 1:d913e0afb2ac 41 static uint8_t wizVal[32] = {
mjr 0:5acbbe3f4cf4 42 0, 0, 0, 0, 0, 0, 0, 0,
mjr 0:5acbbe3f4cf4 43 0, 0, 0, 0, 0, 0, 0, 0,
mjr 0:5acbbe3f4cf4 44 0, 0, 0, 0, 0, 0, 0, 0,
mjr 0:5acbbe3f4cf4 45 0, 0, 0, 0, 0, 0, 0, 0
mjr 0:5acbbe3f4cf4 46 };
mjr 0:5acbbe3f4cf4 47
mjr 1:d913e0afb2ac 48 static float wizState(int idx)
mjr 0:5acbbe3f4cf4 49 {
mjr 1:d913e0afb2ac 50 if (wizOn[idx]) {
mjr 0:5acbbe3f4cf4 51 // on - map profile brightness state to PWM level
mjr 1:d913e0afb2ac 52 uint8_t val = wizVal[idx];
mjr 0:5acbbe3f4cf4 53 if (val >= 1 && val <= 48)
mjr 0:5acbbe3f4cf4 54 return 1.0 - val/48.0;
mjr 0:5acbbe3f4cf4 55 else if (val >= 129 && val <= 132)
mjr 0:5acbbe3f4cf4 56 return 0.0;
mjr 0:5acbbe3f4cf4 57 else
mjr 0:5acbbe3f4cf4 58 return 1.0;
mjr 0:5acbbe3f4cf4 59 }
mjr 0:5acbbe3f4cf4 60 else {
mjr 0:5acbbe3f4cf4 61 // off
mjr 0:5acbbe3f4cf4 62 return 1.0;
mjr 0:5acbbe3f4cf4 63 }
mjr 0:5acbbe3f4cf4 64 }
mjr 0:5acbbe3f4cf4 65
mjr 1:d913e0afb2ac 66 static void updateWizOuts()
mjr 1:d913e0afb2ac 67 {
mjr 1:d913e0afb2ac 68 led1 = wizState(0);
mjr 1:d913e0afb2ac 69 led2 = wizState(1);
mjr 1:d913e0afb2ac 70 led3 = wizState(2);
mjr 1:d913e0afb2ac 71 }
mjr 1:d913e0afb2ac 72
mjr 1:d913e0afb2ac 73 struct AccPrv
mjr 0:5acbbe3f4cf4 74 {
mjr 1:d913e0afb2ac 75 AccPrv() : x(0), y(0) { }
mjr 1:d913e0afb2ac 76 float x;
mjr 1:d913e0afb2ac 77 float y;
mjr 1:d913e0afb2ac 78
mjr 1:d913e0afb2ac 79 double dist(AccPrv &b)
mjr 1:d913e0afb2ac 80 {
mjr 1:d913e0afb2ac 81 float dx = x - b.x, dy = y - b.y;
mjr 1:d913e0afb2ac 82 return sqrt(dx*dx + dy*dy);
mjr 1:d913e0afb2ac 83 }
mjr 1:d913e0afb2ac 84 };
mjr 0:5acbbe3f4cf4 85
mjr 2:c174f9ee414a 86 // Non-volatile memory structure. We store persistent a small
mjr 2:c174f9ee414a 87 // amount of persistent data in flash memory to retain calibration
mjr 2:c174f9ee414a 88 // data between sessions.
mjr 2:c174f9ee414a 89 struct NVM
mjr 2:c174f9ee414a 90 {
mjr 2:c174f9ee414a 91 // checksum - we use this to determine if the flash record
mjr 2:c174f9ee414a 92 // has been initialized
mjr 2:c174f9ee414a 93 uint32_t checksum;
mjr 2:c174f9ee414a 94
mjr 2:c174f9ee414a 95 // signature value
mjr 2:c174f9ee414a 96 static const uint32_t SIGNATURE = 0x4D4A522A;
mjr 2:c174f9ee414a 97 static const uint16_t VERSION = 0x0002;
mjr 2:c174f9ee414a 98
mjr 2:c174f9ee414a 99 // stored data (excluding the checksum)
mjr 2:c174f9ee414a 100 struct
mjr 2:c174f9ee414a 101 {
mjr 2:c174f9ee414a 102 // signature and version - further verification that we have valid
mjr 2:c174f9ee414a 103 // initialized data
mjr 2:c174f9ee414a 104 uint32_t sig;
mjr 2:c174f9ee414a 105 uint16_t vsn;
mjr 2:c174f9ee414a 106
mjr 2:c174f9ee414a 107 // direction - 0 means unknown, 1 means bright end is pixel 0, 2 means reversed
mjr 2:c174f9ee414a 108 uint8_t dir;
mjr 2:c174f9ee414a 109
mjr 2:c174f9ee414a 110 // plunger calibration min and max
mjr 2:c174f9ee414a 111 int plungerMin;
mjr 2:c174f9ee414a 112 int plungerMax;
mjr 2:c174f9ee414a 113 } d;
mjr 2:c174f9ee414a 114 };
mjr 2:c174f9ee414a 115
mjr 3:3514575d4f86 116 // Accelerometer handler
mjr 3:3514575d4f86 117 const int MMA8451_I2C_ADDRESS = (0x1d<<1);
mjr 3:3514575d4f86 118 class Accel
mjr 3:3514575d4f86 119 {
mjr 3:3514575d4f86 120 public:
mjr 3:3514575d4f86 121 Accel(PinName sda, PinName scl, int i2cAddr, PinName irqPin)
mjr 3:3514575d4f86 122 : mma_(sda, scl, i2cAddr), intIn_(irqPin)
mjr 3:3514575d4f86 123 {
mjr 3:3514575d4f86 124 // set the initial ball velocity to zero
mjr 3:3514575d4f86 125 vx_ = vy_ = 0;
mjr 3:3514575d4f86 126
mjr 3:3514575d4f86 127 // set the initial raw acceleration reading to zero
mjr 3:3514575d4f86 128 xRaw_ = yRaw_ = 0;
mjr 3:3514575d4f86 129
mjr 3:3514575d4f86 130 // enable the interrupt
mjr 3:3514575d4f86 131 mma_.setInterruptMode(irqPin == PTA14 ? 1 : 2);
mjr 3:3514575d4f86 132
mjr 3:3514575d4f86 133 // set up the interrupt handler
mjr 3:3514575d4f86 134 intIn_.rise(this, &Accel::isr);
mjr 3:3514575d4f86 135
mjr 3:3514575d4f86 136 // read the current registers to clear the data ready flag
mjr 3:3514575d4f86 137 float z;
mjr 3:3514575d4f86 138 mma_.getAccXYZ(xRaw_, yRaw_, z);
mjr 3:3514575d4f86 139
mjr 3:3514575d4f86 140 // start our timers
mjr 3:3514575d4f86 141 tGet_.start();
mjr 3:3514575d4f86 142 tInt_.start();
mjr 3:3514575d4f86 143 }
mjr 3:3514575d4f86 144
mjr 3:3514575d4f86 145 void get(float &x, float &y, float &rx, float &ry)
mjr 3:3514575d4f86 146 {
mjr 3:3514575d4f86 147 // disable interrupts while manipulating the shared data
mjr 3:3514575d4f86 148 __disable_irq();
mjr 3:3514575d4f86 149
mjr 3:3514575d4f86 150 // read the shared data and store locally for calculations
mjr 3:3514575d4f86 151 float vx = vx_, vy = vy_, xRaw = xRaw_, yRaw = yRaw_;
mjr 3:3514575d4f86 152
mjr 3:3514575d4f86 153 // reset the velocity
mjr 3:3514575d4f86 154 vx_ = vy_ = 0;
mjr 3:3514575d4f86 155
mjr 3:3514575d4f86 156 // get the time since the last get() sample
mjr 3:3514575d4f86 157 float dt = tGet_.read_us()/1.0e6;
mjr 3:3514575d4f86 158 tGet_.reset();
mjr 3:3514575d4f86 159
mjr 3:3514575d4f86 160 // done manipulating the shared data
mjr 3:3514575d4f86 161 __enable_irq();
mjr 3:3514575d4f86 162
mjr 3:3514575d4f86 163 // calculate the acceleration since the last get(): a = dv/dt
mjr 3:3514575d4f86 164 x = vx/dt;
mjr 3:3514575d4f86 165 y = vy/dt;
mjr 3:3514575d4f86 166
mjr 3:3514575d4f86 167 // return the raw accelerometer data in rx,ry
mjr 3:3514575d4f86 168 rx = xRaw;
mjr 3:3514575d4f86 169 ry = yRaw;
mjr 3:3514575d4f86 170 }
mjr 3:3514575d4f86 171
mjr 3:3514575d4f86 172 private:
mjr 3:3514575d4f86 173 // interrupt handler
mjr 3:3514575d4f86 174 void isr()
mjr 3:3514575d4f86 175 {
mjr 3:3514575d4f86 176 // Read the axes. Note that we have to read all three axes
mjr 3:3514575d4f86 177 // (even though we only really use x and y) in order to clear
mjr 3:3514575d4f86 178 // the "data ready" status bit in the accelerometer. The
mjr 3:3514575d4f86 179 // interrupt only occurs when the "ready" bit transitions from
mjr 3:3514575d4f86 180 // off to on, so we have to make sure it's off.
mjr 3:3514575d4f86 181 float z;
mjr 3:3514575d4f86 182 mma_.getAccXYZ(xRaw_, yRaw_, z);
mjr 3:3514575d4f86 183
mjr 3:3514575d4f86 184 // calculate the time since the last interrupt
mjr 3:3514575d4f86 185 float dt = tInt_.read_us()/1.0e6;
mjr 3:3514575d4f86 186 tInt_.reset();
mjr 3:3514575d4f86 187
mjr 3:3514575d4f86 188 // Accelerate the model ball: v = a*dt. Assume that the raw
mjr 3:3514575d4f86 189 // data from the accelerometer reflects the average physical
mjr 3:3514575d4f86 190 // acceleration over the interval since the last sample.
mjr 3:3514575d4f86 191 vx_ += xRaw_ * dt;
mjr 3:3514575d4f86 192 vy_ += yRaw_ * dt;
mjr 3:3514575d4f86 193 }
mjr 3:3514575d4f86 194
mjr 3:3514575d4f86 195 // current modeled ball velocity
mjr 3:3514575d4f86 196 float vx_, vy_;
mjr 3:3514575d4f86 197
mjr 3:3514575d4f86 198 // last raw axis readings
mjr 3:3514575d4f86 199 float xRaw_, yRaw_;
mjr 3:3514575d4f86 200
mjr 3:3514575d4f86 201 // underlying accelerometer object
mjr 3:3514575d4f86 202 MMA8451Q mma_;
mjr 3:3514575d4f86 203
mjr 3:3514575d4f86 204 // interrupt router
mjr 3:3514575d4f86 205 InterruptIn intIn_;
mjr 3:3514575d4f86 206
mjr 3:3514575d4f86 207 // timer for measuring time between get() samples
mjr 3:3514575d4f86 208 Timer tGet_;
mjr 3:3514575d4f86 209
mjr 3:3514575d4f86 210 // timer for measuring time between interrupts
mjr 3:3514575d4f86 211 Timer tInt_;
mjr 3:3514575d4f86 212 };
mjr 3:3514575d4f86 213
mjr 0:5acbbe3f4cf4 214 int main(void)
mjr 0:5acbbe3f4cf4 215 {
mjr 1:d913e0afb2ac 216 // turn off our on-board indicator LED
mjr 0:5acbbe3f4cf4 217 led1 = 1;
mjr 0:5acbbe3f4cf4 218 led2 = 1;
mjr 0:5acbbe3f4cf4 219 led3 = 1;
mjr 1:d913e0afb2ac 220
mjr 2:c174f9ee414a 221 // set up a flash memory controller
mjr 2:c174f9ee414a 222 FreescaleIAP iap;
mjr 2:c174f9ee414a 223
mjr 2:c174f9ee414a 224 // use the last sector of flash for our non-volatile memory structure
mjr 2:c174f9ee414a 225 int flash_addr = (iap.flash_size() - SECTOR_SIZE);
mjr 2:c174f9ee414a 226 NVM *flash = (NVM *)flash_addr;
mjr 2:c174f9ee414a 227 NVM cfg;
mjr 2:c174f9ee414a 228
mjr 2:c174f9ee414a 229 // check for valid flash
mjr 2:c174f9ee414a 230 bool flash_valid = (flash->d.sig == flash->SIGNATURE
mjr 2:c174f9ee414a 231 && flash->d.vsn == flash->VERSION
mjr 2:c174f9ee414a 232 && flash->checksum == CRC32(&flash->d, sizeof(flash->d)));
mjr 2:c174f9ee414a 233
mjr 2:c174f9ee414a 234 // Number of pixels we read from the sensor on each frame. This can be
mjr 2:c174f9ee414a 235 // less than the physical pixel count if desired; we'll read every nth
mjr 2:c174f9ee414a 236 // piexl if so. E.g., with a 1280-pixel physical sensor, if npix is 320,
mjr 2:c174f9ee414a 237 // we'll read every 4th pixel. VP doesn't seem to have very high
mjr 2:c174f9ee414a 238 // resolution internally for the plunger, so it's probably not necessary
mjr 2:c174f9ee414a 239 // to use the full resolution of the sensor - about 160 pixels seems
mjr 2:c174f9ee414a 240 // perfectly adequate. We can read the sensor faster (and thus provide
mjr 2:c174f9ee414a 241 // a higher refresh rate) if we read fewer pixels in each frame.
mjr 2:c174f9ee414a 242 const int npix = 160;
mjr 2:c174f9ee414a 243
mjr 2:c174f9ee414a 244 // if the flash is valid, load it; otherwise initialize to defaults
mjr 2:c174f9ee414a 245 if (flash_valid) {
mjr 2:c174f9ee414a 246 memcpy(&cfg, flash, sizeof(cfg));
mjr 2:c174f9ee414a 247 printf("Flash restored: plunger min=%d, max=%d\r\n",
mjr 2:c174f9ee414a 248 cfg.d.plungerMin, cfg.d.plungerMax);
mjr 2:c174f9ee414a 249 }
mjr 2:c174f9ee414a 250 else {
mjr 2:c174f9ee414a 251 printf("Factory reset\r\n");
mjr 2:c174f9ee414a 252 cfg.d.sig = cfg.SIGNATURE;
mjr 2:c174f9ee414a 253 cfg.d.vsn = cfg.VERSION;
mjr 2:c174f9ee414a 254 cfg.d.plungerMin = 0;
mjr 2:c174f9ee414a 255 cfg.d.plungerMax = npix;
mjr 2:c174f9ee414a 256 }
mjr 1:d913e0afb2ac 257
mjr 1:d913e0afb2ac 258 // plunger calibration button debounce timer
mjr 1:d913e0afb2ac 259 Timer calBtnTimer;
mjr 1:d913e0afb2ac 260 calBtnTimer.start();
mjr 1:d913e0afb2ac 261 int calBtnDownTime = 0;
mjr 1:d913e0afb2ac 262 int calBtnLit = false;
mjr 1:d913e0afb2ac 263
mjr 1:d913e0afb2ac 264 // Calibration button state:
mjr 1:d913e0afb2ac 265 // 0 = not pushed
mjr 1:d913e0afb2ac 266 // 1 = pushed, not yet debounced
mjr 1:d913e0afb2ac 267 // 2 = pushed, debounced, waiting for hold time
mjr 1:d913e0afb2ac 268 // 3 = pushed, hold time completed - in calibration mode
mjr 1:d913e0afb2ac 269 int calBtnState = 0;
mjr 1:d913e0afb2ac 270
mjr 1:d913e0afb2ac 271 // set up a timer for our heartbeat indicator
mjr 1:d913e0afb2ac 272 Timer hbTimer;
mjr 1:d913e0afb2ac 273 hbTimer.start();
mjr 1:d913e0afb2ac 274 int t0Hb = hbTimer.read_ms();
mjr 1:d913e0afb2ac 275 int hb = 0;
mjr 1:d913e0afb2ac 276
mjr 1:d913e0afb2ac 277 // set a timer for accelerometer auto-centering
mjr 1:d913e0afb2ac 278 Timer acTimer;
mjr 1:d913e0afb2ac 279 acTimer.start();
mjr 1:d913e0afb2ac 280 int t0ac = acTimer.read_ms();
mjr 1:d913e0afb2ac 281
mjr 1:d913e0afb2ac 282 // Create the joystick USB client. Light the on-board indicator LED
mjr 1:d913e0afb2ac 283 // red while connecting, and change to green after we connect.
mjr 2:c174f9ee414a 284 led1 = 0;
mjr 3:3514575d4f86 285 MyUSBJoystick js(0xFAFA, 0x00F7, 0x0003);
mjr 0:5acbbe3f4cf4 286 led1 = 1;
mjr 2:c174f9ee414a 287 led2 = 0;
mjr 2:c174f9ee414a 288
mjr 0:5acbbe3f4cf4 289 // create the accelerometer object
mjr 3:3514575d4f86 290 Accel accel(PTE25, PTE24, MMA8451_I2C_ADDRESS, PTA15);
mjr 0:5acbbe3f4cf4 291
mjr 0:5acbbe3f4cf4 292 // create the CCD array object
mjr 1:d913e0afb2ac 293 TSL1410R ccd(PTE20, PTE21, PTB0);
mjr 2:c174f9ee414a 294
mjr 1:d913e0afb2ac 295 // recent accelerometer readings, for auto centering
mjr 1:d913e0afb2ac 296 int iAccPrv = 0, nAccPrv = 0;
mjr 1:d913e0afb2ac 297 const int maxAccPrv = 5;
mjr 1:d913e0afb2ac 298 AccPrv accPrv[maxAccPrv];
mjr 0:5acbbe3f4cf4 299
mjr 1:d913e0afb2ac 300 // last accelerometer report, in mouse coordinates
mjr 1:d913e0afb2ac 301 int x = 127, y = 127, z = 0;
mjr 2:c174f9ee414a 302
mjr 1:d913e0afb2ac 303 // raw accelerator centerpoint, on the unit interval (-1.0 .. +1.0)
mjr 1:d913e0afb2ac 304 float xCenter = 0.0, yCenter = 0.0;
mjr 2:c174f9ee414a 305
mjr 2:c174f9ee414a 306 // start the first CCD integration cycle
mjr 2:c174f9ee414a 307 ccd.clear();
mjr 1:d913e0afb2ac 308
mjr 1:d913e0afb2ac 309 // we're all set up - now just loop, processing sensor reports and
mjr 1:d913e0afb2ac 310 // host requests
mjr 0:5acbbe3f4cf4 311 for (;;)
mjr 0:5acbbe3f4cf4 312 {
mjr 0:5acbbe3f4cf4 313 // Look for an incoming report. Continue processing input as
mjr 0:5acbbe3f4cf4 314 // long as there's anything pending - this ensures that we
mjr 0:5acbbe3f4cf4 315 // handle input in as timely a fashion as possible by deferring
mjr 0:5acbbe3f4cf4 316 // output tasks as long as there's input to process.
mjr 0:5acbbe3f4cf4 317 HID_REPORT report;
mjr 0:5acbbe3f4cf4 318 while (js.readNB(&report) && report.length == 8)
mjr 0:5acbbe3f4cf4 319 {
mjr 0:5acbbe3f4cf4 320 uint8_t *data = report.data;
mjr 1:d913e0afb2ac 321 if (data[0] == 64)
mjr 1:d913e0afb2ac 322 {
mjr 0:5acbbe3f4cf4 323 // LWZ-SBA - first four bytes are bit-packed on/off flags
mjr 0:5acbbe3f4cf4 324 // for the outputs; 5th byte is the pulse speed (0-7)
mjr 0:5acbbe3f4cf4 325 //printf("LWZ-SBA %02x %02x %02x %02x ; %02x\r\n",
mjr 0:5acbbe3f4cf4 326 // data[1], data[2], data[3], data[4], data[5]);
mjr 0:5acbbe3f4cf4 327
mjr 0:5acbbe3f4cf4 328 // update all on/off states
mjr 0:5acbbe3f4cf4 329 for (int i = 0, bit = 1, ri = 1 ; i < 32 ; ++i, bit <<= 1)
mjr 0:5acbbe3f4cf4 330 {
mjr 0:5acbbe3f4cf4 331 if (bit == 0x100) {
mjr 0:5acbbe3f4cf4 332 bit = 1;
mjr 0:5acbbe3f4cf4 333 ++ri;
mjr 0:5acbbe3f4cf4 334 }
mjr 1:d913e0afb2ac 335 wizOn[i] = ((data[ri] & bit) != 0);
mjr 0:5acbbe3f4cf4 336 }
mjr 0:5acbbe3f4cf4 337
mjr 1:d913e0afb2ac 338 // update the physical outputs
mjr 1:d913e0afb2ac 339 updateWizOuts();
mjr 0:5acbbe3f4cf4 340
mjr 0:5acbbe3f4cf4 341 // reset the PBA counter
mjr 0:5acbbe3f4cf4 342 pbaIdx = 0;
mjr 0:5acbbe3f4cf4 343 }
mjr 1:d913e0afb2ac 344 else
mjr 1:d913e0afb2ac 345 {
mjr 0:5acbbe3f4cf4 346 // LWZ-PBA - full state dump; each byte is one output
mjr 0:5acbbe3f4cf4 347 // in the current bank. pbaIdx keeps track of the bank;
mjr 0:5acbbe3f4cf4 348 // this is incremented implicitly by each PBA message.
mjr 0:5acbbe3f4cf4 349 //printf("LWZ-PBA[%d] %02x %02x %02x %02x %02x %02x %02x %02x\r\n",
mjr 0:5acbbe3f4cf4 350 // pbaIdx, data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7]);
mjr 0:5acbbe3f4cf4 351
mjr 0:5acbbe3f4cf4 352 // update all output profile settings
mjr 0:5acbbe3f4cf4 353 for (int i = 0 ; i < 8 ; ++i)
mjr 1:d913e0afb2ac 354 wizVal[pbaIdx + i] = data[i];
mjr 0:5acbbe3f4cf4 355
mjr 0:5acbbe3f4cf4 356 // update the physical LED state if this is the last bank
mjr 0:5acbbe3f4cf4 357 if (pbaIdx == 24)
mjr 1:d913e0afb2ac 358 updateWizOuts();
mjr 0:5acbbe3f4cf4 359
mjr 0:5acbbe3f4cf4 360 // advance to the next bank
mjr 0:5acbbe3f4cf4 361 pbaIdx = (pbaIdx + 8) & 31;
mjr 0:5acbbe3f4cf4 362 }
mjr 0:5acbbe3f4cf4 363 }
mjr 1:d913e0afb2ac 364
mjr 1:d913e0afb2ac 365 // check for plunger calibration
mjr 1:d913e0afb2ac 366 if (!calBtn)
mjr 0:5acbbe3f4cf4 367 {
mjr 1:d913e0afb2ac 368 // check the state
mjr 1:d913e0afb2ac 369 switch (calBtnState)
mjr 0:5acbbe3f4cf4 370 {
mjr 1:d913e0afb2ac 371 case 0:
mjr 1:d913e0afb2ac 372 // button not yet pushed - start debouncing
mjr 1:d913e0afb2ac 373 calBtnTimer.reset();
mjr 1:d913e0afb2ac 374 calBtnDownTime = calBtnTimer.read_ms();
mjr 1:d913e0afb2ac 375 calBtnState = 1;
mjr 1:d913e0afb2ac 376 break;
mjr 1:d913e0afb2ac 377
mjr 1:d913e0afb2ac 378 case 1:
mjr 1:d913e0afb2ac 379 // pushed, not yet debounced - if the debounce time has
mjr 1:d913e0afb2ac 380 // passed, start the hold period
mjr 1:d913e0afb2ac 381 if (calBtnTimer.read_ms() - calBtnDownTime > 50)
mjr 1:d913e0afb2ac 382 calBtnState = 2;
mjr 1:d913e0afb2ac 383 break;
mjr 1:d913e0afb2ac 384
mjr 1:d913e0afb2ac 385 case 2:
mjr 1:d913e0afb2ac 386 // in the hold period - if the button has been held down
mjr 1:d913e0afb2ac 387 // for the entire hold period, move to calibration mode
mjr 1:d913e0afb2ac 388 if (calBtnTimer.read_ms() - calBtnDownTime > 2050)
mjr 1:d913e0afb2ac 389 {
mjr 1:d913e0afb2ac 390 // enter calibration mode
mjr 1:d913e0afb2ac 391 calBtnState = 3;
mjr 1:d913e0afb2ac 392
mjr 1:d913e0afb2ac 393 // reset the calibration limits
mjr 2:c174f9ee414a 394 cfg.d.plungerMax = 0;
mjr 2:c174f9ee414a 395 cfg.d.plungerMin = npix;
mjr 1:d913e0afb2ac 396 }
mjr 1:d913e0afb2ac 397 break;
mjr 2:c174f9ee414a 398
mjr 2:c174f9ee414a 399 case 3:
mjr 2:c174f9ee414a 400 // Already in calibration mode - pushing the button in this
mjr 2:c174f9ee414a 401 // state doesn't change the current state, but we won't leave
mjr 2:c174f9ee414a 402 // this state as long as it's held down. We can simply do
mjr 2:c174f9ee414a 403 // nothing here.
mjr 2:c174f9ee414a 404 break;
mjr 0:5acbbe3f4cf4 405 }
mjr 0:5acbbe3f4cf4 406 }
mjr 1:d913e0afb2ac 407 else
mjr 1:d913e0afb2ac 408 {
mjr 2:c174f9ee414a 409 // Button released. If we're in calibration mode, and
mjr 2:c174f9ee414a 410 // the calibration time has elapsed, end the calibration
mjr 2:c174f9ee414a 411 // and save the results to flash.
mjr 2:c174f9ee414a 412 //
mjr 2:c174f9ee414a 413 // Otherwise, return to the base state without saving anything.
mjr 2:c174f9ee414a 414 // If the button is released before we make it to calibration
mjr 2:c174f9ee414a 415 // mode, it simply cancels the attempt.
mjr 2:c174f9ee414a 416 if (calBtnState == 3
mjr 2:c174f9ee414a 417 && calBtnTimer.read_ms() - calBtnDownTime > 17500)
mjr 2:c174f9ee414a 418 {
mjr 2:c174f9ee414a 419 // exit calibration mode
mjr 1:d913e0afb2ac 420 calBtnState = 0;
mjr 2:c174f9ee414a 421
mjr 2:c174f9ee414a 422 // Save the current configuration state to flash, so that it
mjr 2:c174f9ee414a 423 // will be preserved through power off. Update the checksum
mjr 2:c174f9ee414a 424 // first so that we recognize the flash record as valid.
mjr 2:c174f9ee414a 425 cfg.checksum = CRC32(&cfg.d, sizeof(cfg.d));
mjr 2:c174f9ee414a 426 iap.erase_sector(flash_addr);
mjr 2:c174f9ee414a 427 iap.program_flash(flash_addr, &cfg, sizeof(cfg));
mjr 2:c174f9ee414a 428
mjr 2:c174f9ee414a 429 // the flash state is now valid
mjr 2:c174f9ee414a 430 flash_valid = true;
mjr 2:c174f9ee414a 431 }
mjr 2:c174f9ee414a 432 else if (calBtnState != 3)
mjr 2:c174f9ee414a 433 {
mjr 2:c174f9ee414a 434 // didn't make it to calibration mode - cancel the operation
mjr 1:d913e0afb2ac 435 calBtnState = 0;
mjr 2:c174f9ee414a 436 }
mjr 1:d913e0afb2ac 437 }
mjr 1:d913e0afb2ac 438
mjr 1:d913e0afb2ac 439 // light/flash the calibration button light, if applicable
mjr 1:d913e0afb2ac 440 int newCalBtnLit = calBtnLit;
mjr 1:d913e0afb2ac 441 switch (calBtnState)
mjr 0:5acbbe3f4cf4 442 {
mjr 1:d913e0afb2ac 443 case 2:
mjr 1:d913e0afb2ac 444 // in the hold period - flash the light
mjr 1:d913e0afb2ac 445 newCalBtnLit = (((calBtnTimer.read_ms() - calBtnDownTime)/250) & 1);
mjr 1:d913e0afb2ac 446 break;
mjr 1:d913e0afb2ac 447
mjr 1:d913e0afb2ac 448 case 3:
mjr 1:d913e0afb2ac 449 // calibration mode - show steady on
mjr 1:d913e0afb2ac 450 newCalBtnLit = true;
mjr 1:d913e0afb2ac 451 break;
mjr 1:d913e0afb2ac 452
mjr 1:d913e0afb2ac 453 default:
mjr 1:d913e0afb2ac 454 // not calibrating/holding - show steady off
mjr 1:d913e0afb2ac 455 newCalBtnLit = false;
mjr 1:d913e0afb2ac 456 break;
mjr 1:d913e0afb2ac 457 }
mjr 3:3514575d4f86 458
mjr 3:3514575d4f86 459 // light or flash the external calibration button LED, and
mjr 3:3514575d4f86 460 // do the same with the on-board blue LED
mjr 1:d913e0afb2ac 461 if (calBtnLit != newCalBtnLit)
mjr 1:d913e0afb2ac 462 {
mjr 1:d913e0afb2ac 463 calBtnLit = newCalBtnLit;
mjr 2:c174f9ee414a 464 if (calBtnLit) {
mjr 2:c174f9ee414a 465 calBtnLed = 1;
mjr 3:3514575d4f86 466 led1 = 1;
mjr 3:3514575d4f86 467 led2 = 1;
mjr 2:c174f9ee414a 468 led3 = 1;
mjr 2:c174f9ee414a 469 }
mjr 2:c174f9ee414a 470 else {
mjr 2:c174f9ee414a 471 calBtnLed = 0;
mjr 2:c174f9ee414a 472 led1 = 1;
mjr 2:c174f9ee414a 473 led2 = 1;
mjr 2:c174f9ee414a 474 led3 = 0;
mjr 2:c174f9ee414a 475 }
mjr 1:d913e0afb2ac 476 }
mjr 1:d913e0afb2ac 477
mjr 1:d913e0afb2ac 478 // read the plunger sensor
mjr 1:d913e0afb2ac 479 int znew = z;
mjr 2:c174f9ee414a 480 uint16_t pix[npix];
mjr 2:c174f9ee414a 481 ccd.read(pix, npix);
mjr 2:c174f9ee414a 482
mjr 2:c174f9ee414a 483 // get the average brightness at each end of the sensor
mjr 2:c174f9ee414a 484 long avg1 = (long(pix[0]) + long(pix[1]) + long(pix[2]) + long(pix[3]) + long(pix[4]))/5;
mjr 2:c174f9ee414a 485 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 486
mjr 2:c174f9ee414a 487 // figure the midpoint in the brightness; multiply by 3 so that we can
mjr 2:c174f9ee414a 488 // compare sums of three pixels at a time to smooth out noise
mjr 2:c174f9ee414a 489 long midpt = (avg1 + avg2)/2 * 3;
mjr 2:c174f9ee414a 490
mjr 2:c174f9ee414a 491 // Work from the bright end to the dark end. VP interprets the
mjr 2:c174f9ee414a 492 // Z axis value as the amount the plunger is pulled: the minimum
mjr 2:c174f9ee414a 493 // is the rest position, the maximum is fully pulled. So we
mjr 2:c174f9ee414a 494 // essentially want to report how much of the sensor is lit,
mjr 2:c174f9ee414a 495 // since this increases as the plunger is pulled back.
mjr 2:c174f9ee414a 496 int si = 1, di = 1;
mjr 2:c174f9ee414a 497 if (avg1 < avg2)
mjr 2:c174f9ee414a 498 si = npix - 2, di = -1;
mjr 2:c174f9ee414a 499
mjr 2:c174f9ee414a 500 // scan for the midpoint
mjr 2:c174f9ee414a 501 uint16_t *pixp = pix + si;
mjr 2:c174f9ee414a 502 for (int n = 1 ; n < npix - 1 ; ++n, pixp += di)
mjr 1:d913e0afb2ac 503 {
mjr 2:c174f9ee414a 504 // if we've crossed the midpoint, report this position
mjr 2:c174f9ee414a 505 if (long(pixp[-1]) + long(pixp[0]) + long(pixp[1]) < midpt)
mjr 1:d913e0afb2ac 506 {
mjr 2:c174f9ee414a 507 // note the new position
mjr 2:c174f9ee414a 508 int pos = n;
mjr 2:c174f9ee414a 509
mjr 2:c174f9ee414a 510 // if the bright end and dark end don't differ by enough, skip this
mjr 2:c174f9ee414a 511 // reading entirely - we must have an overexposed or underexposed frame
mjr 2:c174f9ee414a 512 if (labs(avg1 - avg2) < 0x3333)
mjr 2:c174f9ee414a 513 break;
mjr 2:c174f9ee414a 514
mjr 2:c174f9ee414a 515 // Calibrate, or apply calibration, depending on the mode.
mjr 2:c174f9ee414a 516 // In either case, normalize to a 0-127 range. VP appears to
mjr 2:c174f9ee414a 517 // ignore negative Z axis values.
mjr 2:c174f9ee414a 518 if (calBtnState == 3)
mjr 1:d913e0afb2ac 519 {
mjr 2:c174f9ee414a 520 // calibrating - note if we're expanding the calibration envelope
mjr 2:c174f9ee414a 521 if (pos < cfg.d.plungerMin)
mjr 2:c174f9ee414a 522 cfg.d.plungerMin = pos;
mjr 2:c174f9ee414a 523 if (pos > cfg.d.plungerMax)
mjr 2:c174f9ee414a 524 cfg.d.plungerMax = pos;
mjr 2:c174f9ee414a 525
mjr 2:c174f9ee414a 526 // normalize to the full physical range while calibrating
mjr 2:c174f9ee414a 527 znew = int(float(pos)/npix * 127);
mjr 1:d913e0afb2ac 528 }
mjr 2:c174f9ee414a 529 else
mjr 2:c174f9ee414a 530 {
mjr 2:c174f9ee414a 531 // running normally - normalize to the calibration range
mjr 2:c174f9ee414a 532 if (pos < cfg.d.plungerMin)
mjr 2:c174f9ee414a 533 pos = cfg.d.plungerMin;
mjr 2:c174f9ee414a 534 if (pos > cfg.d.plungerMax)
mjr 2:c174f9ee414a 535 pos = cfg.d.plungerMax;
mjr 2:c174f9ee414a 536 znew = int(float(pos - cfg.d.plungerMin)
mjr 2:c174f9ee414a 537 / (cfg.d.plungerMax - cfg.d.plungerMin + 1) * 127);
mjr 2:c174f9ee414a 538 }
mjr 2:c174f9ee414a 539
mjr 2:c174f9ee414a 540 // done
mjr 2:c174f9ee414a 541 break;
mjr 1:d913e0afb2ac 542 }
mjr 2:c174f9ee414a 543 }
mjr 1:d913e0afb2ac 544
mjr 1:d913e0afb2ac 545 // read the accelerometer
mjr 3:3514575d4f86 546 float xa, ya, rxa, rya;
mjr 3:3514575d4f86 547 accel.get(xa, ya, rxa, rya);
mjr 1:d913e0afb2ac 548
mjr 1:d913e0afb2ac 549 // check for auto-centering every so often
mjr 1:d913e0afb2ac 550 if (acTimer.read_ms() - t0ac > 1000)
mjr 1:d913e0afb2ac 551 {
mjr 1:d913e0afb2ac 552 // add the sample to the history list
mjr 1:d913e0afb2ac 553 accPrv[iAccPrv].x = xa;
mjr 1:d913e0afb2ac 554 accPrv[iAccPrv].y = ya;
mjr 1:d913e0afb2ac 555
mjr 1:d913e0afb2ac 556 // store the slot
mjr 1:d913e0afb2ac 557 iAccPrv += 1;
mjr 1:d913e0afb2ac 558 iAccPrv %= maxAccPrv;
mjr 1:d913e0afb2ac 559 nAccPrv += 1;
mjr 1:d913e0afb2ac 560
mjr 1:d913e0afb2ac 561 // If we have a full complement, check for stability. The
mjr 1:d913e0afb2ac 562 // raw accelerometer input is in the rnage -4096 to 4096, but
mjr 1:d913e0afb2ac 563 // the class cover normalizes to a unit interval (-1.0 .. +1.0).
mjr 1:d913e0afb2ac 564 const float accTol = .005;
mjr 1:d913e0afb2ac 565 if (nAccPrv >= maxAccPrv
mjr 1:d913e0afb2ac 566 && accPrv[0].dist(accPrv[1]) < accTol
mjr 1:d913e0afb2ac 567 && accPrv[0].dist(accPrv[2]) < accTol
mjr 1:d913e0afb2ac 568 && accPrv[0].dist(accPrv[3]) < accTol
mjr 1:d913e0afb2ac 569 && accPrv[0].dist(accPrv[4]) < accTol)
mjr 1:d913e0afb2ac 570 {
mjr 1:d913e0afb2ac 571 // figure the new center
mjr 1:d913e0afb2ac 572 xCenter = (accPrv[0].x + accPrv[1].x + accPrv[2].x + accPrv[3].x + accPrv[4].x)/5.0;
mjr 1:d913e0afb2ac 573 yCenter = (accPrv[0].y + accPrv[1].y + accPrv[2].y + accPrv[3].y + accPrv[4].y)/5.0;
mjr 1:d913e0afb2ac 574 }
mjr 1:d913e0afb2ac 575
mjr 1:d913e0afb2ac 576 // reset the auto-center timer
mjr 1:d913e0afb2ac 577 acTimer.reset();
mjr 1:d913e0afb2ac 578 t0ac = acTimer.read_ms();
mjr 1:d913e0afb2ac 579 }
mjr 1:d913e0afb2ac 580
mjr 1:d913e0afb2ac 581 // adjust for our auto centering
mjr 1:d913e0afb2ac 582 xa -= xCenter;
mjr 1:d913e0afb2ac 583 ya -= yCenter;
mjr 1:d913e0afb2ac 584
mjr 1:d913e0afb2ac 585 // confine to the unit interval
mjr 1:d913e0afb2ac 586 if (xa < -1.0) xa = -1.0;
mjr 1:d913e0afb2ac 587 if (xa > 1.0) xa = 1.0;
mjr 1:d913e0afb2ac 588 if (ya < -1.0) ya = -1.0;
mjr 1:d913e0afb2ac 589 if (ya > 1.0) ya = 1.0;
mjr 0:5acbbe3f4cf4 590
mjr 1:d913e0afb2ac 591 // figure the new mouse report data
mjr 1:d913e0afb2ac 592 int xnew = (int)(127 * xa);
mjr 1:d913e0afb2ac 593 int ynew = (int)(127 * ya);
mjr 2:c174f9ee414a 594
mjr 2:c174f9ee414a 595 // store the updated joystick coordinates
mjr 2:c174f9ee414a 596 x = xnew;
mjr 2:c174f9ee414a 597 y = ynew;
mjr 2:c174f9ee414a 598 z = znew;
mjr 1:d913e0afb2ac 599
mjr 2:c174f9ee414a 600 // if we're in USB suspend or disconnect mode, spin
mjr 2:c174f9ee414a 601 if (js.isSuspended() || !js.isConnected())
mjr 0:5acbbe3f4cf4 602 {
mjr 2:c174f9ee414a 603 // go dark (turn off the indicator LEDs)
mjr 2:c174f9ee414a 604 led2 = 1;
mjr 2:c174f9ee414a 605 led3 = 1;
mjr 2:c174f9ee414a 606 led1 = 1;
mjr 2:c174f9ee414a 607
mjr 2:c174f9ee414a 608 // wait until we're connected and come out of suspend mode
mjr 2:c174f9ee414a 609 while (js.isSuspended() || !js.isConnected())
mjr 2:c174f9ee414a 610 {
mjr 2:c174f9ee414a 611 // spin for a bit
mjr 2:c174f9ee414a 612 wait(1);
mjr 2:c174f9ee414a 613
mjr 2:c174f9ee414a 614 // if we're not suspended, flash red; otherwise stay dark
mjr 2:c174f9ee414a 615 if (!js.isSuspended())
mjr 2:c174f9ee414a 616 led1 = !led1;
mjr 2:c174f9ee414a 617 }
mjr 2:c174f9ee414a 618 }
mjr 1:d913e0afb2ac 619
mjr 3:3514575d4f86 620 // Send the status report. It doesn't really matter what
mjr 3:3514575d4f86 621 // coordinate system we use, since Visual Pinball has config
mjr 3:3514575d4f86 622 // options for rotations and axis reversals, but reversing y
mjr 3:3514575d4f86 623 // at the device level seems to produce the most intuitive
mjr 3:3514575d4f86 624 // results for the Windows joystick control panel view, which
mjr 3:3514575d4f86 625 // is an easy way to check that the device is working.
mjr 3:3514575d4f86 626 js.update(x, -y, z, int(rxa*127), int(rya*127), 0);
mjr 1:d913e0afb2ac 627
mjr 2:c174f9ee414a 628 // show a heartbeat flash in blue every so often if not in
mjr 2:c174f9ee414a 629 // calibration mode
mjr 2:c174f9ee414a 630 if (calBtnState < 2 && hbTimer.read_ms() - t0Hb > 1000)
mjr 1:d913e0afb2ac 631 {
mjr 2:c174f9ee414a 632 if (js.isSuspended())
mjr 2:c174f9ee414a 633 {
mjr 2:c174f9ee414a 634 // suspended - turn off the LEDs entirely
mjr 2:c174f9ee414a 635 led1 = 1;
mjr 2:c174f9ee414a 636 led2 = 1;
mjr 2:c174f9ee414a 637 led3 = 1;
mjr 2:c174f9ee414a 638 }
mjr 2:c174f9ee414a 639 else if (!js.isConnected())
mjr 2:c174f9ee414a 640 {
mjr 2:c174f9ee414a 641 // not connected - flash red
mjr 2:c174f9ee414a 642 hb = !hb;
mjr 2:c174f9ee414a 643 led1 = (hb ? 0 : 1);
mjr 2:c174f9ee414a 644 led2 = 1;
mjr 2:c174f9ee414a 645 led3 = 1;
mjr 2:c174f9ee414a 646 }
mjr 2:c174f9ee414a 647 else if (flash_valid)
mjr 2:c174f9ee414a 648 {
mjr 2:c174f9ee414a 649 // connected, NVM valid - flash blue/green
mjr 2:c174f9ee414a 650 hb = !hb;
mjr 2:c174f9ee414a 651 led1 = 1;
mjr 2:c174f9ee414a 652 led2 = (hb ? 0 : 1);
mjr 2:c174f9ee414a 653 led3 = (hb ? 1 : 0);
mjr 2:c174f9ee414a 654 }
mjr 2:c174f9ee414a 655 else
mjr 2:c174f9ee414a 656 {
mjr 2:c174f9ee414a 657 // connected, factory reset - flash yellow/green
mjr 2:c174f9ee414a 658 hb = !hb;
mjr 2:c174f9ee414a 659 led1 = (hb ? 0 : 1);
mjr 2:c174f9ee414a 660 led2 = 0;
mjr 3:3514575d4f86 661 led3 = 1;
mjr 2:c174f9ee414a 662 }
mjr 1:d913e0afb2ac 663
mjr 1:d913e0afb2ac 664 // reset the heartbeat timer
mjr 1:d913e0afb2ac 665 hbTimer.reset();
mjr 1:d913e0afb2ac 666 t0Hb = hbTimer.read_ms();
mjr 1:d913e0afb2ac 667 }
mjr 1:d913e0afb2ac 668 }
mjr 0:5acbbe3f4cf4 669 }