Arnaud VALLEY / Mbed 2 deprecated Pinscape_Controller_V2_arnoz

Mirror with some correction

Dependencies:   mbed FastIO FastPWM USBDevice

main.cpp@4:02c7cd7b2183, 2014-07-24 (annotated)

Committer:
mjr
Date:
Thu Jul 24 05:50:36 2014 +0000
Revision:
4:02c7cd7b2183
Parent:
3:3514575d4f86
Child:
5:a70c0bce770d
USB 3 connection problems fixed. Host power status reflected in diagnostic LEDs. Non-blocking initial connection.

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 4:02c7cd7b2183 13 : USBJoystick(vendor_id, product_id, product_release, false)
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 4:02c7cd7b2183 28 // On-board RGB LED elements - we use these for diagnostic displays.
mjr 4:02c7cd7b2183 29 DigitalOut ledR(LED1), ledG(LED2), ledB(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 4:02c7cd7b2183 68 ledR = wizState(0);
mjr 4:02c7cd7b2183 69 ledG = wizState(1);
mjr 4:02c7cd7b2183 70 ledB = 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 4:02c7cd7b2183 217 ledR = 1;
mjr 4:02c7cd7b2183 218 ledG = 1;
mjr 4:02c7cd7b2183 219 ledB = 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 4:02c7cd7b2183 282 // Create the joystick USB client
mjr 3:3514575d4f86 283 MyUSBJoystick js(0xFAFA, 0x00F7, 0x0003);
mjr 2:c174f9ee414a 284
mjr 0:5acbbe3f4cf4 285 // create the accelerometer object
mjr 3:3514575d4f86 286 Accel accel(PTE25, PTE24, MMA8451_I2C_ADDRESS, PTA15);
mjr 0:5acbbe3f4cf4 287
mjr 0:5acbbe3f4cf4 288 // create the CCD array object
mjr 1:d913e0afb2ac 289 TSL1410R ccd(PTE20, PTE21, PTB0);
mjr 2:c174f9ee414a 290
mjr 1:d913e0afb2ac 291 // recent accelerometer readings, for auto centering
mjr 1:d913e0afb2ac 292 int iAccPrv = 0, nAccPrv = 0;
mjr 1:d913e0afb2ac 293 const int maxAccPrv = 5;
mjr 1:d913e0afb2ac 294 AccPrv accPrv[maxAccPrv];
mjr 0:5acbbe3f4cf4 295
mjr 1:d913e0afb2ac 296 // last accelerometer report, in mouse coordinates
mjr 1:d913e0afb2ac 297 int x = 127, y = 127, z = 0;
mjr 2:c174f9ee414a 298
mjr 1:d913e0afb2ac 299 // raw accelerator centerpoint, on the unit interval (-1.0 .. +1.0)
mjr 1:d913e0afb2ac 300 float xCenter = 0.0, yCenter = 0.0;
mjr 2:c174f9ee414a 301
mjr 2:c174f9ee414a 302 // start the first CCD integration cycle
mjr 2:c174f9ee414a 303 ccd.clear();
mjr 1:d913e0afb2ac 304
mjr 1:d913e0afb2ac 305 // we're all set up - now just loop, processing sensor reports and
mjr 1:d913e0afb2ac 306 // host requests
mjr 0:5acbbe3f4cf4 307 for (;;)
mjr 0:5acbbe3f4cf4 308 {
mjr 0:5acbbe3f4cf4 309 // Look for an incoming report. Continue processing input as
mjr 0:5acbbe3f4cf4 310 // long as there's anything pending - this ensures that we
mjr 0:5acbbe3f4cf4 311 // handle input in as timely a fashion as possible by deferring
mjr 0:5acbbe3f4cf4 312 // output tasks as long as there's input to process.
mjr 0:5acbbe3f4cf4 313 HID_REPORT report;
mjr 0:5acbbe3f4cf4 314 while (js.readNB(&report) && report.length == 8)
mjr 0:5acbbe3f4cf4 315 {
mjr 0:5acbbe3f4cf4 316 uint8_t *data = report.data;
mjr 1:d913e0afb2ac 317 if (data[0] == 64)
mjr 1:d913e0afb2ac 318 {
mjr 0:5acbbe3f4cf4 319 // LWZ-SBA - first four bytes are bit-packed on/off flags
mjr 0:5acbbe3f4cf4 320 // for the outputs; 5th byte is the pulse speed (0-7)
mjr 0:5acbbe3f4cf4 321 //printf("LWZ-SBA %02x %02x %02x %02x ; %02x\r\n",
mjr 0:5acbbe3f4cf4 322 // data[1], data[2], data[3], data[4], data[5]);
mjr 0:5acbbe3f4cf4 323
mjr 0:5acbbe3f4cf4 324 // update all on/off states
mjr 0:5acbbe3f4cf4 325 for (int i = 0, bit = 1, ri = 1 ; i < 32 ; ++i, bit <<= 1)
mjr 0:5acbbe3f4cf4 326 {
mjr 0:5acbbe3f4cf4 327 if (bit == 0x100) {
mjr 0:5acbbe3f4cf4 328 bit = 1;
mjr 0:5acbbe3f4cf4 329 ++ri;
mjr 0:5acbbe3f4cf4 330 }
mjr 1:d913e0afb2ac 331 wizOn[i] = ((data[ri] & bit) != 0);
mjr 0:5acbbe3f4cf4 332 }
mjr 0:5acbbe3f4cf4 333
mjr 1:d913e0afb2ac 334 // update the physical outputs
mjr 1:d913e0afb2ac 335 updateWizOuts();
mjr 0:5acbbe3f4cf4 336
mjr 0:5acbbe3f4cf4 337 // reset the PBA counter
mjr 0:5acbbe3f4cf4 338 pbaIdx = 0;
mjr 0:5acbbe3f4cf4 339 }
mjr 1:d913e0afb2ac 340 else
mjr 1:d913e0afb2ac 341 {
mjr 0:5acbbe3f4cf4 342 // LWZ-PBA - full state dump; each byte is one output
mjr 0:5acbbe3f4cf4 343 // in the current bank. pbaIdx keeps track of the bank;
mjr 0:5acbbe3f4cf4 344 // this is incremented implicitly by each PBA message.
mjr 0:5acbbe3f4cf4 345 //printf("LWZ-PBA[%d] %02x %02x %02x %02x %02x %02x %02x %02x\r\n",
mjr 0:5acbbe3f4cf4 346 // pbaIdx, data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7]);
mjr 0:5acbbe3f4cf4 347
mjr 0:5acbbe3f4cf4 348 // update all output profile settings
mjr 0:5acbbe3f4cf4 349 for (int i = 0 ; i < 8 ; ++i)
mjr 1:d913e0afb2ac 350 wizVal[pbaIdx + i] = data[i];
mjr 0:5acbbe3f4cf4 351
mjr 0:5acbbe3f4cf4 352 // update the physical LED state if this is the last bank
mjr 0:5acbbe3f4cf4 353 if (pbaIdx == 24)
mjr 1:d913e0afb2ac 354 updateWizOuts();
mjr 0:5acbbe3f4cf4 355
mjr 0:5acbbe3f4cf4 356 // advance to the next bank
mjr 0:5acbbe3f4cf4 357 pbaIdx = (pbaIdx + 8) & 31;
mjr 0:5acbbe3f4cf4 358 }
mjr 0:5acbbe3f4cf4 359 }
mjr 1:d913e0afb2ac 360
mjr 1:d913e0afb2ac 361 // check for plunger calibration
mjr 1:d913e0afb2ac 362 if (!calBtn)
mjr 0:5acbbe3f4cf4 363 {
mjr 1:d913e0afb2ac 364 // check the state
mjr 1:d913e0afb2ac 365 switch (calBtnState)
mjr 0:5acbbe3f4cf4 366 {
mjr 1:d913e0afb2ac 367 case 0:
mjr 1:d913e0afb2ac 368 // button not yet pushed - start debouncing
mjr 1:d913e0afb2ac 369 calBtnTimer.reset();
mjr 1:d913e0afb2ac 370 calBtnDownTime = calBtnTimer.read_ms();
mjr 1:d913e0afb2ac 371 calBtnState = 1;
mjr 1:d913e0afb2ac 372 break;
mjr 1:d913e0afb2ac 373
mjr 1:d913e0afb2ac 374 case 1:
mjr 1:d913e0afb2ac 375 // pushed, not yet debounced - if the debounce time has
mjr 1:d913e0afb2ac 376 // passed, start the hold period
mjr 1:d913e0afb2ac 377 if (calBtnTimer.read_ms() - calBtnDownTime > 50)
mjr 1:d913e0afb2ac 378 calBtnState = 2;
mjr 1:d913e0afb2ac 379 break;
mjr 1:d913e0afb2ac 380
mjr 1:d913e0afb2ac 381 case 2:
mjr 1:d913e0afb2ac 382 // in the hold period - if the button has been held down
mjr 1:d913e0afb2ac 383 // for the entire hold period, move to calibration mode
mjr 1:d913e0afb2ac 384 if (calBtnTimer.read_ms() - calBtnDownTime > 2050)
mjr 1:d913e0afb2ac 385 {
mjr 1:d913e0afb2ac 386 // enter calibration mode
mjr 1:d913e0afb2ac 387 calBtnState = 3;
mjr 1:d913e0afb2ac 388
mjr 1:d913e0afb2ac 389 // reset the calibration limits
mjr 2:c174f9ee414a 390 cfg.d.plungerMax = 0;
mjr 2:c174f9ee414a 391 cfg.d.plungerMin = npix;
mjr 1:d913e0afb2ac 392 }
mjr 1:d913e0afb2ac 393 break;
mjr 2:c174f9ee414a 394
mjr 2:c174f9ee414a 395 case 3:
mjr 2:c174f9ee414a 396 // Already in calibration mode - pushing the button in this
mjr 2:c174f9ee414a 397 // state doesn't change the current state, but we won't leave
mjr 2:c174f9ee414a 398 // this state as long as it's held down. We can simply do
mjr 2:c174f9ee414a 399 // nothing here.
mjr 2:c174f9ee414a 400 break;
mjr 0:5acbbe3f4cf4 401 }
mjr 0:5acbbe3f4cf4 402 }
mjr 1:d913e0afb2ac 403 else
mjr 1:d913e0afb2ac 404 {
mjr 2:c174f9ee414a 405 // Button released. If we're in calibration mode, and
mjr 2:c174f9ee414a 406 // the calibration time has elapsed, end the calibration
mjr 2:c174f9ee414a 407 // and save the results to flash.
mjr 2:c174f9ee414a 408 //
mjr 2:c174f9ee414a 409 // Otherwise, return to the base state without saving anything.
mjr 2:c174f9ee414a 410 // If the button is released before we make it to calibration
mjr 2:c174f9ee414a 411 // mode, it simply cancels the attempt.
mjr 2:c174f9ee414a 412 if (calBtnState == 3
mjr 2:c174f9ee414a 413 && calBtnTimer.read_ms() - calBtnDownTime > 17500)
mjr 2:c174f9ee414a 414 {
mjr 2:c174f9ee414a 415 // exit calibration mode
mjr 1:d913e0afb2ac 416 calBtnState = 0;
mjr 2:c174f9ee414a 417
mjr 2:c174f9ee414a 418 // Save the current configuration state to flash, so that it
mjr 2:c174f9ee414a 419 // will be preserved through power off. Update the checksum
mjr 2:c174f9ee414a 420 // first so that we recognize the flash record as valid.
mjr 2:c174f9ee414a 421 cfg.checksum = CRC32(&cfg.d, sizeof(cfg.d));
mjr 2:c174f9ee414a 422 iap.erase_sector(flash_addr);
mjr 2:c174f9ee414a 423 iap.program_flash(flash_addr, &cfg, sizeof(cfg));
mjr 2:c174f9ee414a 424
mjr 2:c174f9ee414a 425 // the flash state is now valid
mjr 2:c174f9ee414a 426 flash_valid = true;
mjr 2:c174f9ee414a 427 }
mjr 2:c174f9ee414a 428 else if (calBtnState != 3)
mjr 2:c174f9ee414a 429 {
mjr 2:c174f9ee414a 430 // didn't make it to calibration mode - cancel the operation
mjr 1:d913e0afb2ac 431 calBtnState = 0;
mjr 2:c174f9ee414a 432 }
mjr 1:d913e0afb2ac 433 }
mjr 1:d913e0afb2ac 434
mjr 1:d913e0afb2ac 435 // light/flash the calibration button light, if applicable
mjr 1:d913e0afb2ac 436 int newCalBtnLit = calBtnLit;
mjr 1:d913e0afb2ac 437 switch (calBtnState)
mjr 0:5acbbe3f4cf4 438 {
mjr 1:d913e0afb2ac 439 case 2:
mjr 1:d913e0afb2ac 440 // in the hold period - flash the light
mjr 1:d913e0afb2ac 441 newCalBtnLit = (((calBtnTimer.read_ms() - calBtnDownTime)/250) & 1);
mjr 1:d913e0afb2ac 442 break;
mjr 1:d913e0afb2ac 443
mjr 1:d913e0afb2ac 444 case 3:
mjr 1:d913e0afb2ac 445 // calibration mode - show steady on
mjr 1:d913e0afb2ac 446 newCalBtnLit = true;
mjr 1:d913e0afb2ac 447 break;
mjr 1:d913e0afb2ac 448
mjr 1:d913e0afb2ac 449 default:
mjr 1:d913e0afb2ac 450 // not calibrating/holding - show steady off
mjr 1:d913e0afb2ac 451 newCalBtnLit = false;
mjr 1:d913e0afb2ac 452 break;
mjr 1:d913e0afb2ac 453 }
mjr 3:3514575d4f86 454
mjr 3:3514575d4f86 455 // light or flash the external calibration button LED, and
mjr 3:3514575d4f86 456 // do the same with the on-board blue LED
mjr 1:d913e0afb2ac 457 if (calBtnLit != newCalBtnLit)
mjr 1:d913e0afb2ac 458 {
mjr 1:d913e0afb2ac 459 calBtnLit = newCalBtnLit;
mjr 2:c174f9ee414a 460 if (calBtnLit) {
mjr 2:c174f9ee414a 461 calBtnLed = 1;
mjr 4:02c7cd7b2183 462 ledR = 1;
mjr 4:02c7cd7b2183 463 ledG = 1;
mjr 4:02c7cd7b2183 464 ledB = 1;
mjr 2:c174f9ee414a 465 }
mjr 2:c174f9ee414a 466 else {
mjr 2:c174f9ee414a 467 calBtnLed = 0;
mjr 4:02c7cd7b2183 468 ledR = 1;
mjr 4:02c7cd7b2183 469 ledG = 1;
mjr 4:02c7cd7b2183 470 ledB = 0;
mjr 2:c174f9ee414a 471 }
mjr 1:d913e0afb2ac 472 }
mjr 1:d913e0afb2ac 473
mjr 1:d913e0afb2ac 474 // read the plunger sensor
mjr 1:d913e0afb2ac 475 int znew = z;
mjr 2:c174f9ee414a 476 uint16_t pix[npix];
mjr 2:c174f9ee414a 477 ccd.read(pix, npix);
mjr 2:c174f9ee414a 478
mjr 2:c174f9ee414a 479 // get the average brightness at each end of the sensor
mjr 2:c174f9ee414a 480 long avg1 = (long(pix[0]) + long(pix[1]) + long(pix[2]) + long(pix[3]) + long(pix[4]))/5;
mjr 2:c174f9ee414a 481 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 482
mjr 2:c174f9ee414a 483 // figure the midpoint in the brightness; multiply by 3 so that we can
mjr 2:c174f9ee414a 484 // compare sums of three pixels at a time to smooth out noise
mjr 2:c174f9ee414a 485 long midpt = (avg1 + avg2)/2 * 3;
mjr 2:c174f9ee414a 486
mjr 2:c174f9ee414a 487 // Work from the bright end to the dark end. VP interprets the
mjr 2:c174f9ee414a 488 // Z axis value as the amount the plunger is pulled: the minimum
mjr 2:c174f9ee414a 489 // is the rest position, the maximum is fully pulled. So we
mjr 2:c174f9ee414a 490 // essentially want to report how much of the sensor is lit,
mjr 2:c174f9ee414a 491 // since this increases as the plunger is pulled back.
mjr 2:c174f9ee414a 492 int si = 1, di = 1;
mjr 2:c174f9ee414a 493 if (avg1 < avg2)
mjr 2:c174f9ee414a 494 si = npix - 2, di = -1;
mjr 2:c174f9ee414a 495
mjr 2:c174f9ee414a 496 // scan for the midpoint
mjr 2:c174f9ee414a 497 uint16_t *pixp = pix + si;
mjr 2:c174f9ee414a 498 for (int n = 1 ; n < npix - 1 ; ++n, pixp += di)
mjr 1:d913e0afb2ac 499 {
mjr 2:c174f9ee414a 500 // if we've crossed the midpoint, report this position
mjr 2:c174f9ee414a 501 if (long(pixp[-1]) + long(pixp[0]) + long(pixp[1]) < midpt)
mjr 1:d913e0afb2ac 502 {
mjr 2:c174f9ee414a 503 // note the new position
mjr 2:c174f9ee414a 504 int pos = n;
mjr 2:c174f9ee414a 505
mjr 2:c174f9ee414a 506 // if the bright end and dark end don't differ by enough, skip this
mjr 2:c174f9ee414a 507 // reading entirely - we must have an overexposed or underexposed frame
mjr 2:c174f9ee414a 508 if (labs(avg1 - avg2) < 0x3333)
mjr 2:c174f9ee414a 509 break;
mjr 2:c174f9ee414a 510
mjr 2:c174f9ee414a 511 // Calibrate, or apply calibration, depending on the mode.
mjr 2:c174f9ee414a 512 // In either case, normalize to a 0-127 range. VP appears to
mjr 2:c174f9ee414a 513 // ignore negative Z axis values.
mjr 2:c174f9ee414a 514 if (calBtnState == 3)
mjr 1:d913e0afb2ac 515 {
mjr 2:c174f9ee414a 516 // calibrating - note if we're expanding the calibration envelope
mjr 2:c174f9ee414a 517 if (pos < cfg.d.plungerMin)
mjr 2:c174f9ee414a 518 cfg.d.plungerMin = pos;
mjr 2:c174f9ee414a 519 if (pos > cfg.d.plungerMax)
mjr 2:c174f9ee414a 520 cfg.d.plungerMax = pos;
mjr 2:c174f9ee414a 521
mjr 2:c174f9ee414a 522 // normalize to the full physical range while calibrating
mjr 2:c174f9ee414a 523 znew = int(float(pos)/npix * 127);
mjr 1:d913e0afb2ac 524 }
mjr 2:c174f9ee414a 525 else
mjr 2:c174f9ee414a 526 {
mjr 2:c174f9ee414a 527 // running normally - normalize to the calibration range
mjr 2:c174f9ee414a 528 if (pos < cfg.d.plungerMin)
mjr 2:c174f9ee414a 529 pos = cfg.d.plungerMin;
mjr 2:c174f9ee414a 530 if (pos > cfg.d.plungerMax)
mjr 2:c174f9ee414a 531 pos = cfg.d.plungerMax;
mjr 2:c174f9ee414a 532 znew = int(float(pos - cfg.d.plungerMin)
mjr 2:c174f9ee414a 533 / (cfg.d.plungerMax - cfg.d.plungerMin + 1) * 127);
mjr 2:c174f9ee414a 534 }
mjr 2:c174f9ee414a 535
mjr 2:c174f9ee414a 536 // done
mjr 2:c174f9ee414a 537 break;
mjr 1:d913e0afb2ac 538 }
mjr 2:c174f9ee414a 539 }
mjr 1:d913e0afb2ac 540
mjr 1:d913e0afb2ac 541 // read the accelerometer
mjr 3:3514575d4f86 542 float xa, ya, rxa, rya;
mjr 3:3514575d4f86 543 accel.get(xa, ya, rxa, rya);
mjr 1:d913e0afb2ac 544
mjr 1:d913e0afb2ac 545 // check for auto-centering every so often
mjr 1:d913e0afb2ac 546 if (acTimer.read_ms() - t0ac > 1000)
mjr 1:d913e0afb2ac 547 {
mjr 1:d913e0afb2ac 548 // add the sample to the history list
mjr 1:d913e0afb2ac 549 accPrv[iAccPrv].x = xa;
mjr 1:d913e0afb2ac 550 accPrv[iAccPrv].y = ya;
mjr 1:d913e0afb2ac 551
mjr 1:d913e0afb2ac 552 // store the slot
mjr 1:d913e0afb2ac 553 iAccPrv += 1;
mjr 1:d913e0afb2ac 554 iAccPrv %= maxAccPrv;
mjr 1:d913e0afb2ac 555 nAccPrv += 1;
mjr 1:d913e0afb2ac 556
mjr 1:d913e0afb2ac 557 // If we have a full complement, check for stability. The
mjr 1:d913e0afb2ac 558 // raw accelerometer input is in the rnage -4096 to 4096, but
mjr 1:d913e0afb2ac 559 // the class cover normalizes to a unit interval (-1.0 .. +1.0).
mjr 1:d913e0afb2ac 560 const float accTol = .005;
mjr 1:d913e0afb2ac 561 if (nAccPrv >= maxAccPrv
mjr 1:d913e0afb2ac 562 && accPrv[0].dist(accPrv[1]) < accTol
mjr 1:d913e0afb2ac 563 && accPrv[0].dist(accPrv[2]) < accTol
mjr 1:d913e0afb2ac 564 && accPrv[0].dist(accPrv[3]) < accTol
mjr 1:d913e0afb2ac 565 && accPrv[0].dist(accPrv[4]) < accTol)
mjr 1:d913e0afb2ac 566 {
mjr 1:d913e0afb2ac 567 // figure the new center
mjr 1:d913e0afb2ac 568 xCenter = (accPrv[0].x + accPrv[1].x + accPrv[2].x + accPrv[3].x + accPrv[4].x)/5.0;
mjr 1:d913e0afb2ac 569 yCenter = (accPrv[0].y + accPrv[1].y + accPrv[2].y + accPrv[3].y + accPrv[4].y)/5.0;
mjr 1:d913e0afb2ac 570 }
mjr 1:d913e0afb2ac 571
mjr 1:d913e0afb2ac 572 // reset the auto-center timer
mjr 1:d913e0afb2ac 573 acTimer.reset();
mjr 1:d913e0afb2ac 574 t0ac = acTimer.read_ms();
mjr 1:d913e0afb2ac 575 }
mjr 1:d913e0afb2ac 576
mjr 1:d913e0afb2ac 577 // adjust for our auto centering
mjr 1:d913e0afb2ac 578 xa -= xCenter;
mjr 1:d913e0afb2ac 579 ya -= yCenter;
mjr 1:d913e0afb2ac 580
mjr 1:d913e0afb2ac 581 // confine to the unit interval
mjr 1:d913e0afb2ac 582 if (xa < -1.0) xa = -1.0;
mjr 1:d913e0afb2ac 583 if (xa > 1.0) xa = 1.0;
mjr 1:d913e0afb2ac 584 if (ya < -1.0) ya = -1.0;
mjr 1:d913e0afb2ac 585 if (ya > 1.0) ya = 1.0;
mjr 0:5acbbe3f4cf4 586
mjr 1:d913e0afb2ac 587 // figure the new mouse report data
mjr 1:d913e0afb2ac 588 int xnew = (int)(127 * xa);
mjr 1:d913e0afb2ac 589 int ynew = (int)(127 * ya);
mjr 2:c174f9ee414a 590
mjr 2:c174f9ee414a 591 // store the updated joystick coordinates
mjr 2:c174f9ee414a 592 x = xnew;
mjr 2:c174f9ee414a 593 y = ynew;
mjr 2:c174f9ee414a 594 z = znew;
mjr 1:d913e0afb2ac 595
mjr 2:c174f9ee414a 596 // if we're in USB suspend or disconnect mode, spin
mjr 2:c174f9ee414a 597 if (js.isSuspended() || !js.isConnected())
mjr 0:5acbbe3f4cf4 598 {
mjr 2:c174f9ee414a 599 // go dark (turn off the indicator LEDs)
mjr 4:02c7cd7b2183 600 ledG = 1;
mjr 4:02c7cd7b2183 601 ledB = 1;
mjr 4:02c7cd7b2183 602 ledR = 1;
mjr 2:c174f9ee414a 603
mjr 2:c174f9ee414a 604 // wait until we're connected and come out of suspend mode
mjr 4:02c7cd7b2183 605 for (uint32_t n = 0 ; js.isSuspended() || !js.isConnected() ; ++n)
mjr 2:c174f9ee414a 606 {
mjr 2:c174f9ee414a 607 // spin for a bit
mjr 2:c174f9ee414a 608 wait(1);
mjr 2:c174f9ee414a 609
mjr 4:02c7cd7b2183 610 // if we're suspended, do a brief red flash; otherwise do a long red flash
mjr 4:02c7cd7b2183 611 if (js.isSuspended())
mjr 4:02c7cd7b2183 612 {
mjr 4:02c7cd7b2183 613 // suspended - flash briefly ever few seconds
mjr 4:02c7cd7b2183 614 if (n % 3 == 0)
mjr 4:02c7cd7b2183 615 {
mjr 4:02c7cd7b2183 616 ledR = 0;
mjr 4:02c7cd7b2183 617 wait(0.05);
mjr 4:02c7cd7b2183 618 ledR = 1;
mjr 4:02c7cd7b2183 619 }
mjr 4:02c7cd7b2183 620 }
mjr 4:02c7cd7b2183 621 else
mjr 4:02c7cd7b2183 622 {
mjr 4:02c7cd7b2183 623 // running, not connected - flash red
mjr 4:02c7cd7b2183 624 ledR = !ledR;
mjr 4:02c7cd7b2183 625 }
mjr 2:c174f9ee414a 626 }
mjr 2:c174f9ee414a 627 }
mjr 1:d913e0afb2ac 628
mjr 3:3514575d4f86 629 // Send the status report. It doesn't really matter what
mjr 3:3514575d4f86 630 // coordinate system we use, since Visual Pinball has config
mjr 3:3514575d4f86 631 // options for rotations and axis reversals, but reversing y
mjr 3:3514575d4f86 632 // at the device level seems to produce the most intuitive
mjr 3:3514575d4f86 633 // results for the Windows joystick control panel view, which
mjr 3:3514575d4f86 634 // is an easy way to check that the device is working.
mjr 3:3514575d4f86 635 js.update(x, -y, z, int(rxa*127), int(rya*127), 0);
mjr 1:d913e0afb2ac 636
mjr 2:c174f9ee414a 637 // show a heartbeat flash in blue every so often if not in
mjr 2:c174f9ee414a 638 // calibration mode
mjr 2:c174f9ee414a 639 if (calBtnState < 2 && hbTimer.read_ms() - t0Hb > 1000)
mjr 1:d913e0afb2ac 640 {
mjr 2:c174f9ee414a 641 if (js.isSuspended())
mjr 2:c174f9ee414a 642 {
mjr 2:c174f9ee414a 643 // suspended - turn off the LEDs entirely
mjr 4:02c7cd7b2183 644 ledR = 1;
mjr 4:02c7cd7b2183 645 ledG = 1;
mjr 4:02c7cd7b2183 646 ledB = 1;
mjr 2:c174f9ee414a 647 }
mjr 2:c174f9ee414a 648 else if (!js.isConnected())
mjr 2:c174f9ee414a 649 {
mjr 2:c174f9ee414a 650 // not connected - flash red
mjr 2:c174f9ee414a 651 hb = !hb;
mjr 4:02c7cd7b2183 652 ledR = (hb ? 0 : 1);
mjr 4:02c7cd7b2183 653 ledG = 1;
mjr 4:02c7cd7b2183 654 ledB = 1;
mjr 2:c174f9ee414a 655 }
mjr 2:c174f9ee414a 656 else if (flash_valid)
mjr 2:c174f9ee414a 657 {
mjr 2:c174f9ee414a 658 // connected, NVM valid - flash blue/green
mjr 2:c174f9ee414a 659 hb = !hb;
mjr 4:02c7cd7b2183 660 ledR = 1;
mjr 4:02c7cd7b2183 661 ledG = (hb ? 0 : 1);
mjr 4:02c7cd7b2183 662 ledB = (hb ? 1 : 0);
mjr 2:c174f9ee414a 663 }
mjr 2:c174f9ee414a 664 else
mjr 2:c174f9ee414a 665 {
mjr 2:c174f9ee414a 666 // connected, factory reset - flash yellow/green
mjr 2:c174f9ee414a 667 hb = !hb;
mjr 4:02c7cd7b2183 668 ledR = (hb ? 0 : 1);
mjr 4:02c7cd7b2183 669 ledG = 0;
mjr 4:02c7cd7b2183 670 ledB = 1;
mjr 2:c174f9ee414a 671 }
mjr 1:d913e0afb2ac 672
mjr 1:d913e0afb2ac 673 // reset the heartbeat timer
mjr 1:d913e0afb2ac 674 hbTimer.reset();
mjr 1:d913e0afb2ac 675 t0Hb = hbTimer.read_ms();
mjr 1:d913e0afb2ac 676 }
mjr 1:d913e0afb2ac 677 }
mjr 0:5acbbe3f4cf4 678 }