An I/O controller for virtual pinball machines: accelerometer nudge sensing, analog plunger input, button input encoding, LedWiz compatible output controls, and more.

Dependencies:   mbed FastIO FastPWM USBDevice

Fork of Pinscape_Controller by Mike R

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers rotarySensor.h Source File

rotarySensor.h

00001 // Plunger sensor implementation for rotary absolute encoders
00002 //
00003 // This implements the plunger interfaces for rotary absolute encoders.  A
00004 // rotary encoder measures the angle of a rotating shaft.  An absolute encoder
00005 // is one where the microcontroller can ask the sensor for its current angular
00006 // position at any time.  (As opposed to incremental encoders, which don't have
00007 // any notion of their current position, but can only signal the host on each
00008 // change in position.)
00009 //
00010 // 
00011 // For plunger sensing, we can convert the plunger's linear motion into angular
00012 // motion using a mechanical link between the plunger rod and a rotating shaft 
00013 // positioned at a fixed point, somewhere nearby, but away from the plunger's 
00014 // axis of motion:
00015 //
00016 //    =X=======================|===   <- plunger, X = connector attachment point
00017 //      \
00018 //       \                            <- connector between plunger and shaft
00019 //        \
00020 //         *                          <- rotating shaft, at a fixed position
00021 //
00022 // As the plunger moves, the angle of the connector relative to the fixed
00023 // shaft position changes in a predictable way, so we can infer the plunger's
00024 // linear position at any given time by measuring the current rotational
00025 // angle of the shaft.
00026 //
00027 // The mechanical diagram above is, obviously, simplified for ASCII art's sake.
00028 // What's not shown is that the distance between the rotating shaft and the
00029 // "X" connection point on the plunger varies as the plunger moves, so the
00030 // mechanical linkage requires some way to accommodate that changing length.
00031 // If the connector is a rigid rod, it has to be able to slide at one or
00032 // the other connection points.  Alternatively, rather than using a rigid
00033 // linkage, we can use a spring or elastic band.  We leave these details up 
00034 // to the mechanical design, since the software isn't affected by that, as 
00035 // long as the basic relationship between linear and angular motion as shown
00036 // in the diagram is achieved.
00037 //
00038 //
00039 // Translating the angle to a linear position
00040 //
00041 // There are two complications to translating the angular reading back to
00042 // a linear plunger position.
00043 //
00044 // 1. We have to consider the sensor's zero point to be arbitrary, because
00045 // these sorts of sensors don't typically give the user a way to align the
00046 // zero point at a desired physical position.  The zero point will just be
00047 // wherever it ends up after installation.  The zero point could easily end 
00048 // up being somewhere in the middle of the plunger's travel range, which
00049 // means that readings might "wrap" - e.g., we might see a series of readings 
00050 // when the plunger is moving in one direction like this: 4050, 4070, 4090, 
00051 // 14, 34 (note how we "wrapped" past some maximum angle reading for the
00052 // sensor and went back to zero, then continued from there).
00053 //
00054 // To deal with this, we have to make a couple of assumptions:
00055 //
00056 //   - The park position is at about 1/6 of the overall travel range
00057 //   - The total angular travel range is less than one full revolution
00058 //
00059 // With those assumptions in hand, we can bias the raw readings to the
00060 // park position, and then take them modulo the raw scale.  That will
00061 // ensure that readings wrap properly, regardless of where the raw zero
00062 // point lies.
00063 //
00064 // 2. Going back to the original diagram, you can see that there's some
00065 // trigonometry required to interpret the sensor's angular reading as a
00066 // linear position on the plunger axis, which is of course what we need
00067 // to report to the PC software.
00068 //
00069 // Let's use the vertical line between the plunger and the rotation point
00070 // as the zero-degree reference point.  To figure the plunger position, 
00071 // we need to figure the difference between the raw angle reading and the
00072 // zero-degree point; call this theta.  Let L be the position of the plunger
00073 // relative to the vertical reference point, let D be the length of the 
00074 // vertical reference point line, and let H by the distance from the rotation 
00075 // point to the plunger connection point.  This is a right triangle with 
00076 // hypotenuse H and sides L and D.  D is a constant, because the rotation 
00077 // point never moves, and the plunger never moves vertically.  Thus we can
00078 // calculate D = H*cos(theta) and L = H*sin(theta).  D is a constant, so
00079 // we can figure H = D/cos(theta) hence L = D*sin(theta)/cos(theta) or
00080 // D*tan(theta).  If we wanted to know the true position in real-world
00081 // units, we'd have to know D, but only need arbitrary linear units, so
00082 // we can choose whatever value for D we find convenient: in particular,
00083 // a value that gives us the desired range and resolution for the final
00084 // result.
00085 //
00086 // Note that the tangent diverges at +/-90 degrees, but that's okay,
00087 // because the mechanical setup we've described is inherently constrained
00088 // to stay well within those limits.  This would even be true for an 
00089 // arbitrarily long range of motion along the travel axis, but we don't
00090 // even have to worry about that since we have such a well-defined range
00091 // of travel (of only about 3") to track.
00092 //
00093 // There's still one big piece missing here: we somehow have to know where
00094 // that vertical zero point lies.  That's something we can only learn by
00095 // calibration.  Unfortunately, we don't have a good way to detect this
00096 // directly.  We *could* ask the user to look inside the cabinet and press
00097 // a button when the needle is straight up, but that seems too cumbersome
00098 // for the user, not to mention terribly imprecise.  So we'll approach this
00099 // from the other direction: we'll assume a particular placement of the
00100 // rotation point relative to the travel range, and we'll provide
00101 // installation instructions to achieve that assumed alignment.
00102 //
00103 // The full range we actually have after calibration consists of the park
00104 // position and the maximum retracted position.  We could in principle also
00105 // calibrate the maximum forward position, but that can't be read as reliably
00106 // as the other two, because the barrel spring makes it difficult for the 
00107 // user to be sure they've pushed it all the way forward.  Since we can 
00108 // extract the information we need from the park and max retract positions,
00109 // it's better to rely on those alone and not ask for information that the
00110 // user can't as easily provide.  Given these positions, AND the assumption
00111 // that the rotation point is at the midpoint of the plunger travel range,
00112 // we can do some grungy trig work to come up with a formula for the angle 
00113 // between the park position and the vertical:
00114 //
00115 //    let C1 = 1 1/32" (distance from midpoint to park),
00116 //        C2 = 1 17/32" (distance from midpoint to max retract),
00117 //        C = C2/C1 = 1.48484849,
00118 //        alpha = angle from park to vertical,
00119 //        beta = angle from max retract to vertical
00120 //        theta = alpha + beta = angle from park to max retract, known from calibration,
00121 //        T = tan(theta);
00122 //
00123 //    then
00124 //        alpha = atan(sqrt(4*T*T*C + C^2 + 2*C + 1) - C - 1)/(2*T*C))
00125 //
00126 // Did I mention this was grungy?  At any rate, everything going into that
00127 // last equation is either constant or known from the calibration, so we 
00128 // can pre-compute alpha and store it after each calibration operation.
00129 // And once we've computed alpha, we can easily translate an angle reading 
00130 // from the sensor to an angle relative to the vertical, which we can plug 
00131 // into D*tan(angle) to convert to a linear position on the plunger axis.
00132 //
00133 // The final step is to scale that linear position into joystick reporting
00134 // units.  Those units are arbitrary, so we don't have to relate this to any
00135 // real-world lengths.  We can simply figure a scaling factor that maps the
00136 // physical range to map to roughly the full range of the joystick units.
00137 //
00138 // If you're wondering how we derived that ugly formula, read on.  Start
00139 // with the basic relationships D*tan(alpha) = C1 and D*tan(beta) = C2.
00140 // This lets us write tan(beta) in terms of tan(alpha) as 
00141 // C2/C1*tan(alpha) = C*tan(alpha).  We can combine this with an identity
00142 // for the tan of a sum of angles:
00143 //
00144 //    tan(alpha + beta) = (tan(alpha) + tan(beta))/(1 - tan(alpha)*tan(beta))
00145 //
00146 // to obtain:
00147 //
00148 //    tan(theta) = tan(alpha + beta) = (1 + C*tan(alpha))/(1 - C*tan^2(alpha))
00149 //
00150 // Everything here except alpha is known, so we now have a quadratic equation
00151 // for tan(alpha).  We can solve that by cranking through the normal algorithm
00152 // for solving a quadratic equation, arriving at the solution above.
00153 //
00154 //
00155 // Choosing an install position
00156 //
00157 // There are two competing factors in choosing the optimal "D".  On the one
00158 // hand, you'd like D to be as large as possible, to maximum linearity of the
00159 // tan function used to translate angle to linear position.  Higher linearity
00160 // gives us greater immunity to variations in the precise centering of the
00161 // rotation axis in the plunger travel range.  tan() is pretty linear (that
00162 // is, tan(theta) is approximately proportional to theta) for small theta, 
00163 // within about +/- 30 degrees.  On the other hand, you'd like D to be as 
00164 // small as possible so that we get the largest overall angle range.  Our 
00165 // sensor has a fixed angular resolution, so the more of the overall circle 
00166 // we use, the more sensor increments we have over the range, and thus the 
00167 // better effective linear resolution.
00168 //
00169 // Let's do some calculations for various "D" values (vertical distance 
00170 // between rotation point and plunger rod).  We'll base our calculations
00171 // on the AEAT-6012 sensor's 12-bit angular resolution.
00172 //
00173 //     D         theta(max)   eff dpi   theta(park)
00174 //  -----------------------------------------------
00175 //    1 17/32"    45 deg       341       34 deg
00176 //    2"          37 deg       280       27 deg
00177 //    2 21/32"    30 deg       228       21 deg
00178 //    3 1/4"      25 deg       190       17 deg
00179 //    4 3/16"     20 deg       152       14 deg
00180 //
00181 // I'd consider 50 dpi to be the minimum for acceptable performance, 100 dpi
00182 // to be excellent, and anything above 300 dpi to be diminishing returns.  So
00183 // for a 12-bit sensor, 2" looks like the sweet spot.  It doesn't take us far
00184 // outside of the +/-30 deg zone of tan() linearity, and it achieves almost 
00185 // 300 dpi of effective linear resolution.  I'd stop there are not try to
00186 // push the angular resolution higher with a shorter D; with the 45 deg
00187 // theta(max) at D = 1-17/32", we'd get a lovely DPI level of 341, but at
00188 // the cost of getting pretty non-linear around the ends of the plunger
00189 // travel.  Our math corrects for the non-linearity, but the more of that
00190 // correction we need, the more sensitive the whole contraption becomes to
00191 // getting the sensor positioning exactly right.  The closer we can stay to
00192 // the linear approximation, the more tolerant we are of inexact sensor
00193 // positioning.
00194 //
00195 //
00196 // Supported sensors
00197 //
00198 //  * AEAT-6012-A06.  This is a magnetic absolute encoder with 12-bit
00199 //    resolution.  It linearly encodes one full (360 degree) rotation in 
00200 //    4096 increments, so each increment represents 360/4096 = .088 degrees.
00201 //
00202 // The base class doesn't actually care much about the sensor type; all it
00203 // needs from the sensor is an angle reading represented on an arbitrary 
00204 // linear scale.  ("Linear" in the angle, so that one increment represents
00205 // a fixed number of degrees of arc.  The full scale can represent one full
00206 // turn but doesn't have to, as long as the scale is linear over the range
00207 // covered.)  To add new sensor types, you just need to add the code to
00208 // interface to the physical sensor and return its reading on an arbitrary
00209 // linear scale.
00210 
00211 #ifndef _ROTARYSENSOR_H_
00212 #define _ROTARYSENSOR_H_
00213 
00214 #include "FastInterruptIn.h"
00215 #include "AEAT6012.h"
00216 
00217 // The conversion from raw sensor reading to linear position involves a
00218 // bunch of translations to different scales and unit systems.  To help
00219 // keep things straight, let's give each scale a name:
00220 //
00221 // * "Raw" refers to the readings directly from the sensor.  These are
00222 //   unsigned ints in the range 0..maxRawAngle, and represent angles in a
00223 //   unit system where one increment equals 360/maxRawAngle degrees.  The
00224 //   zero point is arbitrary, determined by the physical orientation
00225 //   of the sensor.
00226 //
00227 // * "Biased" refers to angular units with a zero point equal to the
00228 //   park position.  This uses the same unit size as the "raw" system, but
00229 //   the zero point is adjusted so that 0 always means the park position.
00230 //   Negative values are forward of the park position.  This scale is
00231 //   also adjusted for wrapping, by ensuring that the value lies in the
00232 //   range -(maximum forward excursion) to +(scale max - max fwd excursion).
00233 //   Any values below or above the range are bumped up or down (respectively)
00234 //   to wrap them back into the range.
00235 //
00236 // * "Linear" refers to the final linear results, in joystick units, on
00237 //   the abstract integer scale from 0..65535 used by the generic plunger
00238 //   base class.
00239 // 
00240 class PlungerSensorRotary: public PlungerSensor
00241 {
00242 public:
00243     PlungerSensorRotary(int maxRawAngle, float radiansPerSensorUnit) : 
00244         PlungerSensor(65535),
00245         maxRawAngle(maxRawAngle),
00246         radiansPerSensorUnit(radiansPerSensorUnit)
00247     {   
00248         // start our sample timer with an arbitrary zero point of now
00249         timer.start();
00250         
00251         // clear the timing statistics
00252         nReads = 0;
00253         totalReadTime = 0;
00254         
00255         // Pre-calculate the maximum forward excursion distance, in raw
00256         // units.  For our reference mechanical setup with "D" in a likely
00257         // range, theta(max) is always about 10 degrees higher than
00258         // theta(park).  10 degrees is about 1/36 of the overall circle,
00259         // which is the same as 1/36 of the sensor scale.  To be 
00260         // conservative, allow for about 3X that, so allow 1/12 of scale
00261         // as the maximum forward excursion.  For wrapping purposes, we'll
00262         // consider any reading outside of the range from -(excursion)
00263         // to +(maxRawAngle - excursion) to be wrapped.
00264         maxForwardExcursionRaw = maxRawAngle/12;
00265                 
00266         // reset the calibration counters
00267         biasedMinObserved = biasedMaxObserved = 0;
00268     }
00269     
00270     // Restore the saved calibration at startup
00271     virtual void restoreCalibration(Config &cfg)
00272     {
00273         // only proceed if there's calibration data to retrieve
00274         if (cfg.plunger.cal.calibrated)
00275         {
00276             // we store the raw park angle in raw0
00277             rawParkAngle = cfg.plunger.cal.raw0;
00278             
00279             // we store biased max angle in raw1
00280             biasedMax = cfg.plunger.cal.raw1;
00281         }
00282         else
00283         {
00284             // Use the current sensor reading as the initial guess at the
00285             // park position.  The system is usually powered up with the
00286             // plunger at the neutral position, so this is a good guess in
00287             // most cases.  If the plunger has been calibrated, we'll restore
00288             // the better guess when we restore the configuration later on in
00289             // the initialization process.
00290             rawParkAngle = 0;
00291             readSensor(rawParkAngle);
00292 
00293             // Set an initial wild guess at a range equal to +/-35 degrees.
00294             // Note that this is in the "biased" coordinate system - raw
00295             // units, but relative to the park angle.  The park angle is
00296             // about -25 degrees in this setup.
00297             biasedMax = (35 + 25) * maxRawAngle/360;        
00298         }
00299             
00300         // recalculate the vertical angle
00301         updateAlpha();
00302     }
00303     
00304     // Begin calibration
00305     virtual void beginCalibration(Config &)
00306     {
00307         // Calibration starts out with the plunger at the park position, so
00308         // we can take the current sensor reading to be the park position.
00309         rawParkAngle = 0;
00310         readSensor(rawParkAngle);
00311         
00312         // Reset the observed calibration counters
00313         biasedMinObserved = biasedMaxObserved = 0;
00314     }
00315     
00316     // End calibration
00317     virtual void endCalibration(Config &cfg)
00318     {
00319         // apply the observed maximum angle
00320         biasedMax = biasedMaxObserved;
00321         
00322         // recalculate the vertical angle
00323         updateAlpha();
00324 
00325         // save our raw configuration data
00326         cfg.plunger.cal.raw0 = static_cast<uint16_t>(rawParkAngle);
00327         cfg.plunger.cal.raw1 = static_cast<uint16_t>(biasedMax);
00328         
00329         // Refigure the range for the generic code
00330         cfg.plunger.cal.min = biasedAngleToLinear(biasedMinObserved);
00331         cfg.plunger.cal.max = biasedAngleToLinear(biasedMaxObserved);
00332         cfg.plunger.cal.zero = biasedAngleToLinear(0);
00333     }
00334     
00335     // figure the average scan time in microseconds
00336     virtual uint32_t getAvgScanTime() 
00337     { 
00338         return nReads == 0 ? 0 : static_cast<uint32_t>(totalReadTime / nReads);
00339     }
00340         
00341     // read the sensor
00342     virtual bool readRaw(PlungerReading &r)
00343     {
00344         // note the starting time for the reading
00345         uint32_t t0 = timer.read_us();
00346         
00347         // read the angular position
00348         int angle;
00349         if (!readSensor(angle))
00350             return false;
00351 
00352         // Refigure the angle relative to the raw park position.  This
00353         // is the "biased" angle.
00354         angle -= rawParkAngle;
00355         
00356         // Adjust for wrapping.
00357         //
00358         // An angular sensor reports the position on a circular scale, for
00359         // obvious reasons, so there's some point along the circle where the
00360         // angle is zero.  One tick before that point reads as the maximum
00361         // angle on the scale, so we say that the scale "wraps" at that point.
00362         //
00363         // To correct for this, we can look to the layout of the mechanical
00364         // setup to constrain the values.  Consider anything below the maximum
00365         // forward exclusion to be wrapped on the low side, and consider
00366         // anything outside of the complementary range on the high side to
00367         // be wrapped on the high side.
00368         if (angle < -maxForwardExcursionRaw)
00369             angle += maxRawAngle;
00370         else if (angle >= maxRawAngle - maxForwardExcursionRaw)
00371             angle -= maxRawAngle;
00372             
00373         // Note if this is the highest/lowest observed reading on the biased 
00374         // scale since the last calibration started.
00375         if (angle > biasedMaxObserved)
00376             biasedMaxObserved = angle;
00377         if (angle < biasedMinObserved)
00378             biasedMinObserved = angle;
00379             
00380         // figure the linear result
00381         r.pos = biasedAngleToLinear(angle);
00382                 
00383         // Set the timestamp on the reading to right now
00384         uint32_t now = timer.read_us();
00385         r.t = now;
00386         
00387         // count the read statistics
00388         totalReadTime += now - t0;
00389         nReads += 1;        
00390         
00391         // success
00392         return true;
00393     }
00394     
00395 private:
00396     // Read the underlying sensor - implemented by the hardware-specific
00397     // subclasses.  Returns true on success, false if the sensor can't
00398     // be read.  The angle is returned in raw sensor units.
00399     virtual bool readSensor(int &angle) = 0;
00400 
00401     // Convert a biased angle value to a linear reading
00402     int biasedAngleToLinear(int angle)
00403     {
00404         // Translate to an angle relative to the vertical, in sensor units
00405         float theta = static_cast<float>(angle)*radiansPerSensorUnit - alpha;
00406         
00407         // Calculate the linear position relative to the vertical.  Zero
00408         // is right at the intersection of the vertical line from the
00409         // sensor rotation center to the plunger axis; positive numbers
00410         // are behind the vertical (more retracted).
00411         int linearPos = static_cast<int>(tanf(theta) * linearScaleFactor);
00412         
00413         // Finally, figure the offset.  The vertical is the halfway point
00414         // of the plunger motion, so we want to put it at half of the raw
00415         // scale of 0..65535.
00416         return linearPos + 32767;
00417     }
00418 
00419     // Update the estimation of the vertical angle, based on the angle
00420     // between the park position and maximum retraction point.
00421     void updateAlpha()
00422     {
00423         // See the comments at the top of the file for details on this
00424         // formula.  This figures the angle between the park position
00425         // and the vertical by applying the known constraints of the
00426         // mechanical setup: the known length of a standard plunger,
00427         // and the requirement that the rotation axis be placed at
00428         // roughly the midpoint of the plunger travel.
00429         const float C = 1.4848489f; // 1-17/32" / 1-1/32"
00430         float maxInRadians = static_cast<float>(biasedMax) * radiansPerSensorUnit;
00431         float T = tanf(maxInRadians);
00432         alpha = atanf((sqrtf(4*T*T*C + C*C + 2*C + 1) - C - 1)/(2*T*C));
00433 
00434         // While we're at it, figure the linear conversion factor.  Alpha
00435         // represents the angle from the park position to the midpoint,
00436         // which in the real world represents about 31/32", or just less
00437         // then 1/3 of the overall travel.  We want to normalize this to
00438         // the corresponding fraction of our 0..65535 abstract linear unit
00439         // system.  To avoid overflow, normalize to a slightly smaller
00440         // scale.
00441         const float safeMax = 60000.0f;
00442         const float alphaInLinearUnits = safeMax * .316327f; // 31/22" / 3-1/16"
00443         linearScaleFactor = static_cast<int>(alphaInLinearUnits / tanf(alpha));
00444     }
00445 
00446     // Maximum raw angular reading from the sensor.  The sensor's readings
00447     // will always be on a scale from 0..maxRawAngle.
00448     int maxRawAngle;
00449     
00450     // Radians per sensor unit.  This is a constant for the sensor.
00451     float radiansPerSensorUnit;
00452     
00453     // Pre-calculated value of the maximum forward excursion, in raw units.
00454     int maxForwardExcursionRaw;
00455     
00456     // Raw reading at the park position.  We use this to handle "wrapping",
00457     // if the sensor's raw zero reading position is within the plunger travel
00458     // range.  All readings are taken to be within 
00459     int rawParkAngle;
00460     
00461     // Biased maximum angle.  This is the angle at the maximum retracted
00462     // position, in biased units (sensor units, relative to the park angle).
00463     int biasedMax;
00464     
00465     // Mininum and maximum angle observed since last calibration start, on 
00466     // the biased scale
00467     int biasedMinObserved;
00468     int biasedMaxObserved;
00469     
00470     // The "alpha" angle - the angle between the park position and the
00471     // vertical line between the rotation axis and the plunger.  This is
00472     // represented in radians.
00473     float alpha;
00474     
00475     // The linear scaling factor, applied in our trig calculation from
00476     // angle to linear position.  This corresponds to the distance from
00477     // the rotation center to the plunger rod, but since the linear result
00478     // is in abstract joystick units, this distance is likewise in abstract
00479     // units.  The value isn't chosen to correspond to any real-world 
00480     // distance units, but rather to yield a joystick result that takes
00481     // advantage of most of the available axis range, to minimize rounding
00482     // errors when converting between scales.
00483     float linearScaleFactor;
00484 
00485     // timer for input timestamps and read timing measurements
00486     Timer timer;
00487     
00488     // read timing statistics
00489     uint64_t totalReadTime;
00490     uint64_t nReads;
00491     
00492     // Keep track of when calibration is in progress.  The calibration
00493     // procedure is usually handled by the generic main loop code, but
00494     // in this case, we have to keep track of some of the raw sensor
00495     // data during calibration for our own internal purposes.
00496     bool calibrating;
00497 };
00498 
00499 // Specialization for the AEAT-601X sensors
00500 template<int nDataBits> class PlungerSensorAEAT601X : public PlungerSensorRotary
00501 {
00502 public:
00503     PlungerSensorAEAT601X(PinName csPin, PinName clkPin, PinName doPin) :
00504         PlungerSensorRotary((1 << nDataBits) - 1, 6.283185f/((1 << nDataBits) - 1)),
00505         aeat(csPin, clkPin, doPin) 
00506     {
00507         // Make sure the sensor has had time to finish initializing.
00508         // Power-up time (tCF) from the data sheet is 20ms for the 12-bit
00509         // version, 50ms for the 10-bit version.
00510         wait_ms(nDataBits == 12 ? 20 :
00511             nDataBits == 10 ? 50 :
00512             50);
00513     }
00514 
00515     // read the angle
00516     virtual bool readSensor(int &angle)
00517     {
00518         angle = aeat.readAngle();
00519         return true;
00520     }
00521         
00522 protected:
00523     // physical sensor interface
00524     AEAT601X<nDataBits> aeat;
00525 };
00526 
00527 #endif