Mirror with some correction
Dependencies: mbed FastIO FastPWM USBDevice
Diff: Plunger/rotarySensor.h
- Revision:
- 100:1ff35c07217c
- Child:
- 102:41d49e78c253
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Plunger/rotarySensor.h Thu Nov 28 23:18:23 2019 +0000 @@ -0,0 +1,497 @@ +// Plunger sensor implementation for rotary absolute encoders +// +// This implements the plunger interfaces for rotary absolute encoders. A +// rotary encoder measures the angle of a rotating shaft. For plunger sensing, +// we can convert the plunger's linear motion into angular motion using a +// mechanical linkage between the plunger rod and a rotating shaft positioned +// at a fixed point, somewhere nearby, but off of the plunger's axis: +// +// =X=======================|=== <- plunger, X = connector attachment point +// \ +// \ <- connector between plunger and shaft +// \ +// * <- rotating shaft, at a fixed position +// +// As the plunger moves, the angle of the connector relative to the fixed +// shaft position changes in a predictable way, so by measuring the rotational +// position of the shaft at any given time, we can infer the plunger's +// linear position. +// +// (Note that the mechanical diagram is simplified for ASCII art purposes. +// What's not shown is that the distance between the rotating shaft and the +// "X" connection point on the plunger varies as the plunger moves, so the +// mechanical linkage requires some way to accommodate that changing length. +// One way is to use a spring as the linkage; another is to use a rigid +// connector with a sliding coupling at one or the other end. We leave +// these details up to the mechanical design; the software isn't affected +// as long as the basic relationship between linear and angular motion as +// shown in the diagram be achieved.) +// +// +// Translating the angle to a linear position +// +// There are two complications to translating the angular reading back to +// a linear plunger position. +// +// 1. We have to consider the sensor's zero point to be arbitrary. That means +// that the zero point could be somewhere within the plunger's travel range, +// so readings might "wrap" - e.g., we might see a series of readings when +// the plunger is moving in one direction like 4050, 4070, 4090, 14, 34 (note +// how we've "wrapped" past the 4096 boundary). +// +// To deal with this, we have to make a couple of assumptions: +// +// - The park position is at about 1/6 of the overall travel range +// - The total angular travel range is less than one full revolution +// +// With those assumptions in hand, we can bias the raw readings to the +// park position, and then take them modulo the raw scale. That will +// ensure that readings wrap properly, regardless of where the raw zero +// point lies. +// +// 2. Going back to the original diagram, you can see that the angle doesn't +// vary linearly with the plunger position. It actually varies sinusoidally. +// Let's use the vertical line between the plunger and the rotation point as +// the zero-degree reference point. To figure the plunger position, then, +// we need to figure the difference between the raw angle reading and the +// zero-degree point; call this theta. Let L be the position of the plunger +// relative to the vertical reference point, let D be the length of the +// vertical reference point line, and let H by the distance from the rotation +// point to the plunger connection point. This is a right triangle with +// hypotenuse H and sides L and D. D is a constant, because the rotation +// point never moves, and the plunger never moves vertically. Thus we can +// calculate D = H*cos(theta) and L = H*sin(theta). D is a constant, so +// we can figure H = D/cos(theta) hence L = D*sin(theta)/cos(theta) or +// D*tan(theta). If we wanted to know the true position in real-world +// units, we'd have to know D, but only need arbitrary linear units, so +// we can choose whatever value for D we find convenient: in particular, +// a value that gives us the desired range and resolution for the final +// result. +// +// Note that the tangent diverges at +/-90 degrees, but that's not a problem +// for the mechanical setup we've described, as the motion is inherently +// limited to stay within this range. +// +// There's still one big piece missing here: we somehow have to know where +// that vertical zero point lies. That's something we can only learn by +// calibration. Unfortunately, we don't have a good way to detect this +// directly. We *could* ask the user to look inside the cabinet and press +// a button when the needle is straight up, but that seems too cumbersome. +// What we'll do instead is provide some mechanical installation guidelines +// about where the rotation point should be positioned, and then use the +// full range to deduce the vertical position. +// +// The full range we actually have after calibration consists of the park +// position and the maximum retracted position. We could in principle also +// calibrate the maximum forward position, but that can't be read as reliably +// as the other two, because the barrel spring makes it difficult for the +// user to be sure they've pushed it all the way forward. Since we can +// extract the information we need from the park and max retract positions, +// it's better to rely on those alone and not ask for information that the +// user can't as easily provide. Given these positions, AND the assumption +// that the rotation point is at the midpoint of the plunger travel range, +// we can do some rather grungy trig work to come up with a formula for the +// angle between the park position and the vertical: +// +// let C1 = 1 1/32" (distance from midpoint to park), +// C2 = 1 17/32" (distance from midpoint to max retract), +// C = C2/C1 = 1.48484849, +// alpha = angle from park to vertical, +// beta = angle from max retract to vertical +// theta = alpha + beta = angle from park to max retract, known from calibration, +// T = tan(theta); +// +// then +// alpha = atan(sqrt(4*T*T*C + C^2 + 2*C + 1) - C - 1)/(2*T*C)) +// +// Did I mention this was grungy? At any rate, everything going into this +// formula is either constant or known from the calibration, so we can +// pre-compute alpha and store it after each calibration operation. And +// knowing alpha, we can translate an angle reading from the sensor to an +// angle relative to the vertical, which we can plug into D*tan(angle) to +// get a linear reading. +// +// If you're wondering how we derived that ugly formula, read on. Start +// with the basic relationships D*tan(alpha) = C1 and D*tan(beta) = C2. +// This lets us write tan(beta) in terms of tan(alpha) as +// C2/C1*tan(alpha) = C*tan(alpha). We can combine this with an identity +// for the tan of a sum of angles: +// +// tan(alpha + beta) = (tan(alpha) + tan(beta))/(1 - tan(alpha)*tan(beta)) +// +// to obtain: +// +// tan(theta) = tan(alpha + beta) = (1 + C*tan(alpha))/(1 - C*tan^2(alpha)) +// +// Everything here except alpha is known, so we now have a quadratic equation +// for tan(alpha). We can solve that by cranking through the normal algorithm +// for solving a quadratic equation, arriving at the solution above. +// +// +// Choosing an install position +// +// There are two competing factors in choosing the optimal "D". On the one +// hand, you'd like D to be as large as possible, to maximum linearity of the +// tan function used to translate angle to linear position. Higher linearity +// gives us greater immunity to variations in the precise centering of the +// rotation axis in the plunger travel range. tan() is pretty linear within +// about +/- 30 degrees. On the other hand, you'd like D to be as small as +// possible so that we get the largest overall angle range. Our sensor has +// a fixed angular resolution, so the more of the overall circle we use, the +// more sensor increments we have over the range, and thus the better +// effective linear resolution. +// +// Let's do some calculations for various "D" values (vertical distance +// between rotation point and plunger rod). For the effective DPI, we'll +// 12-bit angular resolution, per the AEAT-6012 sensor. +// +// D theta(max) eff dpi theta(park) +// ----------------------------------------------- +// 1 17/32" 45 deg 341 34 deg +// 2" 37 deg 280 27 deg +// 2 21/32" 30 deg 228 21 deg +// 3 1/4" 25 deg 190 17 deg +// 4 3/16" 20 deg 152 14 deg +// +// I'd consider 50 dpi to be the minimum for acceptable performance, 100 dpi +// to be excellent, and anything above 300 dpi to be diminishing returns. So +// for a 12-bit sensor, 2" looks like the sweet spot. It doesn't take us far +// outside of the +/-30 deg zone of tan() linearity, and it achieves almost +// 300 dpi of effective linear resolution. I'd stop there are not try to +// push the angular resolution higher with a shorter D; with the 45 deg +// theta(max) at D = 1-17/32", we'd get a lovely DPI level of 341, but at +// the cost of getting pretty non-linear around the ends of the plunger +// travel. Our math corrects for the non-linearity, but the more of that +// correction we need, the more sensitive the whole contraption becomes to +// getting the sensor positioning exactly right. The closer we can stay to +// the linear approximation, the more tolerant we are of inexact sensor +// positioning. +// +// +// Supported sensors +// +// * AEAT-6012-A06. This is a magnetic absolute encoder with 12-bit +// resolution. It linearly encodes one full (360 degree) rotation in +// 4096 increments, so each increment represents 360/4096 = .088 degrees. +// +// The base class doesn't actually care much about the sensor type; all it +// needs from the sensor is an angle reading represented on an arbitrary +// linear scale. ("Linear" in the angle, so that one increment represents +// a fixed number of degrees of arc. The full scale can represent one full +// turn but doesn't have to, as long as the scale is linear over the range +// covered.) To add new sensor types, you just need to add the code to +// interface to the physical sensor and return its reading on an arbitrary +// linear scale. + +#ifndef _ROTARYSENSOR_H_ +#define _ROTARYSENSOR_H_ + +#include "FastInterruptIn.h" +#include "AEAT6012.h" + +// The conversion from raw sensor reading to linear position involves a +// bunch of translations to different scales and unit systems. To help +// keep things straight, let's give each scale a name: +// +// * "Raw" refers to the readings directly from the sensor. These are +// unsigned ints in the range 0..scaleMax, and represent angles in a +// unit system where one increment equals 360/scaleMax degrees. The +// zero point is arbitrary, determined by the physical orientation +// of the sensor. +// +// * "Biased" refers to angular units with a zero point equal to the +// park position. This uses the same units as the "raw" system, but +// the zero point is adjusted so that 0 always means the park position. +// Negative values are forward of the park position. This scale is +// also adjusted for wrapping, by ensuring that the value lies in the +// range -(maximum forward excursion) to +(scale max - max fwd excursion). +// Any values below or above the range are bumped up or down (respectively) +// to wrap them back into the range. +// +// * "Linear" refers to the final linear results, in joystick units, on +// the abstract integer scale from 0..65535 used by the generic plunger +// base class. +// +class PlungerSensorRotary: public PlungerSensor +{ +public: + PlungerSensorRotary(int scaleMax, float radiansPerSensorUnit) : + PlungerSensor(65535), + scaleMax(scaleMax), + radiansPerSensorUnit(radiansPerSensorUnit) + { + // start our sample timer with an arbitrary zero point of now + timer.start(); + + // clear the timing statistics + nReads = 0; + totalReadTime = 0; + + // Pre-calculate the maximum forward excursion distance, in raw + // units. For our reference mechanical setup with "D" in a likely + // range, theta(max) is always about 10 degrees higher than + // theta(park). 10 degrees is about 1/36 of the overall circle, + // which is the same as 1/36 of the sensor scale. To be + // conservative, allow for about 3X that, so allow 1/12 of scale + // as the maximum forward excursion. For wrapping purposes, we'll + // consider any reading outside of the range from -(excursion) + // to +(scaleMax - excursion) to be wrapped. + maxForwardExcursion = scaleMax/12; + + // reset the calibration counters + biasedMinObserved = biasedMaxObserved = 0; + } + + // Restore the saved calibration at startup + virtual void restoreCalibration(Config &cfg) + { + // only proceed if there's calibration data to retrieve + if (cfg.plunger.cal.calibrated) + { + // we store the raw park angle in raw0 + rawParkAngle = cfg.plunger.cal.raw0; + + // we store biased max angle in raw1 + biasedMax = cfg.plunger.cal.raw1; + } + else + { + // Use the current sensor reading as the initial guess at the + // park position. The system is usually powered up with the + // plunger at the neutral position, so this is a good guess in + // most cases. If the plunger has been calibrated, we'll restore + // the better guess when we restore the configuration later on in + // the initialization process. + rawParkAngle = 0; + readSensor(rawParkAngle); + + // Set an initial wild guess at a range equal to +/-35 degrees. + // Note that this is in the "biased" coordinate system - raw + // units, but relative to the park angle. The park angle is + // about 25 degrees in this setup. + biasedMax = (35 + 25) * scaleMax/360; + } + + // recalculate the vertical angle + updateAlpha(); + } + + // Begin calibration + virtual void beginCalibration(Config &) + { + // Calibration starts out with the plunger at the park position, so + // we can take the current sensor reading to be the park position. + rawParkAngle = 0; + readSensor(rawParkAngle); + + // Reset the observed calibration counters + biasedMinObserved = biasedMaxObserved = 0; + } + + // End calibration + virtual void endCalibration(Config &cfg) + { + // apply the observed maximum angle + biasedMax = biasedMaxObserved; + + // recalculate the vertical angle + updateAlpha(); + + // save our raw configuration data + cfg.plunger.cal.raw0 = static_cast<uint16_t>(rawParkAngle); + cfg.plunger.cal.raw1 = static_cast<uint16_t>(biasedMax); + + // Refigure the range for the generic code + cfg.plunger.cal.min = biasedAngleToLinear(biasedMinObserved); + cfg.plunger.cal.max = biasedAngleToLinear(biasedMaxObserved); + cfg.plunger.cal.zero = biasedAngleToLinear(0); + } + + // figure the average scan time in microseconds + virtual uint32_t getAvgScanTime() + { + return nReads == 0 ? 0 : static_cast<uint32_t>(totalReadTime / nReads); + } + + // read the sensor + virtual bool readRaw(PlungerReading &r) + { + // note the starting time for the reading + uint32_t t0 = timer.read_us(); + + // read the angular position + int angle; + if (!readSensor(angle)) + return false; + + // Refigure the angle relative to the raw park position. This + // is the "biased" angle. + angle -= rawParkAngle; + + // Adjust for wrapping. + // + // An angular sensor reports the position on a circular scale, for + // obvious reasons, so there's some point along the circle where the + // angle is zero. One tick before that point reads as the maximum + // angle on the scale, so we say that the scale "wraps" at that point. + // + // To correct for this, we can look to the layout of the mechanical + // setup to constrain the values. Consider anything below the maximum + // forward exclusion to be wrapped on the low side, and consider + // anything outside of the complementary range on the high side to + // be wrapped on the high side. + if (angle < -maxForwardExcursion) + angle += scaleMax; + else if (angle >= scaleMax - maxForwardExcursion) + angle -= scaleMax; + + // Note if this is the highest/lowest observed reading on the biased + // scale since the last calibration started. + if (angle > biasedMaxObserved) + biasedMaxObserved = angle; + if (angle < biasedMinObserved) + biasedMinObserved = angle; + + // figure the linear result + r.pos = biasedAngleToLinear(angle); + + // Set the timestamp on the reading to right now + uint32_t now = timer.read_us(); + r.t = now; + + // count the read statistics + totalReadTime += now - t0; + nReads += 1; + + // success + return true; + } + +private: + // Read the underlying sensor - implemented by the hardware-specific + // subclasses. Returns true on success, false if the sensor can't + // be read. The angle is returned in raw sensor units. + virtual bool readSensor(int &angle) = 0; + + // Convert a biased angle value to a linear reading + int biasedAngleToLinear(int angle) + { + // Translate to an angle relative to the vertical, in sensor units + int angleFromVert = angle - alpha; + + // Translate the angle in sensor units to radians + float theta = static_cast<float>(angleFromVert) * radiansPerSensorUnit; + + // Calculate the linear position + return static_cast<int>(tanf(theta) * linearScaleFactor); + } + + // Update the estimation of the vertical angle, based on the angle + // between the park position and maximum retraction point. + void updateAlpha() + { + // See the comments at the top of the file for details on this + // formula. This figures the angle between the park position + // and the vertical by applying the known constraints of the + // mechanical setup: the known length of a standard plunger, + // and the requirement that the rotation axis be placed at + // roughly the midpoint of the plunger travel. + const float C = 1.4848489f; // 1-17/32" / 1-1/32" + float T = tanf(biasedMax * radiansPerSensorUnit); + float a = atanf((sqrtf(4*T*T*C + C*C + 2*C + 1) - C - 1)/(2*T*C)); + + // Convert back to sensor units. Note that alpha represents + // a relative angle between two specific points, so it's not + // further biased to any absolute point. + alpha = static_cast<int>(a / radiansPerSensorUnit); + + // While we're at it, figure the linear conversion factor. The + // goal is to use most of the abstract axis range, 0..65535. + // To avoid overflow, though, leave a little headroom. + const float safeMax = 60000.0f; + linearScaleFactor = static_cast<int>(safeMax / + tanf(static_cast<float>(biasedMax - alpha) * radiansPerSensorUnit)); + } + + // Maximum raw angular reading from the sensor. The sensor's readings + // will always be on a scale from 0..maxAngle. + int scaleMax; + + // Radians per sensor unit. This is a constant for the sensor. + float radiansPerSensorUnit; + + // Pre-calculated value of the maximum forward excursion, in raw units. + int maxForwardExcursion; + + // Raw reading at the park position. We use this to handle "wrapping", + // if the sensor's raw zero reading position is within the plunger travel + // range. All readings are taken to be within + int rawParkAngle; + + // Biased maximum angle. This is the angle at the maximum retracted + // position, in biased units (sensor units, relative to the park angle). + int biasedMax; + + // Mininum and maximum angle observed since last calibration start, on + // the biased scale + int biasedMinObserved; + int biasedMaxObserved; + + // The "alpha" angle - the angle between the park position and the + // vertical line between the rotation axis and the plunger. This is + // represented in sensor units. + int alpha; + + // The linear scaling factor, applied in our trig calculation from + // angle to linear position. This corresponds to the distance from + // the rotation center to the plunger rod, but since the linear result + // is in abstract joystick units, this distance is likewise in abstract + // units. The value isn't chosen to correspond to any real-world + // distance units, but rather to yield a joystick result that takes + // advantage of most of the available axis range, to minimize rounding + // errors when converting between scales. + float linearScaleFactor; + + // timer for input timestamps and read timing measurements + Timer timer; + + // read timing statistics + uint64_t totalReadTime; + uint64_t nReads; + + // Keep track of when calibration is in progress. The calibration + // procedure is usually handled by the generic main loop code, but + // in this case, we have to keep track of some of the raw sensor + // data during calibration for our own internal purposes. + bool calibrating; +}; + +// Specialization for the AEAT-601X sensors +template<int nDataBits> class PlungerSensorAEAT601X : public PlungerSensorRotary +{ +public: + PlungerSensorAEAT601X(PinName csPin, PinName clkPin, PinName doPin) : + PlungerSensorRotary((1 << nDataBits) - 1, 6.283185f/((1 << nDataBits) - 1)), + aeat(csPin, clkPin, doPin) + { + // Make sure the sensor has had time to finish initializing. + // Power-up time (tCF) from the data sheet is 20ms for the 12-bit + // version, 50ms for the 10-bit version. + wait_ms(nDataBits == 12 ? 20 : + nDataBits == 10 ? 50 : + 50); + } + + // read the angle + virtual bool readSensor(int &angle) + { + angle = aeat.readAngle(); + return true; + } + +protected: + // physical sensor interface + AEAT601X<nDataBits> aeat; +}; + +#endif