Arnaud VALLEY
Mirror with some correction
Dependencies: mbed FastIO FastPWM USBDevice
Diff: main.cpp
- Revision:
- 50:40015764bbe6
- Parent:
- 49:37bd97eb7688
- Child:
- 51:57eb311faafa
diff -r 37bd97eb7688 -r 40015764bbe6 main.cpp
--- a/main.cpp Sat Feb 27 00:22:04 2016 +0000
+++ b/main.cpp Sat Feb 27 06:41:17 2016 +0000
@@ -1,3 +1,47 @@
+// NEW PLUNGER PROCESSING 1 - 26 Feb 2016
+// This version takes advantage of the new, faster TSL1410R DMA processing
+// to implement better firing event detection. This attempt works basically
+// like the old version, but uses the higher time resolution to detect firing
+// events more reliably. The scheme here watches for accelerations (the old
+// TSL1410R code wasn't fast enough to do that). We observed that a release
+// takes about 65ms from the maximum retraction point to crossing the zero
+// point. Our 2.5ms snapshots allow us to see about 25 frames over this
+// span. The first 5-10 frames will show the position moving forward, but
+// we don't see a clear acceleration trend in that first section. After
+// that we see almost perfectly uniform acceleration for the rest of the
+// release until we cross the zero point. "Almost" in that we often have
+// one or two frames where the velocity is just slightly lower than the
+// previous frame's. I think this is probably imprecision in the sensor;
+// realistically, our time base is probably good to only +/- 1ms or so,
+// since the shutter time for each frame is about 2.3ms. We assume that
+// each frame captures the midpoint time of the shutter span, but that's
+// a crude approximation; the scientifically right way to look at this is
+// that our snapshot times have an uncertainty on the order of the shutter
+// time. Those error bars of course propagate into the velocity readings.
+// Fortunately, the true acceleration is high enough that it overwhelms
+// the error bars on almost every sample. It appears to solve this
+// entirely if we simply skip a sample where we don't see acceleration
+// once we think a release has started - this takes our time between
+// samples up to about 5ms, at which point the acceleration does seem to
+// overwhelm the error bars 100% of the time.
+//
+// I'm capturing a snapshot of this implementation because I'm going to
+// try something different. It would be much simpler if we could put our
+// readings on a slight time delay, and identify firing events
+// retrospectively when we actually cross the zero point. I'm going to
+// experiment first with a time delay to see what the maximum acceptable
+// delay time is. I expect that I can go up to about 30ms without it
+// becoming noticeable, but I need to try it out. If we can go up to
+// 70ms, we can capture firing events perfectly because we can delay
+// reports long enough to have an entire firing event in history before
+// we report anything. That will let us fix up the history to report an
+// idealized firing event to VP every time, with no false positives.
+// But I suspect a 70ms delay is going to be way too noticeable. If
+// a 30ms delay works, I think we can still do a pretty good job - that
+// gets us about halfway into a release motion, at which point it's
+// pretty certain that it's really a release.
+
+
/* Copyright 2014, 2015 M J Roberts, MIT License
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
@@ -2319,33 +2363,9 @@
// no history yet
histIdx = 0;
- histR = 0;
// not in calibration mode
- cal = false;
- }
-
- // prime the history
- void prime()
- {
- // fill the initial history buffer, timing out if it takes too long
- plungerSensor->read(prv);
- Timer t;
- t.start();
- const uint32_t timeout = 1000000UL;
- for (int i = 0 ; i < countof(hist) && t.read_us() < timeout ; ++i)
- {
- // take a new reading that's at least 2ms newer than the last
- PlungerReading r;
- while (t.read_us() < timeout)
- {
- if (plungerSensor->read(r) && uint32_t(r.t - prv.t) > 2000UL)
- break;
- }
- addHist(r);
- prv = r;
- }
- histR = 0;
+ cal = false;
}
// Collect a reading from the plunger sensor. The main loop calls
@@ -2374,17 +2394,12 @@
// sensor position as the joystick value, adjusted to the
// JOYMAX scale.
z = int16_t((long(r.pos) * JOYMAX)/65535);
+ return;
}
- else
- {
- // bounds-check the calibration data
- checkCalBounds(r.pos);
-
- // calibrate and rescale the value
- r.pos = int(
- (long(r.pos - cfg.plunger.cal.zero) * JOYMAX)
- / (cfg.plunger.cal.max - cfg.plunger.cal.zero));
- }
+
+ // Pull the last two readings from the history
+ const PlungerReading &prv = nthHist(0);
+ const PlungerReading &prv2 = nthHist(1);
// If the new reading is within 2ms of the previous reading,
// ignore it. We require a minimum time between samples to
@@ -2397,46 +2412,225 @@
if (uint32_t(r.t - prv.t) < 2000UL)
return;
- // Save it as the previous sample
- prv = r;
+ // bounds-check the calibration data
+ checkCalBounds(r.pos);
+
+ // calibrate and rescale the value
+ r.pos = int(
+ (long(r.pos - cfg.plunger.cal.zero) * JOYMAX)
+ / (cfg.plunger.cal.max - cfg.plunger.cal.zero));
+
+ // Calculate the velocity from the second-to-last reading
+ // to here, in joystick distance units per microsecond.
+ // Note that we use the second-to-last reading rather than
+ // the very last reading to give ourselves a little longer
+ // time base. The time base is so short between consecutive
+ // readings that the error bars in the position would be too
+ // large.
+ //
+ // For reference, the physical plunger velocity ranges up
+ // to about 100,000 joystick distance units/sec. This is
+ // based on empirical measurements. The typical time for
+ // a real plunger to travel the full distance when released
+ // from full retraction is about 85ms, so the average velocity
+ // covering this distance is about 56,000 units/sec. The
+ // peak is probably about twice that. In real-world units,
+ // this translates to an average speed of about .75 m/s and
+ // a peak of about 1.5 m/s.
+ //
+ // Note that we actually calculate the value here in units
+ // per *microsecond* - the discussion above is in terms of
+ // units/sec because that's more on a human scale. Our
+ // choice of internal units here really isn't important,
+ // since we only use the velocity for comparison purposes,
+ // to detect acceleration trends. We therefore save ourselves
+ // a little CPU time by using the natural units of our inputs.
+ float v = float(r.pos - prv2.pos)/float(r.t - prv2.t);
+
+ // presume we'll report the latest instantaneous reading
+ z = r.pos;
+ vz = v;
- // Add it to our history
- addHist(r);
+ // Check firing events
+ switch (firing)
+ {
+ case 0:
+ // Default state - not in a firing event.
+
+ // If we have forward motion from a position that's retracted
+ // beyond a threshold, enter phase 1. If we're not pulled back
+ // far enough, don't bother with this, as a release wouldn't
+ // be strong enough to require the synthetic firing treatment.
+ if (v < 0 && r.pos > JOYMAX/6)
+ {
+ // enter phase 1
+ firingMode(1);
+
+ // we don't have a freeze position yet, but note the start time
+ f1.pos = 0;
+ f1.t = r.t;
+
+ // Figure the barrel spring "bounce" position in case we complete
+ // the firing event. This is the amount that the forward momentum
+ // of the plunger will compress the barrel spring at the peak of
+ // the forward travel during the release. Assume that this is
+ // linearly proportional to the starting retraction distance.
+ // The barrel spring is about 1/6 the length of the main spring,
+ // so figure it compresses by 1/6 the distance. (This is overly
+ // simplistic and inaccurate, but it seems to give perfectly good
+ // visual results, and that's all it's for.)
+ f2.pos = -r.pos/6;
+ }
+ break;
+
+ case 1:
+ // Phase 1 - acceleration. If we cross the zero point, trigger
+ // the firing event. Otherwise, continue monitoring as long as we
+ // see acceleration in the forward direction.
+ if (r.pos <= 0)
+ {
+ // switch to the synthetic firing mode
+ firingMode(2);
+ z = f2.pos;
+
+ // note the start time for the firing phase
+ f2.t = r.t;
+ }
+ else if (v < vprv2)
+ {
+ // We're still accelerating, and we haven't crossed the zero
+ // point yet - stay in phase 1. (Note that forward motion is
+ // negative velocity, so accelerating means that the new
+ // velocity is more negative than the previous one, which
+ // is to say numerically less than - that's why the test
+ // for acceleration is the seemingly backwards 'v < vprv'.)
+
+ // If we've been accelerating for at least 20ms, we're probably
+ // really doing a release. Jump back to the recent local
+ // maximum where the release *really* started. This is always
+ // a bit before we started seeing sustained accleration, because
+ // the plunger motion for the first few milliseconds is too slow
+ // for our sensor precision to reliably detect acceleration.
+ if (f1.pos != 0)
+ {
+ // we have a reset point - freeze there
+ z = f1.pos;
+ }
+ else if (uint32_t(r.t - f1.t) >= 20000UL)
+ {
+ // it's been long enough - set a reset point.
+ f1.pos = z = histLocalMax(r.t, 50000UL);
+ }
+ }
+ else
+ {
+ // We're not accelerating. Cancel the firing event.
+ firingMode(0);
+ }
+ break;
+
+ case 2:
+ // Phase 2 - start of synthetic firing event. Report the fake
+ // bounce for 25ms. VP polls the joystick about every 10ms, so
+ // this should be enough time to guarantee that VP sees this
+ // report at least once.
+ if (uint32_t(r.t - f2.t) < 25000UL)
+ {
+ // report the bounce position
+ z = f2.pos;
+ }
+ else
+ {
+ // it's been long enough - switch to phase 3, where we
+ // report the park position until the real plunger comes
+ // to rest
+ firingMode(3);
+ z = 0;
+
+ // set the start of the "stability window" to the rest position
+ f3s.t = r.t;
+ f3s.pos = 0;
+
+ // set the start of the "retraction window" to the actual position
+ f3r = r;
+ }
+ break;
+
+ case 3:
+ // Phase 3 - in synthetic firing event. Report the park position
+ // until the plunger position stabilizes. Left to its own devices,
+ // the plunger will usualy bounce off the barrel spring several
+ // times before coming to rest, so we'll see oscillating motion
+ // for a second or two. In the simplest case, we can aimply wait
+ // for the plunger to stop moving for a short time. However, the
+ // player might intervene by pulling the plunger back again, so
+ // watch for that motion as well. If we're just bouncing freely,
+ // we'll see the direction change frequently. If the player is
+ // moving the plunger manually, the direction will be constant
+ // for longer.
+ if (v >= 0)
+ {
+ // We're moving back (or standing still). If this has been
+ // going on for a while, the user must have taken control.
+ if (uint32_t(r.t - f3r.t) > 65000UL)
+ {
+ // user has taken control - cancel firing mode
+ firingMode(0);
+ break;
+ }
+ }
+ else
+ {
+ // forward motion - reset retraction window
+ f3r.t = r.t;
+ }
+
+ // check if we've come to rest, or close enough
+ if (abs(r.pos - f3s.pos) < 200)
+ {
+ // It's within an eighth inch of the last starting point.
+ // If it's been here for 30ms, consider it stable.
+ if (uint32_t(r.t - f3s.t) > 30000UL)
+ {
+ // we're done with the firing event
+ firingMode(0);
+ }
+ else
+ {
+ // it's close to the last position but hasn't been
+ // here long enough; stay in firing mode and continue
+ // to report the park position
+ z = 0;
+ }
+ }
+ else
+ {
+ // It's not close enough to the last starting point, so use
+ // this as a new starting point, and stay in firing mode.
+ f3s = r;
+ z = 0;
+ }
+ break;
+ }
+
+ // save the velocity reading for next time
+ vprv2 = vprv;
+ vprv = v;
+
+ // add the new reading to the history
+ hist[histIdx++] = r;
+ histIdx %= countof(hist);
}
}
// Get the current value to report through the joystick interface
- int16_t getPosition()
- {
- // advance the read pointer until it's not too old
- const uint32_t dtmax = 100000UL;
- for (;;)
- {
- // if this one isn't too old, use it
- if (uint32_t(prv.t - hist[histR].t) <= dtmax)
- break;
-
- // It's too old. Figure the next read index.
- int i = (histR + 1) % countof(hist);
-
- // if discarding this one would empty the history, keep
- // it and stop here
- if (i == histIdx)
- break;
-
- // discard this item by advancing the read pointer
- histR = i;
- }
-
- // return the position at the current read index
- return hist[histR].pos;
- }
+ int16_t getPosition() const { return z; }
// Get the current velocity (joystick distance units per microsecond)
float getVelocity() const { return vz; }
// get the timestamp of the current joystick report (microseconds)
- uint32_t getTimestamp() const { return prv.t; }
+ uint32_t getTimestamp() const { return nthHist(0).t; }
// Set calibration mode on or off
void calMode(bool f)
@@ -2453,19 +2647,6 @@
bool isFiring() { return firing > 3; }
private:
- // add a history entry
- inline void addHist(PlungerReading &r)
- {
- // if we're about to overwrite the item at the read pointer,
- // advance the read pointer
- if (histIdx == histR)
- histR = (histR + 1) % countof(hist);
-
- // add the new entry
- hist[histIdx++] = r;
- histIdx %= countof(hist);
- }
-
// set a firing mode
inline void firingMode(int m)
{
@@ -2559,11 +2740,8 @@
}
}
- // Previous reading
- PlungerReading prv;
-
- // velocity at previous reading
- float vprv;
+ // velocity at previous reading, and the one before that
+ float vprv, vprv2;
// Circular buffer of recent readings. We keep a short history
// of readings to analyze during firing events. We can only identify
@@ -2572,11 +2750,22 @@
// exactly when it started. We throttle our readings to no more
// than one every 2ms, so we have at least N*2ms of history in this
// array.
- PlungerReading hist[50];
+ PlungerReading hist[25];
int histIdx;
- // history read index
- int histR;
+ // get the nth history item (0=last, 1=2nd to last, etc)
+ const PlungerReading &nthHist(int n) const
+ {
+ // histIdx-1 is the last written; go from there
+ n = histIdx - 1 - n;
+
+ // adjust for wrapping
+ if (n < 0)
+ n += countof(hist);
+
+ // return the item
+ return hist[n];
+ }
// Firing event state.
//
@@ -3469,9 +3658,6 @@
// initialize the plunger sensor
plungerSensor->init();
- // prime the plunger reader
- plungerReader.prime();
-
// set up the ZB Launch Ball monitor
ZBLaunchBall zbLaunchBall;
@@ -3622,13 +3808,11 @@
// flag: did we successfully send a joystick report on this round?
bool jsOK = false;
- // If it's been long enough since our last USB status report,
- // send the new report. We throttle the report rate because
- // it can overwhelm the PC side if we report too frequently.
- // VP only wants to sync with the real world in 10ms intervals,
- // so reporting more frequently creates I/O overhead without
- // doing anything to improve the simulation.
- if (cfg.joystickEnabled /* $$$ && jsReportTimer.read_us() > 10000 */)
+ // If it's been long enough since our last USB status report, send
+ // the new report. VP only polls for input in 10ms intervals, so
+ // there's no benefit in sending reports more frequently than this.
+ // More frequent reporting would only add USB I/O overhead.
+ if (cfg.joystickEnabled && jsReportTimer.read_us() > 10000UL)
{
// read the accelerometer
int xa, ya;