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
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
Generated on Wed Jul 13 2022 03:30:11 by 1.7.2