Arnaud VALLEY / Mbed 2 deprecated Pinscape_Controller_V2_arnoz

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed FastIO FastPWM USBDevice

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

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mjr 82:4f6209cb5c33 1 // Plunger sensor type for distance sensors.
mjr 82:4f6209cb5c33 2 //
mjr 82:4f6209cb5c33 3 // This type of sensor measures the distance to a target by sending
mjr 82:4f6209cb5c33 4 // optical or sound signals and watching for the reflection. There are
mjr 82:4f6209cb5c33 5 // many types of these sensors, including sensors that measure the
mjr 82:4f6209cb5c33 6 // intensity of reflected sound or light signals, sensors that measure
mjr 82:4f6209cb5c33 7 // the round-trip time of "pings", and sensors that measure optical
mjr 82:4f6209cb5c33 8 // parallax.
mjr 82:4f6209cb5c33 9 //
mjr 82:4f6209cb5c33 10 // The basic installation for this type of sensor involves placing the
mjr 82:4f6209cb5c33 11 // sensor itself in a fixed location at one end of the plunger, pointing
mjr 82:4f6209cb5c33 12 // down the length of the plunger, and placing a reflective target at
mjr 82:4f6209cb5c33 13 // the end of the plunger. The target can simply be an ordinary plunger
mjr 82:4f6209cb5c33 14 // tip, if the sensor is at the far end of the plunger facing forward
mjr 82:4f6209cb5c33 15 // (facing the front of the cabinet). Alternatively, the target can
mjr 82:4f6209cb5c33 16 // be a disk or similar object attached to the end of the plunger, and
mjr 82:4f6209cb5c33 17 // the sensor can be placed at the front of the machine facing the target.
mjr 82:4f6209cb5c33 18 // In either case, the sensor measures the distance to the target at any
mjr 82:4f6209cb5c33 19 // given time, and we interpret that into the plunger position.
mjr 82:4f6209cb5c33 20 //
mjr 82:4f6209cb5c33 21 // Here are the specific sensor types we currently support:
mjr 82:4f6209cb5c33 22 //
mjr 113:7330439f2ffc 23 // VCNL4010: An IR proximity sensor. This sensor shines an IR light at a
mjr 113:7330439f2ffc 24 // target and measures the intensity of the reflected light. This doesn't
mjr 113:7330439f2ffc 25 // measure distance per se, but since the intensity of a light source
mjr 113:7330439f2ffc 26 // falls off as the square of the distance, we can use the reflected
mjr 113:7330439f2ffc 27 // intensity as a proxy for the distance by calculating 1/sqrt(intensity).
mjr 113:7330439f2ffc 28 // The main reason to support this type of sensor is that it's used in the
mjr 113:7330439f2ffc 29 // VirtuaPin v3 plunger kit, and several people have requested support so
mjr 113:7330439f2ffc 30 // that they can move re-flash that kit using the Pinscape software and
mjr 113:7330439f2ffc 31 // continue using their existing plunger sensor. Many people might also
mjr 113:7330439f2ffc 32 // consider this sensor for new DIY builds, since it produces pretty good
mjr 113:7330439f2ffc 33 // results. It's not as accurate as a potentiometer or quadrature sensor,
mjr 113:7330439f2ffc 34 // but it yields low-noise results with good enough precision for smooth
mjr 113:7330439f2ffc 35 // on-screen animation (maybe around 1mm precision). Its main drawback
mjr 113:7330439f2ffc 36 // is that it's relatively slow (250 Hz maximum sampling rate), but it's
mjr 113:7330439f2ffc 37 // still fast enough to be usable. It has several virtues that might more
mjr 113:7330439f2ffc 38 // than orffset its technical limitations for many paople: it's easy to
mjr 113:7330439f2ffc 39 // set up physically, it's completely non-contact, and it's cheap (under
mjr 113:7330439f2ffc 40 // $10 for the Adafruit breakout board).
mjr 113:7330439f2ffc 41 //
mjr 82:4f6209cb5c33 42 // VL6180X: This is an optical (IR) "time of flight" sensor that measures
mjr 82:4f6209cb5c33 43 // the distance to the target by sending optical pings and timing the
mjr 82:4f6209cb5c33 44 // return signal, converting the result to distance via the known speed
mjr 82:4f6209cb5c33 45 // of light. This sensor has nominal 1mm precision, although its true
mjr 82:4f6209cb5c33 46 // precision in testing is closer to 5mm. Sample times are around 16ms.
mjr 82:4f6209cb5c33 47 // This makes the sensor acceptable but not great by Pinscape standards;
mjr 82:4f6209cb5c33 48 // we generally consider 2.5ms read times and .25mm precision to be the
mjr 90:aa4e571da8e8 49 // minimum standards. However, this sensor is inexpensive and easier to
mjr 90:aa4e571da8e8 50 // set up than most of the better options, so it might be attractive to
mjr 82:4f6209cb5c33 51 // some cab builders despite the quality tradeoffs.
mjr 111:42dc75fbe623 52 //
mjr 111:42dc75fbe623 53 //
mjr 82:4f6209cb5c33 54
mjr 82:4f6209cb5c33 55 #ifndef _DISTANCESENSOR_H_
mjr 82:4f6209cb5c33 56 #define _DISTANCESENSOR_H_
mjr 82:4f6209cb5c33 57
mjr 82:4f6209cb5c33 58 #include "plunger.h"
mjr 82:4f6209cb5c33 59 #include "VL6180X.h"
mjr 111:42dc75fbe623 60 #include "VCNL4010.h"
mjr 111:42dc75fbe623 61
mjr 82:4f6209cb5c33 62
mjr 82:4f6209cb5c33 63 // Base class for distance sensors
mjr 82:4f6209cb5c33 64 class PlungerSensorDistance: public PlungerSensor
mjr 82:4f6209cb5c33 65 {
mjr 82:4f6209cb5c33 66 public:
mjr 86:e30a1f60f783 67 PlungerSensorDistance(int nativeScale) : PlungerSensor(nativeScale)
mjr 82:4f6209cb5c33 68 {
mjr 87:8d35c74403af 69 totalTime = 0;
mjr 87:8d35c74403af 70 nRuns = 0;
mjr 82:4f6209cb5c33 71 }
mjr 82:4f6209cb5c33 72
mjr 82:4f6209cb5c33 73 // get the average scan time
mjr 82:4f6209cb5c33 74 virtual uint32_t getAvgScanTime() { return uint32_t(totalTime / nRuns); }
mjr 82:4f6209cb5c33 75
mjr 82:4f6209cb5c33 76 protected:
mjr 82:4f6209cb5c33 77 // collect scan time statistics
mjr 82:4f6209cb5c33 78 void collectScanTimeStats(uint32_t dt)
mjr 82:4f6209cb5c33 79 {
mjr 82:4f6209cb5c33 80 totalTime += dt;
mjr 82:4f6209cb5c33 81 nRuns += 1;
mjr 82:4f6209cb5c33 82 }
mjr 82:4f6209cb5c33 83
mjr 82:4f6209cb5c33 84 // scan time statistics
mjr 82:4f6209cb5c33 85 uint64_t totalTime; // total time consumed by all reads so far
mjr 82:4f6209cb5c33 86 uint32_t nRuns; // number of runs so far
mjr 82:4f6209cb5c33 87 };
mjr 82:4f6209cb5c33 88
mjr 82:4f6209cb5c33 89 // PlungerSensor interface implementation for VL6180X sensors.
mjr 86:e30a1f60f783 90 //
mjr 86:e30a1f60f783 91 // The VL6180X reports distances in millimeter quanta, so the native
mjr 86:e30a1f60f783 92 // sensor units are millimeters. A physical plunger has about 3" of
mjr 86:e30a1f60f783 93 // total travel, but leave a little extra padding for measurement
mjr 86:e30a1f60f783 94 // inaccuracies and other unusual situations, so'll use an actual
mjr 87:8d35c74403af 95 // native scale of 150mm.
mjr 82:4f6209cb5c33 96 class PlungerSensorVL6180X: public PlungerSensorDistance
mjr 82:4f6209cb5c33 97 {
mjr 82:4f6209cb5c33 98 public:
mjr 82:4f6209cb5c33 99 PlungerSensorVL6180X(PinName sda, PinName scl, PinName gpio0)
mjr 87:8d35c74403af 100 : PlungerSensorDistance(150),
mjr 87:8d35c74403af 101 sensor(sda, scl, I2C_ADDRESS, gpio0, true)
mjr 82:4f6209cb5c33 102 {
mjr 82:4f6209cb5c33 103 }
mjr 82:4f6209cb5c33 104
mjr 87:8d35c74403af 105 // fixed I2C bus address for the VL6180X
mjr 87:8d35c74403af 106 static const int I2C_ADDRESS = 0x29;
mjr 82:4f6209cb5c33 107
mjr 82:4f6209cb5c33 108 virtual void init()
mjr 82:4f6209cb5c33 109 {
mjr 87:8d35c74403af 110 // initialize the sensor and set the default configuration
mjr 82:4f6209cb5c33 111 sensor.init();
mjr 82:4f6209cb5c33 112 sensor.setDefaults();
mjr 82:4f6209cb5c33 113
mjr 87:8d35c74403af 114 // start a reading
mjr 82:4f6209cb5c33 115 sensor.startRangeReading();
mjr 82:4f6209cb5c33 116 }
mjr 82:4f6209cb5c33 117
mjr 82:4f6209cb5c33 118 virtual bool ready()
mjr 82:4f6209cb5c33 119 {
mjr 87:8d35c74403af 120 // make sure a reading has been initiated
mjr 87:8d35c74403af 121 sensor.startRangeReading();
mjr 87:8d35c74403af 122
mjr 87:8d35c74403af 123 // check if a reading is ready
mjr 82:4f6209cb5c33 124 return sensor.rangeReady();
mjr 82:4f6209cb5c33 125 }
mjr 82:4f6209cb5c33 126
mjr 86:e30a1f60f783 127 virtual bool readRaw(PlungerReading &r)
mjr 82:4f6209cb5c33 128 {
mjr 87:8d35c74403af 129 // if we have a new reading ready, collect it
mjr 87:8d35c74403af 130 if (sensor.rangeReady())
mjr 87:8d35c74403af 131 {
mjr 87:8d35c74403af 132 // Get the range reading. Note that we already know that the
mjr 87:8d35c74403af 133 // sensor has a reading ready, so it shouldn't be possible to
mjr 87:8d35c74403af 134 // time out on the read. (The sensor could have timed out on
mjr 87:8d35c74403af 135 // convergence, but if it did, that's in the past already so
mjr 87:8d35c74403af 136 // it's not something we have to wait for now.)
mjr 87:8d35c74403af 137 uint8_t d;
mjr 87:8d35c74403af 138 uint32_t t, dt;
mjr 87:8d35c74403af 139 lastErr = sensor.getRange(d, t, dt, 100);
mjr 87:8d35c74403af 140
mjr 87:8d35c74403af 141 // if we got a reading, update the last reading
mjr 87:8d35c74403af 142 if (lastErr == 0)
mjr 87:8d35c74403af 143 {
mjr 87:8d35c74403af 144 // save the new reading
mjr 87:8d35c74403af 145 last.pos = d;
mjr 87:8d35c74403af 146 last.t = t;
mjr 87:8d35c74403af 147
mjr 87:8d35c74403af 148 // collect scan time statistics
mjr 87:8d35c74403af 149 collectScanTimeStats(dt);
mjr 87:8d35c74403af 150 }
mjr 87:8d35c74403af 151
mjr 87:8d35c74403af 152 // start a new reading
mjr 87:8d35c74403af 153 sensor.startRangeReading();
mjr 87:8d35c74403af 154 }
mjr 82:4f6209cb5c33 155
mjr 87:8d35c74403af 156 // return the most recent reading
mjr 87:8d35c74403af 157 r = last;
mjr 87:8d35c74403af 158 return lastErr == 0;
mjr 82:4f6209cb5c33 159 }
mjr 82:4f6209cb5c33 160
mjr 82:4f6209cb5c33 161 protected:
mjr 82:4f6209cb5c33 162 // underlying sensor interface
mjr 82:4f6209cb5c33 163 VL6180X sensor;
mjr 87:8d35c74403af 164
mjr 87:8d35c74403af 165 // last reading and error status
mjr 87:8d35c74403af 166 PlungerReading last;
mjr 87:8d35c74403af 167 int lastErr;
mjr 82:4f6209cb5c33 168 };
mjr 82:4f6209cb5c33 169
mjr 82:4f6209cb5c33 170
mjr 111:42dc75fbe623 171 // PlungerSensor interface implementation for VCNL4010 IR proximity sensors
mjr 111:42dc75fbe623 172 //
mjr 111:42dc75fbe623 173 // Our hardware interface for this sensor reports distances in abstract
mjr 111:42dc75fbe623 174 // units that fit a 16-bit int, so the native distance scale is 0..65535.
mjr 111:42dc75fbe623 175 // (The sensor itself doesn't have a native distance scale per se, since
mjr 111:42dc75fbe623 176 // it reports results in terms of the intensity of the reflected light.
mjr 111:42dc75fbe623 177 // This is related to the distance by an inverse square law, so since we
mjr 111:42dc75fbe623 178 // have to do some math on the raw readings anyway to convert them to
mjr 111:42dc75fbe623 179 // distances, we can choose whatever units we want for the conversion.
mjr 111:42dc75fbe623 180 // We choose units that are convenient for our purposes at the joystick
mjr 111:42dc75fbe623 181 // layer, given the 16-bit field we use to report the position back to
mjr 111:42dc75fbe623 182 // the PC.)
mjr 113:7330439f2ffc 183 //
mjr 113:7330439f2ffc 184 // The iredCurrent parameter sets the brightness of the sensor's IR LED,
mjr 113:7330439f2ffc 185 // which serves as the light source for the reflected light intensity
mjr 113:7330439f2ffc 186 // readings used for proximity measurements. This is given in units of
mjr 113:7330439f2ffc 187 // 10mA, so 1 means 10mA, 2 means 20mA, etc. Valid values are from 1
mjr 113:7330439f2ffc 188 // (10mA) to 20 (200mA).
mjr 113:7330439f2ffc 189 //
mjr 111:42dc75fbe623 190 class PlungerSensorVCNL4010: public PlungerSensorDistance
mjr 111:42dc75fbe623 191 {
mjr 111:42dc75fbe623 192 public:
mjr 113:7330439f2ffc 193 PlungerSensorVCNL4010(PinName sda, PinName scl, int iredCurrent)
mjr 111:42dc75fbe623 194 : PlungerSensorDistance(65535),
mjr 113:7330439f2ffc 195 sensor(sda, scl, true, iredCurrent)
mjr 111:42dc75fbe623 196 {
mjr 111:42dc75fbe623 197 }
mjr 111:42dc75fbe623 198
mjr 111:42dc75fbe623 199 virtual void init()
mjr 111:42dc75fbe623 200 {
mjr 111:42dc75fbe623 201 // initialize the sensor
mjr 111:42dc75fbe623 202 sensor.init();
mjr 111:42dc75fbe623 203
mjr 111:42dc75fbe623 204 // start a reading
mjr 111:42dc75fbe623 205 sensor.startProxReading();
mjr 111:42dc75fbe623 206 }
mjr 111:42dc75fbe623 207
mjr 111:42dc75fbe623 208 virtual bool ready()
mjr 111:42dc75fbe623 209 {
mjr 111:42dc75fbe623 210 // check if a reading is ready
mjr 111:42dc75fbe623 211 return sensor.proxReady();
mjr 111:42dc75fbe623 212 }
mjr 111:42dc75fbe623 213
mjr 111:42dc75fbe623 214 virtual bool readRaw(PlungerReading &r)
mjr 111:42dc75fbe623 215 {
mjr 111:42dc75fbe623 216 // if we have a new reading ready, collect it
mjr 111:42dc75fbe623 217 if (sensor.proxReady())
mjr 111:42dc75fbe623 218 {
mjr 113:7330439f2ffc 219 // Get the proximity count reading. Note that we already know
mjr 113:7330439f2ffc 220 // that the sensor has a reading ready, so it shouldn't be
mjr 113:7330439f2ffc 221 // possible to time out on the read.
mjr 113:7330439f2ffc 222 int rawCount;
mjr 111:42dc75fbe623 223 uint32_t t, dt;
mjr 113:7330439f2ffc 224 lastErr = sensor.getProx(rawCount, t, dt, 100);
mjr 111:42dc75fbe623 225
mjr 111:42dc75fbe623 226 // if we got a reading, update the last reading
mjr 111:42dc75fbe623 227 if (lastErr == 0)
mjr 111:42dc75fbe623 228 {
mjr 113:7330439f2ffc 229 // run the proximity count through the jitter filter
mjr 113:7330439f2ffc 230 int filteredCount = jitterFilter(rawCount);
mjr 113:7330439f2ffc 231
mjr 113:7330439f2ffc 232 // convert the count to a distance, using the filtered count
mjr 113:7330439f2ffc 233 int dist = sensor.countToDistance(filteredCount);
mjr 113:7330439f2ffc 234
mjr 111:42dc75fbe623 235 // save the new reading
mjr 113:7330439f2ffc 236 last.pos = dist;
mjr 111:42dc75fbe623 237 last.t = t;
mjr 113:7330439f2ffc 238 lastFilteredCount = filteredCount;
mjr 113:7330439f2ffc 239 lastRawCount = rawCount;
mjr 111:42dc75fbe623 240
mjr 111:42dc75fbe623 241 // collect scan time statistics
mjr 111:42dc75fbe623 242 collectScanTimeStats(dt);
mjr 111:42dc75fbe623 243 }
mjr 111:42dc75fbe623 244 }
mjr 111:42dc75fbe623 245
mjr 111:42dc75fbe623 246 // return the most recent reading
mjr 111:42dc75fbe623 247 r = last;
mjr 111:42dc75fbe623 248 return lastErr == 0;
mjr 111:42dc75fbe623 249 }
mjr 111:42dc75fbe623 250
mjr 113:7330439f2ffc 251 // The VCNL4010 applies jitter filtering to the physical sensor reading
mjr 113:7330439f2ffc 252 // instead of to the distance reading. This produces much better results
mjr 113:7330439f2ffc 253 // for this sensor because the sensor's distance resolution gets lower
mjr 113:7330439f2ffc 254 // at longer distances, so the conversion to distance tends to amplify
mjr 113:7330439f2ffc 255 // noise quite a bit at the distant end. It's therefore important to
mjr 113:7330439f2ffc 256 // do the noise reduction in the brightness domain, before that
mjr 113:7330439f2ffc 257 // amplification takes place.
mjr 113:7330439f2ffc 258 virtual int postJitterFilter(int pos) { return pos; }
mjr 113:7330439f2ffc 259
mjr 113:7330439f2ffc 260 // Send a status report for the config tool sensor viewer
mjr 113:7330439f2ffc 261 virtual void sendStatusReport(class USBJoystick &js, uint8_t flags)
mjr 113:7330439f2ffc 262 {
mjr 113:7330439f2ffc 263 // send the common status report
mjr 113:7330439f2ffc 264 PlungerSensor::sendStatusReport(js, flags);
mjr 113:7330439f2ffc 265
mjr 113:7330439f2ffc 266 // send the extra VCNL4010 sensor status report
mjr 113:7330439f2ffc 267 js.sendPlungerStatusVCNL4010(lastFilteredCount, lastRawCount);
mjr 113:7330439f2ffc 268 }
mjr 113:7330439f2ffc 269
mjr 113:7330439f2ffc 270 // Restore the saved calibration data from the configuration. The
mjr 113:7330439f2ffc 271 // main loop calls this at initialization time to pass us saved
mjr 113:7330439f2ffc 272 // private configuration data. The VCNL4010 uses this to store the
mjr 113:7330439f2ffc 273 // minimum proximity count reading observed during calibration, which
mjr 113:7330439f2ffc 274 // it uses to figure the scaling factor for the 1/sqrt(intensity)
mjr 113:7330439f2ffc 275 // distance calculation.
mjr 113:7330439f2ffc 276 virtual void restoreCalibration(Config &config)
mjr 113:7330439f2ffc 277 {
mjr 113:7330439f2ffc 278 // restore the saved minimum count reading
mjr 113:7330439f2ffc 279 sensor.restoreCalibration(config);
mjr 113:7330439f2ffc 280 }
mjr 113:7330439f2ffc 281
mjr 113:7330439f2ffc 282 // Begin calibration. The main loop calls this when the user
mjr 113:7330439f2ffc 283 // initiates a calibration cycle. The VCNL4010 code uses this to
mjr 113:7330439f2ffc 284 // reset its internal record of the proximity minimum.
mjr 113:7330439f2ffc 285 virtual void beginCalibration(Config &)
mjr 113:7330439f2ffc 286 {
mjr 113:7330439f2ffc 287 sensor.beginCalibration();
mjr 113:7330439f2ffc 288 }
mjr 113:7330439f2ffc 289
mjr 113:7330439f2ffc 290 // End calibration. The main loop calls this when a calibration
mjr 113:7330439f2ffc 291 // cycle finishes. The VCNL4010 code uses this to save the minimum
mjr 113:7330439f2ffc 292 // count value observed during the calibration interval, and to
mjr 113:7330439f2ffc 293 // calculate the new scaling factor for the 1/sqrt(intensity)
mjr 113:7330439f2ffc 294 // distance calculation.
mjr 113:7330439f2ffc 295 virtual void endCalibration(Config &config)
mjr 113:7330439f2ffc 296 {
mjr 113:7330439f2ffc 297 // let the sensor figure the new scaling factor
mjr 113:7330439f2ffc 298 sensor.endCalibration(config);
mjr 113:7330439f2ffc 299 }
mjr 113:7330439f2ffc 300
mjr 113:7330439f2ffc 301
mjr 111:42dc75fbe623 302 protected:
mjr 111:42dc75fbe623 303 // underlying sensor interface
mjr 111:42dc75fbe623 304 VCNL4010 sensor;
mjr 111:42dc75fbe623 305
mjr 111:42dc75fbe623 306 // last reading and error status
mjr 111:42dc75fbe623 307 PlungerReading last;
mjr 113:7330439f2ffc 308 int lastFilteredCount;
mjr 113:7330439f2ffc 309 int lastRawCount;
mjr 111:42dc75fbe623 310 int lastErr;
mjr 111:42dc75fbe623 311 };
mjr 111:42dc75fbe623 312
mjr 82:4f6209cb5c33 313 #endif