Arnaud VALLEY / Mbed 2 deprecated Pinscape_Controller_V2_arnoz

Mirror with some correction

Dependencies:   mbed FastIO FastPWM USBDevice

Plunger/rotarySensor.h@116:7a67265d7c19, 2021-10-01 (annotated)

Committer:
arnoz
Date:
Fri Oct 01 08:19:46 2021 +0000
Revision:
116:7a67265d7c19
Parent:
106:e9e3b46132c1 
- Correct information regarding your last merge

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mjr 100:1ff35c07217c 1 // Plunger sensor implementation for rotary absolute encoders
mjr 100:1ff35c07217c 2 //
mjr 100:1ff35c07217c 3 // This implements the plunger interfaces for rotary absolute encoders. A
mjr 106:e9e3b46132c1 4 // rotary encoder measures the angle of a rotating shaft. An absolute encoder
mjr 106:e9e3b46132c1 5 // is one where the microcontroller can ask the sensor for its current angular
mjr 106:e9e3b46132c1 6 // position at any time. (As opposed to incremental encoders, which don't have
mjr 106:e9e3b46132c1 7 // any notion of their current position, but can only signal the host on each
mjr 106:e9e3b46132c1 8 // change in position.)
mjr 106:e9e3b46132c1 9 //
mjr 106:e9e3b46132c1 10 //
mjr 106:e9e3b46132c1 11 // For plunger sensing, we can convert the plunger's linear motion into angular
mjr 106:e9e3b46132c1 12 // motion using a mechanical link between the plunger rod and a rotating shaft
mjr 106:e9e3b46132c1 13 // positioned at a fixed point, somewhere nearby, but away from the plunger's
mjr 106:e9e3b46132c1 14 // axis of motion:
mjr 100:1ff35c07217c 15 //
mjr 100:1ff35c07217c 16 // =X=======================|=== <- plunger, X = connector attachment point
mjr 100:1ff35c07217c 17 // \
mjr 100:1ff35c07217c 18 // \ <- connector between plunger and shaft
mjr 100:1ff35c07217c 19 // \
mjr 100:1ff35c07217c 20 // * <- rotating shaft, at a fixed position
mjr 100:1ff35c07217c 21 //
mjr 100:1ff35c07217c 22 // As the plunger moves, the angle of the connector relative to the fixed
mjr 106:e9e3b46132c1 23 // shaft position changes in a predictable way, so we can infer the plunger's
mjr 106:e9e3b46132c1 24 // linear position at any given time by measuring the current rotational
mjr 106:e9e3b46132c1 25 // angle of the shaft.
mjr 100:1ff35c07217c 26 //
mjr 106:e9e3b46132c1 27 // The mechanical diagram above is, obviously, simplified for ASCII art's sake.
mjr 100:1ff35c07217c 28 // What's not shown is that the distance between the rotating shaft and the
mjr 100:1ff35c07217c 29 // "X" connection point on the plunger varies as the plunger moves, so the
mjr 100:1ff35c07217c 30 // mechanical linkage requires some way to accommodate that changing length.
mjr 106:e9e3b46132c1 31 // If the connector is a rigid rod, it has to be able to slide at one or
mjr 106:e9e3b46132c1 32 // the other connection points. Alternatively, rather than using a rigid
mjr 106:e9e3b46132c1 33 // linkage, we can use a spring or elastic band. We leave these details up
mjr 106:e9e3b46132c1 34 // to the mechanical design, since the software isn't affected by that, as
mjr 106:e9e3b46132c1 35 // long as the basic relationship between linear and angular motion as shown
mjr 106:e9e3b46132c1 36 // in the diagram is achieved.
mjr 100:1ff35c07217c 37 //
mjr 100:1ff35c07217c 38 //
mjr 100:1ff35c07217c 39 // Translating the angle to a linear position
mjr 100:1ff35c07217c 40 //
mjr 100:1ff35c07217c 41 // There are two complications to translating the angular reading back to
mjr 100:1ff35c07217c 42 // a linear plunger position.
mjr 100:1ff35c07217c 43 //
mjr 106:e9e3b46132c1 44 // 1. We have to consider the sensor's zero point to be arbitrary, because
mjr 106:e9e3b46132c1 45 // these sorts of sensors don't typically give the user a way to align the
mjr 106:e9e3b46132c1 46 // zero point at a desired physical position. The zero point will just be
mjr 106:e9e3b46132c1 47 // wherever it ends up after installation. The zero point could easily end
mjr 106:e9e3b46132c1 48 // up being somewhere in the middle of the plunger's travel range, which
mjr 106:e9e3b46132c1 49 // means that readings might "wrap" - e.g., we might see a series of readings
mjr 106:e9e3b46132c1 50 // when the plunger is moving in one direction like this: 4050, 4070, 4090,
mjr 106:e9e3b46132c1 51 // 14, 34 (note how we "wrapped" past some maximum angle reading for the
mjr 106:e9e3b46132c1 52 // sensor and went back to zero, then continued from there).
mjr 100:1ff35c07217c 53 //
mjr 100:1ff35c07217c 54 // To deal with this, we have to make a couple of assumptions:
mjr 100:1ff35c07217c 55 //
mjr 100:1ff35c07217c 56 // - The park position is at about 1/6 of the overall travel range
mjr 100:1ff35c07217c 57 // - The total angular travel range is less than one full revolution
mjr 100:1ff35c07217c 58 //
mjr 100:1ff35c07217c 59 // With those assumptions in hand, we can bias the raw readings to the
mjr 100:1ff35c07217c 60 // park position, and then take them modulo the raw scale. That will
mjr 100:1ff35c07217c 61 // ensure that readings wrap properly, regardless of where the raw zero
mjr 100:1ff35c07217c 62 // point lies.
mjr 100:1ff35c07217c 63 //
mjr 103:dec22cd65b2a 64 // 2. Going back to the original diagram, you can see that there's some
mjr 103:dec22cd65b2a 65 // trigonometry required to interpret the sensor's angular reading as a
mjr 103:dec22cd65b2a 66 // linear position on the plunger axis, which is of course what we need
mjr 103:dec22cd65b2a 67 // to report to the PC software.
mjr 103:dec22cd65b2a 68 //
mjr 103:dec22cd65b2a 69 // Let's use the vertical line between the plunger and the rotation point
mjr 103:dec22cd65b2a 70 // as the zero-degree reference point. To figure the plunger position,
mjr 100:1ff35c07217c 71 // we need to figure the difference between the raw angle reading and the
mjr 100:1ff35c07217c 72 // zero-degree point; call this theta. Let L be the position of the plunger
mjr 100:1ff35c07217c 73 // relative to the vertical reference point, let D be the length of the
mjr 100:1ff35c07217c 74 // vertical reference point line, and let H by the distance from the rotation
mjr 100:1ff35c07217c 75 // point to the plunger connection point. This is a right triangle with
mjr 100:1ff35c07217c 76 // hypotenuse H and sides L and D. D is a constant, because the rotation
mjr 100:1ff35c07217c 77 // point never moves, and the plunger never moves vertically. Thus we can
mjr 100:1ff35c07217c 78 // calculate D = H*cos(theta) and L = H*sin(theta). D is a constant, so
mjr 100:1ff35c07217c 79 // we can figure H = D/cos(theta) hence L = D*sin(theta)/cos(theta) or
mjr 100:1ff35c07217c 80 // D*tan(theta). If we wanted to know the true position in real-world
mjr 100:1ff35c07217c 81 // units, we'd have to know D, but only need arbitrary linear units, so
mjr 100:1ff35c07217c 82 // we can choose whatever value for D we find convenient: in particular,
mjr 100:1ff35c07217c 83 // a value that gives us the desired range and resolution for the final
mjr 100:1ff35c07217c 84 // result.
mjr 100:1ff35c07217c 85 //
mjr 103:dec22cd65b2a 86 // Note that the tangent diverges at +/-90 degrees, but that's okay,
mjr 103:dec22cd65b2a 87 // because the mechanical setup we've described is inherently constrained
mjr 103:dec22cd65b2a 88 // to stay well within those limits. This would even be true for an
mjr 103:dec22cd65b2a 89 // arbitrarily long range of motion along the travel axis, but we don't
mjr 103:dec22cd65b2a 90 // even have to worry about that since we have such a well-defined range
mjr 103:dec22cd65b2a 91 // of travel (of only about 3") to track.
mjr 100:1ff35c07217c 92 //
mjr 100:1ff35c07217c 93 // There's still one big piece missing here: we somehow have to know where
mjr 100:1ff35c07217c 94 // that vertical zero point lies. That's something we can only learn by
mjr 100:1ff35c07217c 95 // calibration. Unfortunately, we don't have a good way to detect this
mjr 100:1ff35c07217c 96 // directly. We *could* ask the user to look inside the cabinet and press
mjr 103:dec22cd65b2a 97 // a button when the needle is straight up, but that seems too cumbersome
mjr 103:dec22cd65b2a 98 // for the user, not to mention terribly imprecise. So we'll approach this
mjr 103:dec22cd65b2a 99 // from the other direction: we'll assume a particular placement of the
mjr 103:dec22cd65b2a 100 // rotation point relative to the travel range, and we'll provide
mjr 103:dec22cd65b2a 101 // installation instructions to achieve that assumed alignment.
mjr 100:1ff35c07217c 102 //
mjr 100:1ff35c07217c 103 // The full range we actually have after calibration consists of the park
mjr 100:1ff35c07217c 104 // position and the maximum retracted position. We could in principle also
mjr 100:1ff35c07217c 105 // calibrate the maximum forward position, but that can't be read as reliably
mjr 100:1ff35c07217c 106 // as the other two, because the barrel spring makes it difficult for the
mjr 100:1ff35c07217c 107 // user to be sure they've pushed it all the way forward. Since we can
mjr 100:1ff35c07217c 108 // extract the information we need from the park and max retract positions,
mjr 100:1ff35c07217c 109 // it's better to rely on those alone and not ask for information that the
mjr 100:1ff35c07217c 110 // user can't as easily provide. Given these positions, AND the assumption
mjr 100:1ff35c07217c 111 // that the rotation point is at the midpoint of the plunger travel range,
mjr 103:dec22cd65b2a 112 // we can do some grungy trig work to come up with a formula for the angle
mjr 103:dec22cd65b2a 113 // between the park position and the vertical:
mjr 100:1ff35c07217c 114 //
mjr 100:1ff35c07217c 115 // let C1 = 1 1/32" (distance from midpoint to park),
mjr 100:1ff35c07217c 116 // C2 = 1 17/32" (distance from midpoint to max retract),
mjr 100:1ff35c07217c 117 // C = C2/C1 = 1.48484849,
mjr 100:1ff35c07217c 118 // alpha = angle from park to vertical,
mjr 100:1ff35c07217c 119 // beta = angle from max retract to vertical
mjr 100:1ff35c07217c 120 // theta = alpha + beta = angle from park to max retract, known from calibration,
mjr 100:1ff35c07217c 121 // T = tan(theta);
mjr 100:1ff35c07217c 122 //
mjr 100:1ff35c07217c 123 // then
mjr 100:1ff35c07217c 124 // alpha = atan(sqrt(4*T*T*C + C^2 + 2*C + 1) - C - 1)/(2*T*C))
mjr 100:1ff35c07217c 125 //
mjr 103:dec22cd65b2a 126 // Did I mention this was grungy? At any rate, everything going into that
mjr 103:dec22cd65b2a 127 // last equation is either constant or known from the calibration, so we
mjr 103:dec22cd65b2a 128 // can pre-compute alpha and store it after each calibration operation.
mjr 103:dec22cd65b2a 129 // And once we've computed alpha, we can easily translate an angle reading
mjr 103:dec22cd65b2a 130 // from the sensor to an angle relative to the vertical, which we can plug
mjr 103:dec22cd65b2a 131 // into D*tan(angle) to convert to a linear position on the plunger axis.
mjr 103:dec22cd65b2a 132 //
mjr 103:dec22cd65b2a 133 // The final step is to scale that linear position into joystick reporting
mjr 103:dec22cd65b2a 134 // units. Those units are arbitrary, so we don't have to relate this to any
mjr 103:dec22cd65b2a 135 // real-world lengths. We can simply figure a scaling factor that maps the
mjr 103:dec22cd65b2a 136 // physical range to map to roughly the full range of the joystick units.
mjr 100:1ff35c07217c 137 //
mjr 100:1ff35c07217c 138 // If you're wondering how we derived that ugly formula, read on. Start
mjr 100:1ff35c07217c 139 // with the basic relationships D*tan(alpha) = C1 and D*tan(beta) = C2.
mjr 100:1ff35c07217c 140 // This lets us write tan(beta) in terms of tan(alpha) as
mjr 100:1ff35c07217c 141 // C2/C1*tan(alpha) = C*tan(alpha). We can combine this with an identity
mjr 100:1ff35c07217c 142 // for the tan of a sum of angles:
mjr 100:1ff35c07217c 143 //
mjr 100:1ff35c07217c 144 // tan(alpha + beta) = (tan(alpha) + tan(beta))/(1 - tan(alpha)*tan(beta))
mjr 100:1ff35c07217c 145 //
mjr 100:1ff35c07217c 146 // to obtain:
mjr 100:1ff35c07217c 147 //
mjr 100:1ff35c07217c 148 // tan(theta) = tan(alpha + beta) = (1 + C*tan(alpha))/(1 - C*tan^2(alpha))
mjr 100:1ff35c07217c 149 //
mjr 100:1ff35c07217c 150 // Everything here except alpha is known, so we now have a quadratic equation
mjr 100:1ff35c07217c 151 // for tan(alpha). We can solve that by cranking through the normal algorithm
mjr 100:1ff35c07217c 152 // for solving a quadratic equation, arriving at the solution above.
mjr 100:1ff35c07217c 153 //
mjr 100:1ff35c07217c 154 //
mjr 100:1ff35c07217c 155 // Choosing an install position
mjr 100:1ff35c07217c 156 //
mjr 100:1ff35c07217c 157 // There are two competing factors in choosing the optimal "D". On the one
mjr 100:1ff35c07217c 158 // hand, you'd like D to be as large as possible, to maximum linearity of the
mjr 100:1ff35c07217c 159 // tan function used to translate angle to linear position. Higher linearity
mjr 100:1ff35c07217c 160 // gives us greater immunity to variations in the precise centering of the
mjr 103:dec22cd65b2a 161 // rotation axis in the plunger travel range. tan() is pretty linear (that
mjr 103:dec22cd65b2a 162 // is, tan(theta) is approximately proportional to theta) for small theta,
mjr 103:dec22cd65b2a 163 // within about +/- 30 degrees. On the other hand, you'd like D to be as
mjr 103:dec22cd65b2a 164 // small as possible so that we get the largest overall angle range. Our
mjr 103:dec22cd65b2a 165 // sensor has a fixed angular resolution, so the more of the overall circle
mjr 103:dec22cd65b2a 166 // we use, the more sensor increments we have over the range, and thus the
mjr 103:dec22cd65b2a 167 // better effective linear resolution.
mjr 100:1ff35c07217c 168 //
mjr 100:1ff35c07217c 169 // Let's do some calculations for various "D" values (vertical distance
mjr 103:dec22cd65b2a 170 // between rotation point and plunger rod). We'll base our calculations
mjr 103:dec22cd65b2a 171 // on the AEAT-6012 sensor's 12-bit angular resolution.
mjr 100:1ff35c07217c 172 //
mjr 100:1ff35c07217c 173 // D theta(max) eff dpi theta(park)
mjr 100:1ff35c07217c 174 // -----------------------------------------------
mjr 100:1ff35c07217c 175 // 1 17/32" 45 deg 341 34 deg
mjr 100:1ff35c07217c 176 // 2" 37 deg 280 27 deg
mjr 100:1ff35c07217c 177 // 2 21/32" 30 deg 228 21 deg
mjr 100:1ff35c07217c 178 // 3 1/4" 25 deg 190 17 deg
mjr 100:1ff35c07217c 179 // 4 3/16" 20 deg 152 14 deg
mjr 100:1ff35c07217c 180 //
mjr 100:1ff35c07217c 181 // I'd consider 50 dpi to be the minimum for acceptable performance, 100 dpi
mjr 100:1ff35c07217c 182 // to be excellent, and anything above 300 dpi to be diminishing returns. So
mjr 100:1ff35c07217c 183 // for a 12-bit sensor, 2" looks like the sweet spot. It doesn't take us far
mjr 100:1ff35c07217c 184 // outside of the +/-30 deg zone of tan() linearity, and it achieves almost
mjr 100:1ff35c07217c 185 // 300 dpi of effective linear resolution. I'd stop there are not try to
mjr 100:1ff35c07217c 186 // push the angular resolution higher with a shorter D; with the 45 deg
mjr 100:1ff35c07217c 187 // theta(max) at D = 1-17/32", we'd get a lovely DPI level of 341, but at
mjr 100:1ff35c07217c 188 // the cost of getting pretty non-linear around the ends of the plunger
mjr 100:1ff35c07217c 189 // travel. Our math corrects for the non-linearity, but the more of that
mjr 100:1ff35c07217c 190 // correction we need, the more sensitive the whole contraption becomes to
mjr 100:1ff35c07217c 191 // getting the sensor positioning exactly right. The closer we can stay to
mjr 100:1ff35c07217c 192 // the linear approximation, the more tolerant we are of inexact sensor
mjr 100:1ff35c07217c 193 // positioning.
mjr 100:1ff35c07217c 194 //
mjr 100:1ff35c07217c 195 //
mjr 100:1ff35c07217c 196 // Supported sensors
mjr 100:1ff35c07217c 197 //
mjr 100:1ff35c07217c 198 // * AEAT-6012-A06. This is a magnetic absolute encoder with 12-bit
mjr 100:1ff35c07217c 199 // resolution. It linearly encodes one full (360 degree) rotation in
mjr 100:1ff35c07217c 200 // 4096 increments, so each increment represents 360/4096 = .088 degrees.
mjr 100:1ff35c07217c 201 //
mjr 100:1ff35c07217c 202 // The base class doesn't actually care much about the sensor type; all it
mjr 100:1ff35c07217c 203 // needs from the sensor is an angle reading represented on an arbitrary
mjr 100:1ff35c07217c 204 // linear scale. ("Linear" in the angle, so that one increment represents
mjr 100:1ff35c07217c 205 // a fixed number of degrees of arc. The full scale can represent one full
mjr 100:1ff35c07217c 206 // turn but doesn't have to, as long as the scale is linear over the range
mjr 100:1ff35c07217c 207 // covered.) To add new sensor types, you just need to add the code to
mjr 100:1ff35c07217c 208 // interface to the physical sensor and return its reading on an arbitrary
mjr 100:1ff35c07217c 209 // linear scale.
mjr 100:1ff35c07217c 210
mjr 100:1ff35c07217c 211 #ifndef _ROTARYSENSOR_H_
mjr 100:1ff35c07217c 212 #define _ROTARYSENSOR_H_
mjr 100:1ff35c07217c 213
mjr 100:1ff35c07217c 214 #include "FastInterruptIn.h"
mjr 100:1ff35c07217c 215 #include "AEAT6012.h"
mjr 100:1ff35c07217c 216
mjr 100:1ff35c07217c 217 // The conversion from raw sensor reading to linear position involves a
mjr 100:1ff35c07217c 218 // bunch of translations to different scales and unit systems. To help
mjr 100:1ff35c07217c 219 // keep things straight, let's give each scale a name:
mjr 100:1ff35c07217c 220 //
mjr 100:1ff35c07217c 221 // * "Raw" refers to the readings directly from the sensor. These are
mjr 103:dec22cd65b2a 222 // unsigned ints in the range 0..maxRawAngle, and represent angles in a
mjr 102:41d49e78c253 223 // unit system where one increment equals 360/maxRawAngle degrees. The
mjr 100:1ff35c07217c 224 // zero point is arbitrary, determined by the physical orientation
mjr 100:1ff35c07217c 225 // of the sensor.
mjr 100:1ff35c07217c 226 //
mjr 100:1ff35c07217c 227 // * "Biased" refers to angular units with a zero point equal to the
mjr 103:dec22cd65b2a 228 // park position. This uses the same unit size as the "raw" system, but
mjr 100:1ff35c07217c 229 // the zero point is adjusted so that 0 always means the park position.
mjr 100:1ff35c07217c 230 // Negative values are forward of the park position. This scale is
mjr 100:1ff35c07217c 231 // also adjusted for wrapping, by ensuring that the value lies in the
mjr 100:1ff35c07217c 232 // range -(maximum forward excursion) to +(scale max - max fwd excursion).
mjr 100:1ff35c07217c 233 // Any values below or above the range are bumped up or down (respectively)
mjr 100:1ff35c07217c 234 // to wrap them back into the range.
mjr 100:1ff35c07217c 235 //
mjr 100:1ff35c07217c 236 // * "Linear" refers to the final linear results, in joystick units, on
mjr 100:1ff35c07217c 237 // the abstract integer scale from 0..65535 used by the generic plunger
mjr 100:1ff35c07217c 238 // base class.
mjr 100:1ff35c07217c 239 //
mjr 100:1ff35c07217c 240 class PlungerSensorRotary: public PlungerSensor
mjr 100:1ff35c07217c 241 {
mjr 100:1ff35c07217c 242 public:
mjr 102:41d49e78c253 243 PlungerSensorRotary(int maxRawAngle, float radiansPerSensorUnit) :
mjr 100:1ff35c07217c 244 PlungerSensor(65535),
mjr 102:41d49e78c253 245 maxRawAngle(maxRawAngle),
mjr 100:1ff35c07217c 246 radiansPerSensorUnit(radiansPerSensorUnit)
mjr 100:1ff35c07217c 247 {
mjr 100:1ff35c07217c 248 // start our sample timer with an arbitrary zero point of now
mjr 100:1ff35c07217c 249 timer.start();
mjr 100:1ff35c07217c 250
mjr 100:1ff35c07217c 251 // clear the timing statistics
mjr 100:1ff35c07217c 252 nReads = 0;
mjr 100:1ff35c07217c 253 totalReadTime = 0;
mjr 100:1ff35c07217c 254
mjr 100:1ff35c07217c 255 // Pre-calculate the maximum forward excursion distance, in raw
mjr 100:1ff35c07217c 256 // units. For our reference mechanical setup with "D" in a likely
mjr 100:1ff35c07217c 257 // range, theta(max) is always about 10 degrees higher than
mjr 100:1ff35c07217c 258 // theta(park). 10 degrees is about 1/36 of the overall circle,
mjr 100:1ff35c07217c 259 // which is the same as 1/36 of the sensor scale. To be
mjr 100:1ff35c07217c 260 // conservative, allow for about 3X that, so allow 1/12 of scale
mjr 100:1ff35c07217c 261 // as the maximum forward excursion. For wrapping purposes, we'll
mjr 100:1ff35c07217c 262 // consider any reading outside of the range from -(excursion)
mjr 102:41d49e78c253 263 // to +(maxRawAngle - excursion) to be wrapped.
mjr 102:41d49e78c253 264 maxForwardExcursionRaw = maxRawAngle/12;
mjr 100:1ff35c07217c 265
mjr 100:1ff35c07217c 266 // reset the calibration counters
mjr 100:1ff35c07217c 267 biasedMinObserved = biasedMaxObserved = 0;
mjr 100:1ff35c07217c 268 }
mjr 100:1ff35c07217c 269
mjr 100:1ff35c07217c 270 // Restore the saved calibration at startup
mjr 100:1ff35c07217c 271 virtual void restoreCalibration(Config &cfg)
mjr 100:1ff35c07217c 272 {
mjr 100:1ff35c07217c 273 // only proceed if there's calibration data to retrieve
mjr 100:1ff35c07217c 274 if (cfg.plunger.cal.calibrated)
mjr 100:1ff35c07217c 275 {
mjr 100:1ff35c07217c 276 // we store the raw park angle in raw0
mjr 100:1ff35c07217c 277 rawParkAngle = cfg.plunger.cal.raw0;
mjr 100:1ff35c07217c 278
mjr 100:1ff35c07217c 279 // we store biased max angle in raw1
mjr 100:1ff35c07217c 280 biasedMax = cfg.plunger.cal.raw1;
mjr 100:1ff35c07217c 281 }
mjr 100:1ff35c07217c 282 else
mjr 100:1ff35c07217c 283 {
mjr 100:1ff35c07217c 284 // Use the current sensor reading as the initial guess at the
mjr 100:1ff35c07217c 285 // park position. The system is usually powered up with the
mjr 100:1ff35c07217c 286 // plunger at the neutral position, so this is a good guess in
mjr 100:1ff35c07217c 287 // most cases. If the plunger has been calibrated, we'll restore
mjr 100:1ff35c07217c 288 // the better guess when we restore the configuration later on in
mjr 100:1ff35c07217c 289 // the initialization process.
mjr 100:1ff35c07217c 290 rawParkAngle = 0;
mjr 100:1ff35c07217c 291 readSensor(rawParkAngle);
mjr 100:1ff35c07217c 292
mjr 100:1ff35c07217c 293 // Set an initial wild guess at a range equal to +/-35 degrees.
mjr 100:1ff35c07217c 294 // Note that this is in the "biased" coordinate system - raw
mjr 100:1ff35c07217c 295 // units, but relative to the park angle. The park angle is
mjr 102:41d49e78c253 296 // about -25 degrees in this setup.
mjr 102:41d49e78c253 297 biasedMax = (35 + 25) * maxRawAngle/360;
mjr 100:1ff35c07217c 298 }
mjr 100:1ff35c07217c 299
mjr 100:1ff35c07217c 300 // recalculate the vertical angle
mjr 100:1ff35c07217c 301 updateAlpha();
mjr 100:1ff35c07217c 302 }
mjr 100:1ff35c07217c 303
mjr 100:1ff35c07217c 304 // Begin calibration
mjr 100:1ff35c07217c 305 virtual void beginCalibration(Config &)
mjr 100:1ff35c07217c 306 {
mjr 100:1ff35c07217c 307 // Calibration starts out with the plunger at the park position, so
mjr 100:1ff35c07217c 308 // we can take the current sensor reading to be the park position.
mjr 100:1ff35c07217c 309 rawParkAngle = 0;
mjr 100:1ff35c07217c 310 readSensor(rawParkAngle);
mjr 100:1ff35c07217c 311
mjr 100:1ff35c07217c 312 // Reset the observed calibration counters
mjr 100:1ff35c07217c 313 biasedMinObserved = biasedMaxObserved = 0;
mjr 100:1ff35c07217c 314 }
mjr 100:1ff35c07217c 315
mjr 100:1ff35c07217c 316 // End calibration
mjr 100:1ff35c07217c 317 virtual void endCalibration(Config &cfg)
mjr 100:1ff35c07217c 318 {
mjr 100:1ff35c07217c 319 // apply the observed maximum angle
mjr 100:1ff35c07217c 320 biasedMax = biasedMaxObserved;
mjr 100:1ff35c07217c 321
mjr 100:1ff35c07217c 322 // recalculate the vertical angle
mjr 100:1ff35c07217c 323 updateAlpha();
mjr 100:1ff35c07217c 324
mjr 100:1ff35c07217c 325 // save our raw configuration data
mjr 100:1ff35c07217c 326 cfg.plunger.cal.raw0 = static_cast<uint16_t>(rawParkAngle);
mjr 100:1ff35c07217c 327 cfg.plunger.cal.raw1 = static_cast<uint16_t>(biasedMax);
mjr 100:1ff35c07217c 328
mjr 100:1ff35c07217c 329 // Refigure the range for the generic code
mjr 100:1ff35c07217c 330 cfg.plunger.cal.min = biasedAngleToLinear(biasedMinObserved);
mjr 100:1ff35c07217c 331 cfg.plunger.cal.max = biasedAngleToLinear(biasedMaxObserved);
mjr 100:1ff35c07217c 332 cfg.plunger.cal.zero = biasedAngleToLinear(0);
mjr 100:1ff35c07217c 333 }
mjr 100:1ff35c07217c 334
mjr 100:1ff35c07217c 335 // figure the average scan time in microseconds
mjr 100:1ff35c07217c 336 virtual uint32_t getAvgScanTime()
mjr 100:1ff35c07217c 337 {
mjr 100:1ff35c07217c 338 return nReads == 0 ? 0 : static_cast<uint32_t>(totalReadTime / nReads);
mjr 100:1ff35c07217c 339 }
mjr 100:1ff35c07217c 340
mjr 100:1ff35c07217c 341 // read the sensor
mjr 100:1ff35c07217c 342 virtual bool readRaw(PlungerReading &r)
mjr 100:1ff35c07217c 343 {
mjr 100:1ff35c07217c 344 // note the starting time for the reading
mjr 100:1ff35c07217c 345 uint32_t t0 = timer.read_us();
mjr 100:1ff35c07217c 346
mjr 100:1ff35c07217c 347 // read the angular position
mjr 100:1ff35c07217c 348 int angle;
mjr 100:1ff35c07217c 349 if (!readSensor(angle))
mjr 100:1ff35c07217c 350 return false;
mjr 102:41d49e78c253 351
mjr 100:1ff35c07217c 352 // Refigure the angle relative to the raw park position. This
mjr 100:1ff35c07217c 353 // is the "biased" angle.
mjr 100:1ff35c07217c 354 angle -= rawParkAngle;
mjr 100:1ff35c07217c 355
mjr 100:1ff35c07217c 356 // Adjust for wrapping.
mjr 100:1ff35c07217c 357 //
mjr 100:1ff35c07217c 358 // An angular sensor reports the position on a circular scale, for
mjr 100:1ff35c07217c 359 // obvious reasons, so there's some point along the circle where the
mjr 100:1ff35c07217c 360 // angle is zero. One tick before that point reads as the maximum
mjr 100:1ff35c07217c 361 // angle on the scale, so we say that the scale "wraps" at that point.
mjr 100:1ff35c07217c 362 //
mjr 100:1ff35c07217c 363 // To correct for this, we can look to the layout of the mechanical
mjr 100:1ff35c07217c 364 // setup to constrain the values. Consider anything below the maximum
mjr 100:1ff35c07217c 365 // forward exclusion to be wrapped on the low side, and consider
mjr 100:1ff35c07217c 366 // anything outside of the complementary range on the high side to
mjr 100:1ff35c07217c 367 // be wrapped on the high side.
mjr 102:41d49e78c253 368 if (angle < -maxForwardExcursionRaw)
mjr 102:41d49e78c253 369 angle += maxRawAngle;
mjr 102:41d49e78c253 370 else if (angle >= maxRawAngle - maxForwardExcursionRaw)
mjr 102:41d49e78c253 371 angle -= maxRawAngle;
mjr 100:1ff35c07217c 372
mjr 100:1ff35c07217c 373 // Note if this is the highest/lowest observed reading on the biased
mjr 100:1ff35c07217c 374 // scale since the last calibration started.
mjr 100:1ff35c07217c 375 if (angle > biasedMaxObserved)
mjr 100:1ff35c07217c 376 biasedMaxObserved = angle;
mjr 100:1ff35c07217c 377 if (angle < biasedMinObserved)
mjr 100:1ff35c07217c 378 biasedMinObserved = angle;
mjr 100:1ff35c07217c 379
mjr 100:1ff35c07217c 380 // figure the linear result
mjr 100:1ff35c07217c 381 r.pos = biasedAngleToLinear(angle);
mjr 102:41d49e78c253 382
mjr 100:1ff35c07217c 383 // Set the timestamp on the reading to right now
mjr 100:1ff35c07217c 384 uint32_t now = timer.read_us();
mjr 100:1ff35c07217c 385 r.t = now;
mjr 100:1ff35c07217c 386
mjr 100:1ff35c07217c 387 // count the read statistics
mjr 100:1ff35c07217c 388 totalReadTime += now - t0;
mjr 100:1ff35c07217c 389 nReads += 1;
mjr 100:1ff35c07217c 390
mjr 100:1ff35c07217c 391 // success
mjr 100:1ff35c07217c 392 return true;
mjr 100:1ff35c07217c 393 }
mjr 100:1ff35c07217c 394
mjr 100:1ff35c07217c 395 private:
mjr 100:1ff35c07217c 396 // Read the underlying sensor - implemented by the hardware-specific
mjr 100:1ff35c07217c 397 // subclasses. Returns true on success, false if the sensor can't
mjr 100:1ff35c07217c 398 // be read. The angle is returned in raw sensor units.
mjr 100:1ff35c07217c 399 virtual bool readSensor(int &angle) = 0;
mjr 100:1ff35c07217c 400
mjr 100:1ff35c07217c 401 // Convert a biased angle value to a linear reading
mjr 100:1ff35c07217c 402 int biasedAngleToLinear(int angle)
mjr 100:1ff35c07217c 403 {
mjr 100:1ff35c07217c 404 // Translate to an angle relative to the vertical, in sensor units
mjr 102:41d49e78c253 405 float theta = static_cast<float>(angle)*radiansPerSensorUnit - alpha;
mjr 100:1ff35c07217c 406
mjr 102:41d49e78c253 407 // Calculate the linear position relative to the vertical. Zero
mjr 102:41d49e78c253 408 // is right at the intersection of the vertical line from the
mjr 102:41d49e78c253 409 // sensor rotation center to the plunger axis; positive numbers
mjr 102:41d49e78c253 410 // are behind the vertical (more retracted).
mjr 102:41d49e78c253 411 int linearPos = static_cast<int>(tanf(theta) * linearScaleFactor);
mjr 100:1ff35c07217c 412
mjr 102:41d49e78c253 413 // Finally, figure the offset. The vertical is the halfway point
mjr 102:41d49e78c253 414 // of the plunger motion, so we want to put it at half of the raw
mjr 102:41d49e78c253 415 // scale of 0..65535.
mjr 102:41d49e78c253 416 return linearPos + 32767;
mjr 100:1ff35c07217c 417 }
mjr 100:1ff35c07217c 418
mjr 100:1ff35c07217c 419 // Update the estimation of the vertical angle, based on the angle
mjr 100:1ff35c07217c 420 // between the park position and maximum retraction point.
mjr 100:1ff35c07217c 421 void updateAlpha()
mjr 100:1ff35c07217c 422 {
mjr 100:1ff35c07217c 423 // See the comments at the top of the file for details on this
mjr 100:1ff35c07217c 424 // formula. This figures the angle between the park position
mjr 100:1ff35c07217c 425 // and the vertical by applying the known constraints of the
mjr 100:1ff35c07217c 426 // mechanical setup: the known length of a standard plunger,
mjr 100:1ff35c07217c 427 // and the requirement that the rotation axis be placed at
mjr 100:1ff35c07217c 428 // roughly the midpoint of the plunger travel.
mjr 100:1ff35c07217c 429 const float C = 1.4848489f; // 1-17/32" / 1-1/32"
mjr 102:41d49e78c253 430 float maxInRadians = static_cast<float>(biasedMax) * radiansPerSensorUnit;
mjr 102:41d49e78c253 431 float T = tanf(maxInRadians);
mjr 102:41d49e78c253 432 alpha = atanf((sqrtf(4*T*T*C + C*C + 2*C + 1) - C - 1)/(2*T*C));
mjr 102:41d49e78c253 433
mjr 102:41d49e78c253 434 // While we're at it, figure the linear conversion factor. Alpha
mjr 102:41d49e78c253 435 // represents the angle from the park position to the midpoint,
mjr 102:41d49e78c253 436 // which in the real world represents about 31/32", or just less
mjr 102:41d49e78c253 437 // then 1/3 of the overall travel. We want to normalize this to
mjr 102:41d49e78c253 438 // the corresponding fraction of our 0..65535 abstract linear unit
mjr 102:41d49e78c253 439 // system. To avoid overflow, normalize to a slightly smaller
mjr 102:41d49e78c253 440 // scale.
mjr 100:1ff35c07217c 441 const float safeMax = 60000.0f;
mjr 102:41d49e78c253 442 const float alphaInLinearUnits = safeMax * .316327f; // 31/22" / 3-1/16"
mjr 102:41d49e78c253 443 linearScaleFactor = static_cast<int>(alphaInLinearUnits / tanf(alpha));
mjr 100:1ff35c07217c 444 }
mjr 100:1ff35c07217c 445
mjr 100:1ff35c07217c 446 // Maximum raw angular reading from the sensor. The sensor's readings
mjr 102:41d49e78c253 447 // will always be on a scale from 0..maxRawAngle.
mjr 102:41d49e78c253 448 int maxRawAngle;
mjr 100:1ff35c07217c 449
mjr 100:1ff35c07217c 450 // Radians per sensor unit. This is a constant for the sensor.
mjr 100:1ff35c07217c 451 float radiansPerSensorUnit;
mjr 100:1ff35c07217c 452
mjr 100:1ff35c07217c 453 // Pre-calculated value of the maximum forward excursion, in raw units.
mjr 102:41d49e78c253 454 int maxForwardExcursionRaw;
mjr 100:1ff35c07217c 455
mjr 100:1ff35c07217c 456 // Raw reading at the park position. We use this to handle "wrapping",
mjr 100:1ff35c07217c 457 // if the sensor's raw zero reading position is within the plunger travel
mjr 100:1ff35c07217c 458 // range. All readings are taken to be within
mjr 100:1ff35c07217c 459 int rawParkAngle;
mjr 100:1ff35c07217c 460
mjr 100:1ff35c07217c 461 // Biased maximum angle. This is the angle at the maximum retracted
mjr 100:1ff35c07217c 462 // position, in biased units (sensor units, relative to the park angle).
mjr 100:1ff35c07217c 463 int biasedMax;
mjr 100:1ff35c07217c 464
mjr 100:1ff35c07217c 465 // Mininum and maximum angle observed since last calibration start, on
mjr 100:1ff35c07217c 466 // the biased scale
mjr 100:1ff35c07217c 467 int biasedMinObserved;
mjr 100:1ff35c07217c 468 int biasedMaxObserved;
mjr 100:1ff35c07217c 469
mjr 100:1ff35c07217c 470 // The "alpha" angle - the angle between the park position and the
mjr 100:1ff35c07217c 471 // vertical line between the rotation axis and the plunger. This is
mjr 102:41d49e78c253 472 // represented in radians.
mjr 102:41d49e78c253 473 float alpha;
mjr 100:1ff35c07217c 474
mjr 100:1ff35c07217c 475 // The linear scaling factor, applied in our trig calculation from
mjr 100:1ff35c07217c 476 // angle to linear position. This corresponds to the distance from
mjr 100:1ff35c07217c 477 // the rotation center to the plunger rod, but since the linear result
mjr 100:1ff35c07217c 478 // is in abstract joystick units, this distance is likewise in abstract
mjr 100:1ff35c07217c 479 // units. The value isn't chosen to correspond to any real-world
mjr 100:1ff35c07217c 480 // distance units, but rather to yield a joystick result that takes
mjr 100:1ff35c07217c 481 // advantage of most of the available axis range, to minimize rounding
mjr 100:1ff35c07217c 482 // errors when converting between scales.
mjr 100:1ff35c07217c 483 float linearScaleFactor;
mjr 100:1ff35c07217c 484
mjr 100:1ff35c07217c 485 // timer for input timestamps and read timing measurements
mjr 100:1ff35c07217c 486 Timer timer;
mjr 100:1ff35c07217c 487
mjr 100:1ff35c07217c 488 // read timing statistics
mjr 100:1ff35c07217c 489 uint64_t totalReadTime;
mjr 100:1ff35c07217c 490 uint64_t nReads;
mjr 100:1ff35c07217c 491
mjr 100:1ff35c07217c 492 // Keep track of when calibration is in progress. The calibration
mjr 100:1ff35c07217c 493 // procedure is usually handled by the generic main loop code, but
mjr 100:1ff35c07217c 494 // in this case, we have to keep track of some of the raw sensor
mjr 100:1ff35c07217c 495 // data during calibration for our own internal purposes.
mjr 100:1ff35c07217c 496 bool calibrating;
mjr 100:1ff35c07217c 497 };
mjr 100:1ff35c07217c 498
mjr 100:1ff35c07217c 499 // Specialization for the AEAT-601X sensors
mjr 100:1ff35c07217c 500 template<int nDataBits> class PlungerSensorAEAT601X : public PlungerSensorRotary
mjr 100:1ff35c07217c 501 {
mjr 100:1ff35c07217c 502 public:
mjr 100:1ff35c07217c 503 PlungerSensorAEAT601X(PinName csPin, PinName clkPin, PinName doPin) :
mjr 100:1ff35c07217c 504 PlungerSensorRotary((1 << nDataBits) - 1, 6.283185f/((1 << nDataBits) - 1)),
mjr 100:1ff35c07217c 505 aeat(csPin, clkPin, doPin)
mjr 100:1ff35c07217c 506 {
mjr 100:1ff35c07217c 507 // Make sure the sensor has had time to finish initializing.
mjr 100:1ff35c07217c 508 // Power-up time (tCF) from the data sheet is 20ms for the 12-bit
mjr 100:1ff35c07217c 509 // version, 50ms for the 10-bit version.
mjr 100:1ff35c07217c 510 wait_ms(nDataBits == 12 ? 20 :
mjr 100:1ff35c07217c 511 nDataBits == 10 ? 50 :
mjr 100:1ff35c07217c 512 50);
mjr 100:1ff35c07217c 513 }
mjr 100:1ff35c07217c 514
mjr 100:1ff35c07217c 515 // read the angle
mjr 100:1ff35c07217c 516 virtual bool readSensor(int &angle)
mjr 100:1ff35c07217c 517 {
mjr 100:1ff35c07217c 518 angle = aeat.readAngle();
mjr 100:1ff35c07217c 519 return true;
mjr 100:1ff35c07217c 520 }
mjr 100:1ff35c07217c 521
mjr 100:1ff35c07217c 522 protected:
mjr 100:1ff35c07217c 523 // physical sensor interface
mjr 100:1ff35c07217c 524 AEAT601X<nDataBits> aeat;
mjr 100:1ff35c07217c 525 };
mjr 100:1ff35c07217c 526
mjr 100:1ff35c07217c 527 #endif