Pololu QTR Sensor Library, based on the QTR Arduino Library
QTRSensors.cpp@1:a664ab7aba8d, 2015-08-27 (annotated)
- Committer:
- phillippsm
- Date:
- Thu Aug 27 01:42:08 2015 +0000
- Revision:
- 1:a664ab7aba8d
- Parent:
- 0:d54bb6a949bf
Alpha 0.1 - Minor changes to timing and removing redundant code
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
phillippsm | 0:d54bb6a949bf | 1 | /* |
phillippsm | 0:d54bb6a949bf | 2 | QTRSensors.cpp - Arduino library for using Pololu QTR reflectance |
phillippsm | 0:d54bb6a949bf | 3 | sensors and reflectance sensor arrays: QTR-1A, QTR-8A, QTR-1RC, and |
phillippsm | 0:d54bb6a949bf | 4 | QTR-8RC. The object used will determine the type of the sensor (either |
phillippsm | 0:d54bb6a949bf | 5 | QTR-xA or QTR-xRC). Then simply specify in the constructor which |
phillippsm | 0:d54bb6a949bf | 6 | Arduino I/O pins are connected to a QTR sensor, and the read() method |
phillippsm | 0:d54bb6a949bf | 7 | will obtain reflectance measurements for those sensors. Smaller sensor |
phillippsm | 0:d54bb6a949bf | 8 | values correspond to higher reflectance (e.g. white) while larger |
phillippsm | 0:d54bb6a949bf | 9 | sensor values correspond to lower reflectance (e.g. black or a void). |
phillippsm | 0:d54bb6a949bf | 10 | |
phillippsm | 0:d54bb6a949bf | 11 | * QTRSensorsRC should be used for QTR-1RC and QTR-8RC sensors. |
phillippsm | 0:d54bb6a949bf | 12 | * QTRSensorsAnalog should be used for QTR-1A and QTR-8A sensors. |
phillippsm | 0:d54bb6a949bf | 13 | */ |
phillippsm | 0:d54bb6a949bf | 14 | |
phillippsm | 0:d54bb6a949bf | 15 | /* |
phillippsm | 0:d54bb6a949bf | 16 | * Written by Ben Schmidel et al., October 4, 2010. |
phillippsm | 0:d54bb6a949bf | 17 | * Copyright (c) 2008-2012 Pololu Corporation. For more information, see |
phillippsm | 0:d54bb6a949bf | 18 | * |
phillippsm | 0:d54bb6a949bf | 19 | * http://www.pololu.com |
phillippsm | 0:d54bb6a949bf | 20 | * http://forum.pololu.com |
phillippsm | 0:d54bb6a949bf | 21 | * http://www.pololu.com/docs/0J19 |
phillippsm | 0:d54bb6a949bf | 22 | * |
phillippsm | 0:d54bb6a949bf | 23 | * You may freely modify and share this code, as long as you keep this |
phillippsm | 0:d54bb6a949bf | 24 | * notice intact (including the two links above). Licensed under the |
phillippsm | 0:d54bb6a949bf | 25 | * Creative Commons BY-SA 3.0 license: |
phillippsm | 0:d54bb6a949bf | 26 | * |
phillippsm | 0:d54bb6a949bf | 27 | * http://creativecommons.org/licenses/by-sa/3.0/ |
phillippsm | 0:d54bb6a949bf | 28 | * |
phillippsm | 0:d54bb6a949bf | 29 | * Disclaimer: To the extent permitted by law, Pololu provides this work |
phillippsm | 0:d54bb6a949bf | 30 | * without any warranty. It might be defective, in which case you agree |
phillippsm | 0:d54bb6a949bf | 31 | * to be responsible for all resulting costs and damages. |
phillippsm | 0:d54bb6a949bf | 32 | * |
phillippsm | 0:d54bb6a949bf | 33 | * Modified by Matthew Phillipps, August 24, 2015 |
phillippsm | 0:d54bb6a949bf | 34 | * Adapted to mbed platform (especially STM Nucleo boards) |
phillippsm | 0:d54bb6a949bf | 35 | * Some changes to memory management |
phillippsm | 0:d54bb6a949bf | 36 | */ |
phillippsm | 0:d54bb6a949bf | 37 | |
phillippsm | 0:d54bb6a949bf | 38 | #include <stdlib.h> |
phillippsm | 0:d54bb6a949bf | 39 | #include "QTRSensors.h" |
phillippsm | 0:d54bb6a949bf | 40 | #include "mbed.h" |
phillippsm | 0:d54bb6a949bf | 41 | |
phillippsm | 0:d54bb6a949bf | 42 | |
phillippsm | 0:d54bb6a949bf | 43 | Timer timer; |
phillippsm | 0:d54bb6a949bf | 44 | |
phillippsm | 0:d54bb6a949bf | 45 | |
phillippsm | 0:d54bb6a949bf | 46 | // Base class data member initialization (called by derived class init()) |
phillippsm | 0:d54bb6a949bf | 47 | void QTRSensors::init(PinName *pins, unsigned char numSensors, |
phillippsm | 1:a664ab7aba8d | 48 | PinName emitterPin, bool analog = false) |
phillippsm | 0:d54bb6a949bf | 49 | { |
phillippsm | 0:d54bb6a949bf | 50 | calibratedMinimumOn=0; |
phillippsm | 0:d54bb6a949bf | 51 | calibratedMaximumOn=0; |
phillippsm | 0:d54bb6a949bf | 52 | calibratedMinimumOff=0; |
phillippsm | 0:d54bb6a949bf | 53 | calibratedMaximumOff=0; |
phillippsm | 0:d54bb6a949bf | 54 | |
phillippsm | 0:d54bb6a949bf | 55 | if (numSensors > QTR_MAX_SENSORS) |
phillippsm | 0:d54bb6a949bf | 56 | _numSensors = QTR_MAX_SENSORS; |
phillippsm | 0:d54bb6a949bf | 57 | else |
phillippsm | 0:d54bb6a949bf | 58 | _numSensors = numSensors; |
phillippsm | 1:a664ab7aba8d | 59 | |
phillippsm | 0:d54bb6a949bf | 60 | |
phillippsm | 1:a664ab7aba8d | 61 | if (_pins == 0) { |
phillippsm | 0:d54bb6a949bf | 62 | _pins = (PinName *)malloc(sizeof(PinName)*_numSensors); |
phillippsm | 0:d54bb6a949bf | 63 | if (_pins == 0) |
phillippsm | 0:d54bb6a949bf | 64 | return; |
phillippsm | 0:d54bb6a949bf | 65 | } |
phillippsm | 1:a664ab7aba8d | 66 | |
phillippsm | 0:d54bb6a949bf | 67 | unsigned char i; |
phillippsm | 1:a664ab7aba8d | 68 | // Copy parameter values to local storage |
phillippsm | 1:a664ab7aba8d | 69 | for (i = 0; i < _numSensors; i++) { |
phillippsm | 0:d54bb6a949bf | 70 | _pins[i] = pins[i]; |
phillippsm | 0:d54bb6a949bf | 71 | } |
phillippsm | 0:d54bb6a949bf | 72 | |
phillippsm | 1:a664ab7aba8d | 73 | // Allocate the arrays |
phillippsm | 1:a664ab7aba8d | 74 | // Allocate Space for Calibrated Maximum On Values |
phillippsm | 1:a664ab7aba8d | 75 | calibratedMaximumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors); |
phillippsm | 1:a664ab7aba8d | 76 | |
phillippsm | 1:a664ab7aba8d | 77 | // If the malloc failed, don't continue. |
phillippsm | 1:a664ab7aba8d | 78 | if(calibratedMaximumOn == 0) |
phillippsm | 1:a664ab7aba8d | 79 | return; |
phillippsm | 1:a664ab7aba8d | 80 | |
phillippsm | 1:a664ab7aba8d | 81 | // Initialize the max and min calibrated values to values that |
phillippsm | 1:a664ab7aba8d | 82 | // will cause the first reading to update them. |
phillippsm | 1:a664ab7aba8d | 83 | |
phillippsm | 1:a664ab7aba8d | 84 | for(i=0; i<_numSensors; i++) |
phillippsm | 1:a664ab7aba8d | 85 | calibratedMaximumOn[i] = 0; |
phillippsm | 1:a664ab7aba8d | 86 | |
phillippsm | 1:a664ab7aba8d | 87 | // Allocate Space for Calibrated Maximum Off Values |
phillippsm | 1:a664ab7aba8d | 88 | calibratedMaximumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors); |
phillippsm | 1:a664ab7aba8d | 89 | |
phillippsm | 1:a664ab7aba8d | 90 | // If the malloc failed, don't continue. |
phillippsm | 1:a664ab7aba8d | 91 | if(calibratedMaximumOff == 0) |
phillippsm | 1:a664ab7aba8d | 92 | return; |
phillippsm | 1:a664ab7aba8d | 93 | |
phillippsm | 1:a664ab7aba8d | 94 | // Initialize the max and min calibrated values to values that |
phillippsm | 1:a664ab7aba8d | 95 | // will cause the first reading to update them. |
phillippsm | 1:a664ab7aba8d | 96 | |
phillippsm | 1:a664ab7aba8d | 97 | for(i=0; i<_numSensors; i++) |
phillippsm | 1:a664ab7aba8d | 98 | calibratedMaximumOff[i] = 0; |
phillippsm | 1:a664ab7aba8d | 99 | |
phillippsm | 1:a664ab7aba8d | 100 | // Allocate Space for Calibrated Minimum On Values |
phillippsm | 1:a664ab7aba8d | 101 | calibratedMinimumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors); |
phillippsm | 1:a664ab7aba8d | 102 | |
phillippsm | 1:a664ab7aba8d | 103 | // If the malloc failed, don't continue. |
phillippsm | 1:a664ab7aba8d | 104 | if(calibratedMinimumOn == 0) |
phillippsm | 1:a664ab7aba8d | 105 | return; |
phillippsm | 1:a664ab7aba8d | 106 | |
phillippsm | 1:a664ab7aba8d | 107 | for(i=0; i<_numSensors; i++) |
phillippsm | 1:a664ab7aba8d | 108 | calibratedMinimumOn[i] = _maxValue; |
phillippsm | 1:a664ab7aba8d | 109 | |
phillippsm | 1:a664ab7aba8d | 110 | // Allocate Space for Calibrated Minimum Off Values |
phillippsm | 1:a664ab7aba8d | 111 | calibratedMinimumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors); |
phillippsm | 1:a664ab7aba8d | 112 | |
phillippsm | 1:a664ab7aba8d | 113 | // If the malloc failed, don't continue. |
phillippsm | 1:a664ab7aba8d | 114 | if(calibratedMinimumOff == 0) |
phillippsm | 1:a664ab7aba8d | 115 | return; |
phillippsm | 1:a664ab7aba8d | 116 | |
phillippsm | 1:a664ab7aba8d | 117 | for(i=0; i<_numSensors; i++) |
phillippsm | 1:a664ab7aba8d | 118 | calibratedMinimumOff[i] = _maxValue; |
phillippsm | 1:a664ab7aba8d | 119 | |
phillippsm | 1:a664ab7aba8d | 120 | // emitter pin is used for DigitalOut |
phillippsm | 1:a664ab7aba8d | 121 | // So we create a DigitalOut on that pin |
phillippsm | 0:d54bb6a949bf | 122 | _emitterPin = emitterPin; |
phillippsm | 0:d54bb6a949bf | 123 | _emitter = new DigitalOut(emitterPin); |
phillippsm | 0:d54bb6a949bf | 124 | |
phillippsm | 1:a664ab7aba8d | 125 | // If we have an Analog Sensor then we wil used AnalogIn on the pins provided |
phillippsm | 1:a664ab7aba8d | 126 | // We use a Vector for our collection of pins |
phillippsm | 1:a664ab7aba8d | 127 | // Here we reserve space for the pins |
phillippsm | 0:d54bb6a949bf | 128 | _analog = analog; |
phillippsm | 0:d54bb6a949bf | 129 | if (_analog) { |
phillippsm | 0:d54bb6a949bf | 130 | _qtrAIPins.reserve(_numSensors); |
phillippsm | 0:d54bb6a949bf | 131 | } else { |
phillippsm | 1:a664ab7aba8d | 132 | // Not analog - so we use _qtrPins (which is a Vector on DigitalInOut) |
phillippsm | 1:a664ab7aba8d | 133 | _qtrPins.reserve(_numSensors); |
phillippsm | 0:d54bb6a949bf | 134 | } |
phillippsm | 1:a664ab7aba8d | 135 | // Create the pins and push onto the vectors |
phillippsm | 1:a664ab7aba8d | 136 | for (i = 0; i < _numSensors; i++) { |
phillippsm | 0:d54bb6a949bf | 137 | if (_analog) { |
phillippsm | 1:a664ab7aba8d | 138 | _qtrAIPins.push_back(new AnalogIn(pins[i])); |
phillippsm | 0:d54bb6a949bf | 139 | } else { |
phillippsm | 1:a664ab7aba8d | 140 | _qtrPins.push_back(new DigitalInOut(pins[i])); |
phillippsm | 0:d54bb6a949bf | 141 | } |
phillippsm | 0:d54bb6a949bf | 142 | } |
phillippsm | 0:d54bb6a949bf | 143 | |
phillippsm | 0:d54bb6a949bf | 144 | } |
phillippsm | 0:d54bb6a949bf | 145 | |
phillippsm | 0:d54bb6a949bf | 146 | |
phillippsm | 0:d54bb6a949bf | 147 | // Reads the sensor values into an array. There *MUST* be space |
phillippsm | 0:d54bb6a949bf | 148 | // for as many values as there were sensors specified in the constructor. |
phillippsm | 0:d54bb6a949bf | 149 | // Example usage: |
phillippsm | 0:d54bb6a949bf | 150 | // unsigned int sensor_values[8]; |
phillippsm | 0:d54bb6a949bf | 151 | // sensors.read(sensor_values); |
phillippsm | 0:d54bb6a949bf | 152 | // The values returned are a measure of the reflectance in abstract units, |
phillippsm | 0:d54bb6a949bf | 153 | // with higher values corresponding to lower reflectance (e.g. a black |
phillippsm | 0:d54bb6a949bf | 154 | // surface or a void). |
phillippsm | 0:d54bb6a949bf | 155 | void QTRSensors::read(unsigned int *sensor_values, unsigned char readMode) |
phillippsm | 0:d54bb6a949bf | 156 | { |
phillippsm | 0:d54bb6a949bf | 157 | unsigned int off_values[QTR_MAX_SENSORS]; |
phillippsm | 0:d54bb6a949bf | 158 | unsigned char i; |
phillippsm | 1:a664ab7aba8d | 159 | |
phillippsm | 0:d54bb6a949bf | 160 | |
phillippsm | 1:a664ab7aba8d | 161 | if(readMode == QTR_EMITTERS_ON || readMode == QTR_EMITTERS_ON_AND_OFF) { |
phillippsm | 1:a664ab7aba8d | 162 | emittersOn(); |
phillippsm | 1:a664ab7aba8d | 163 | } else { |
phillippsm | 0:d54bb6a949bf | 164 | emittersOff(); |
phillippsm | 0:d54bb6a949bf | 165 | } |
phillippsm | 0:d54bb6a949bf | 166 | |
phillippsm | 0:d54bb6a949bf | 167 | |
phillippsm | 0:d54bb6a949bf | 168 | readPrivate(sensor_values); |
phillippsm | 1:a664ab7aba8d | 169 | |
phillippsm | 0:d54bb6a949bf | 170 | emittersOff(); |
phillippsm | 0:d54bb6a949bf | 171 | |
phillippsm | 1:a664ab7aba8d | 172 | if(readMode == QTR_EMITTERS_ON_AND_OFF) { |
phillippsm | 0:d54bb6a949bf | 173 | readPrivate(off_values); |
phillippsm | 0:d54bb6a949bf | 174 | |
phillippsm | 1:a664ab7aba8d | 175 | for(i=0; i<_numSensors; i++) { |
phillippsm | 0:d54bb6a949bf | 176 | sensor_values[i] += _maxValue - off_values[i]; |
phillippsm | 0:d54bb6a949bf | 177 | } |
phillippsm | 0:d54bb6a949bf | 178 | } |
phillippsm | 0:d54bb6a949bf | 179 | } |
phillippsm | 0:d54bb6a949bf | 180 | |
phillippsm | 0:d54bb6a949bf | 181 | |
phillippsm | 0:d54bb6a949bf | 182 | // Turn the IR LEDs off and on. This is mainly for use by the |
phillippsm | 0:d54bb6a949bf | 183 | // read method, and calling these functions before or |
phillippsm | 0:d54bb6a949bf | 184 | // after the reading the sensors will have no effect on the |
phillippsm | 0:d54bb6a949bf | 185 | // readings, but you may wish to use these for testing purposes. |
phillippsm | 0:d54bb6a949bf | 186 | void QTRSensors::emittersOff() |
phillippsm | 0:d54bb6a949bf | 187 | { |
phillippsm | 0:d54bb6a949bf | 188 | if (_emitterPin == QTR_NO_EMITTER_PIN) |
phillippsm | 0:d54bb6a949bf | 189 | return; |
phillippsm | 0:d54bb6a949bf | 190 | |
phillippsm | 1:a664ab7aba8d | 191 | _emitter->write(LOW); |
phillippsm | 1:a664ab7aba8d | 192 | wait_us(200); // wait 200 microseconds for the emitters to settle |
phillippsm | 0:d54bb6a949bf | 193 | } |
phillippsm | 0:d54bb6a949bf | 194 | |
phillippsm | 0:d54bb6a949bf | 195 | void QTRSensors::emittersOn() |
phillippsm | 0:d54bb6a949bf | 196 | { |
phillippsm | 0:d54bb6a949bf | 197 | if (_emitterPin == QTR_NO_EMITTER_PIN) |
phillippsm | 0:d54bb6a949bf | 198 | return; |
phillippsm | 1:a664ab7aba8d | 199 | |
phillippsm | 0:d54bb6a949bf | 200 | _emitter->write(HIGH); |
phillippsm | 1:a664ab7aba8d | 201 | wait_us(200); // wait 200 microseconds for the emitters to settle |
phillippsm | 0:d54bb6a949bf | 202 | } |
phillippsm | 0:d54bb6a949bf | 203 | |
phillippsm | 0:d54bb6a949bf | 204 | // Resets the calibration. |
phillippsm | 0:d54bb6a949bf | 205 | void QTRSensors::resetCalibration() |
phillippsm | 0:d54bb6a949bf | 206 | { |
phillippsm | 0:d54bb6a949bf | 207 | unsigned char i; |
phillippsm | 1:a664ab7aba8d | 208 | for(i=0; i<_numSensors; i++) { |
phillippsm | 0:d54bb6a949bf | 209 | if(calibratedMinimumOn) |
phillippsm | 0:d54bb6a949bf | 210 | calibratedMinimumOn[i] = _maxValue; |
phillippsm | 0:d54bb6a949bf | 211 | if(calibratedMinimumOff) |
phillippsm | 0:d54bb6a949bf | 212 | calibratedMinimumOff[i] = _maxValue; |
phillippsm | 0:d54bb6a949bf | 213 | if(calibratedMaximumOn) |
phillippsm | 0:d54bb6a949bf | 214 | calibratedMaximumOn[i] = 0; |
phillippsm | 0:d54bb6a949bf | 215 | if(calibratedMaximumOff) |
phillippsm | 0:d54bb6a949bf | 216 | calibratedMaximumOff[i] = 0; |
phillippsm | 0:d54bb6a949bf | 217 | } |
phillippsm | 0:d54bb6a949bf | 218 | } |
phillippsm | 0:d54bb6a949bf | 219 | |
phillippsm | 0:d54bb6a949bf | 220 | // Reads the sensors 10 times and uses the results for |
phillippsm | 0:d54bb6a949bf | 221 | // calibration. The sensor values are not returned; instead, the |
phillippsm | 0:d54bb6a949bf | 222 | // maximum and minimum values found over time are stored internally |
phillippsm | 0:d54bb6a949bf | 223 | // and used for the readCalibrated() method. |
phillippsm | 0:d54bb6a949bf | 224 | void QTRSensors::calibrate(unsigned char readMode) |
phillippsm | 0:d54bb6a949bf | 225 | { |
phillippsm | 1:a664ab7aba8d | 226 | if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON) { |
phillippsm | 0:d54bb6a949bf | 227 | calibrateOnOrOff(&calibratedMinimumOn, |
phillippsm | 0:d54bb6a949bf | 228 | &calibratedMaximumOn, |
phillippsm | 0:d54bb6a949bf | 229 | QTR_EMITTERS_ON); |
phillippsm | 0:d54bb6a949bf | 230 | } |
phillippsm | 0:d54bb6a949bf | 231 | |
phillippsm | 0:d54bb6a949bf | 232 | |
phillippsm | 1:a664ab7aba8d | 233 | if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF) { |
phillippsm | 0:d54bb6a949bf | 234 | calibrateOnOrOff(&calibratedMinimumOff, |
phillippsm | 0:d54bb6a949bf | 235 | &calibratedMaximumOff, |
phillippsm | 0:d54bb6a949bf | 236 | QTR_EMITTERS_OFF); |
phillippsm | 0:d54bb6a949bf | 237 | } |
phillippsm | 0:d54bb6a949bf | 238 | } |
phillippsm | 0:d54bb6a949bf | 239 | |
phillippsm | 0:d54bb6a949bf | 240 | void QTRSensors::calibrateOnOrOff(unsigned int **calibratedMinimum, |
phillippsm | 0:d54bb6a949bf | 241 | unsigned int **calibratedMaximum, |
phillippsm | 0:d54bb6a949bf | 242 | unsigned char readMode) |
phillippsm | 0:d54bb6a949bf | 243 | { |
phillippsm | 0:d54bb6a949bf | 244 | int i; |
phillippsm | 0:d54bb6a949bf | 245 | unsigned int sensor_values[16]; |
phillippsm | 0:d54bb6a949bf | 246 | unsigned int max_sensor_values[16]; |
phillippsm | 0:d54bb6a949bf | 247 | unsigned int min_sensor_values[16]; |
phillippsm | 1:a664ab7aba8d | 248 | |
phillippsm | 1:a664ab7aba8d | 249 | // initialisation of calibrated sensor values moved to init() |
phillippsm | 0:d54bb6a949bf | 250 | |
phillippsm | 0:d54bb6a949bf | 251 | int j; |
phillippsm | 1:a664ab7aba8d | 252 | for(j=0; j<10; j++) { |
phillippsm | 0:d54bb6a949bf | 253 | read(sensor_values,readMode); |
phillippsm | 1:a664ab7aba8d | 254 | for(i=0; i<_numSensors; i++) { |
phillippsm | 0:d54bb6a949bf | 255 | // set the max we found THIS time |
phillippsm | 0:d54bb6a949bf | 256 | if(j == 0 || max_sensor_values[i] < sensor_values[i]) |
phillippsm | 0:d54bb6a949bf | 257 | max_sensor_values[i] = sensor_values[i]; |
phillippsm | 0:d54bb6a949bf | 258 | |
phillippsm | 0:d54bb6a949bf | 259 | // set the min we found THIS time |
phillippsm | 0:d54bb6a949bf | 260 | if(j == 0 || min_sensor_values[i] > sensor_values[i]) |
phillippsm | 0:d54bb6a949bf | 261 | min_sensor_values[i] = sensor_values[i]; |
phillippsm | 0:d54bb6a949bf | 262 | } |
phillippsm | 0:d54bb6a949bf | 263 | } |
phillippsm | 0:d54bb6a949bf | 264 | |
phillippsm | 0:d54bb6a949bf | 265 | // record the min and max calibration values |
phillippsm | 1:a664ab7aba8d | 266 | for(i=0; i<_numSensors; i++) { |
phillippsm | 0:d54bb6a949bf | 267 | if(min_sensor_values[i] > (*calibratedMaximum)[i]) // this was min_sensor_values[i] > (*calibratedMaximum)[i] |
phillippsm | 0:d54bb6a949bf | 268 | (*calibratedMaximum)[i] = min_sensor_values[i]; |
phillippsm | 0:d54bb6a949bf | 269 | if(max_sensor_values[i] < (*calibratedMinimum)[i]) |
phillippsm | 0:d54bb6a949bf | 270 | (*calibratedMinimum)[i] = max_sensor_values[i]; |
phillippsm | 0:d54bb6a949bf | 271 | } |
phillippsm | 1:a664ab7aba8d | 272 | |
phillippsm | 0:d54bb6a949bf | 273 | } |
phillippsm | 0:d54bb6a949bf | 274 | |
phillippsm | 0:d54bb6a949bf | 275 | |
phillippsm | 0:d54bb6a949bf | 276 | // Returns values calibrated to a value between 0 and 1000, where |
phillippsm | 0:d54bb6a949bf | 277 | // 0 corresponds to the minimum value read by calibrate() and 1000 |
phillippsm | 0:d54bb6a949bf | 278 | // corresponds to the maximum value. Calibration values are |
phillippsm | 0:d54bb6a949bf | 279 | // stored separately for each sensor, so that differences in the |
phillippsm | 0:d54bb6a949bf | 280 | // sensors are accounted for automatically. |
phillippsm | 0:d54bb6a949bf | 281 | void QTRSensors::readCalibrated(unsigned int *sensor_values, unsigned char readMode) |
phillippsm | 0:d54bb6a949bf | 282 | { |
phillippsm | 0:d54bb6a949bf | 283 | int i; |
phillippsm | 0:d54bb6a949bf | 284 | |
phillippsm | 0:d54bb6a949bf | 285 | // if not calibrated, do nothing |
phillippsm | 0:d54bb6a949bf | 286 | if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF) |
phillippsm | 0:d54bb6a949bf | 287 | if(!calibratedMinimumOff || !calibratedMaximumOff) |
phillippsm | 0:d54bb6a949bf | 288 | return; |
phillippsm | 0:d54bb6a949bf | 289 | if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON) |
phillippsm | 0:d54bb6a949bf | 290 | if(!calibratedMinimumOn || !calibratedMaximumOn) |
phillippsm | 0:d54bb6a949bf | 291 | return; |
phillippsm | 0:d54bb6a949bf | 292 | |
phillippsm | 0:d54bb6a949bf | 293 | // read the needed values |
phillippsm | 0:d54bb6a949bf | 294 | read(sensor_values,readMode); |
phillippsm | 0:d54bb6a949bf | 295 | |
phillippsm | 1:a664ab7aba8d | 296 | for(i=0; i<_numSensors; i++) { |
phillippsm | 0:d54bb6a949bf | 297 | unsigned int calmin,calmax; |
phillippsm | 0:d54bb6a949bf | 298 | unsigned int denominator; |
phillippsm | 0:d54bb6a949bf | 299 | |
phillippsm | 0:d54bb6a949bf | 300 | // find the correct calibration |
phillippsm | 1:a664ab7aba8d | 301 | if(readMode == QTR_EMITTERS_ON) { |
phillippsm | 0:d54bb6a949bf | 302 | calmax = calibratedMaximumOn[i]; |
phillippsm | 0:d54bb6a949bf | 303 | calmin = calibratedMinimumOn[i]; |
phillippsm | 1:a664ab7aba8d | 304 | } else if(readMode == QTR_EMITTERS_OFF) { |
phillippsm | 0:d54bb6a949bf | 305 | calmax = calibratedMaximumOff[i]; |
phillippsm | 0:d54bb6a949bf | 306 | calmin = calibratedMinimumOff[i]; |
phillippsm | 1:a664ab7aba8d | 307 | } else { // QTR_EMITTERS_ON_AND_OFF |
phillippsm | 0:d54bb6a949bf | 308 | |
phillippsm | 0:d54bb6a949bf | 309 | if(calibratedMinimumOff[i] < calibratedMinimumOn[i]) // no meaningful signal |
phillippsm | 0:d54bb6a949bf | 310 | calmin = _maxValue; |
phillippsm | 0:d54bb6a949bf | 311 | else |
phillippsm | 0:d54bb6a949bf | 312 | calmin = calibratedMinimumOn[i] + _maxValue - calibratedMinimumOff[i]; // this won't go past _maxValue |
phillippsm | 0:d54bb6a949bf | 313 | |
phillippsm | 0:d54bb6a949bf | 314 | if(calibratedMaximumOff[i] < calibratedMaximumOn[i]) // no meaningful signal |
phillippsm | 0:d54bb6a949bf | 315 | calmax = _maxValue; |
phillippsm | 0:d54bb6a949bf | 316 | else |
phillippsm | 0:d54bb6a949bf | 317 | calmax = calibratedMaximumOn[i] + _maxValue - calibratedMaximumOff[i]; // this won't go past _maxValue |
phillippsm | 0:d54bb6a949bf | 318 | } |
phillippsm | 0:d54bb6a949bf | 319 | |
phillippsm | 0:d54bb6a949bf | 320 | denominator = calmax - calmin; |
phillippsm | 0:d54bb6a949bf | 321 | |
phillippsm | 0:d54bb6a949bf | 322 | signed int x = 0; |
phillippsm | 0:d54bb6a949bf | 323 | if(denominator != 0) |
phillippsm | 0:d54bb6a949bf | 324 | x = (((signed long)sensor_values[i]) - calmin) |
phillippsm | 0:d54bb6a949bf | 325 | * 1000 / denominator; |
phillippsm | 0:d54bb6a949bf | 326 | if(x < 0) |
phillippsm | 0:d54bb6a949bf | 327 | x = 0; |
phillippsm | 0:d54bb6a949bf | 328 | else if(x > 1000) |
phillippsm | 0:d54bb6a949bf | 329 | x = 1000; |
phillippsm | 0:d54bb6a949bf | 330 | sensor_values[i] = x; |
phillippsm | 0:d54bb6a949bf | 331 | } |
phillippsm | 0:d54bb6a949bf | 332 | |
phillippsm | 0:d54bb6a949bf | 333 | } |
phillippsm | 0:d54bb6a949bf | 334 | |
phillippsm | 0:d54bb6a949bf | 335 | |
phillippsm | 0:d54bb6a949bf | 336 | // Operates the same as read calibrated, but also returns an |
phillippsm | 0:d54bb6a949bf | 337 | // estimated position of the robot with respect to a line. The |
phillippsm | 0:d54bb6a949bf | 338 | // estimate is made using a weighted average of the sensor indices |
phillippsm | 0:d54bb6a949bf | 339 | // multiplied by 1000, so that a return value of 0 indicates that |
phillippsm | 0:d54bb6a949bf | 340 | // the line is directly below sensor 0, a return value of 1000 |
phillippsm | 0:d54bb6a949bf | 341 | // indicates that the line is directly below sensor 1, 2000 |
phillippsm | 0:d54bb6a949bf | 342 | // indicates that it's below sensor 2000, etc. Intermediate |
phillippsm | 0:d54bb6a949bf | 343 | // values indicate that the line is between two sensors. The |
phillippsm | 0:d54bb6a949bf | 344 | // formula is: |
phillippsm | 0:d54bb6a949bf | 345 | // |
phillippsm | 0:d54bb6a949bf | 346 | // 0*value0 + 1000*value1 + 2000*value2 + ... |
phillippsm | 0:d54bb6a949bf | 347 | // -------------------------------------------- |
phillippsm | 0:d54bb6a949bf | 348 | // value0 + value1 + value2 + ... |
phillippsm | 0:d54bb6a949bf | 349 | // |
phillippsm | 0:d54bb6a949bf | 350 | // By default, this function assumes a dark line (high values) |
phillippsm | 0:d54bb6a949bf | 351 | // surrounded by white (low values). If your line is light on |
phillippsm | 0:d54bb6a949bf | 352 | // black, set the optional second argument white_line to true. In |
phillippsm | 0:d54bb6a949bf | 353 | // this case, each sensor value will be replaced by (1000-value) |
phillippsm | 0:d54bb6a949bf | 354 | // before the averaging. |
phillippsm | 0:d54bb6a949bf | 355 | int QTRSensors::readLine(unsigned int *sensor_values, |
phillippsm | 1:a664ab7aba8d | 356 | unsigned char readMode, unsigned char white_line) |
phillippsm | 0:d54bb6a949bf | 357 | { |
phillippsm | 0:d54bb6a949bf | 358 | unsigned char i, on_line = 0; |
phillippsm | 0:d54bb6a949bf | 359 | unsigned long avg; // this is for the weighted total, which is long |
phillippsm | 1:a664ab7aba8d | 360 | // before division |
phillippsm | 0:d54bb6a949bf | 361 | unsigned int sum; // this is for the denominator which is <= 64000 |
phillippsm | 0:d54bb6a949bf | 362 | static int last_value=0; // assume initially that the line is left. |
phillippsm | 0:d54bb6a949bf | 363 | |
phillippsm | 0:d54bb6a949bf | 364 | readCalibrated(sensor_values, readMode); |
phillippsm | 0:d54bb6a949bf | 365 | |
phillippsm | 0:d54bb6a949bf | 366 | avg = 0; |
phillippsm | 0:d54bb6a949bf | 367 | sum = 0; |
phillippsm | 0:d54bb6a949bf | 368 | |
phillippsm | 1:a664ab7aba8d | 369 | for(i=0; i<_numSensors; i++) { |
phillippsm | 0:d54bb6a949bf | 370 | int value = sensor_values[i]; |
phillippsm | 0:d54bb6a949bf | 371 | if(white_line) |
phillippsm | 0:d54bb6a949bf | 372 | value = 1000-value; |
phillippsm | 0:d54bb6a949bf | 373 | |
phillippsm | 0:d54bb6a949bf | 374 | // keep track of whether we see the line at all |
phillippsm | 0:d54bb6a949bf | 375 | if(value > 200) { |
phillippsm | 0:d54bb6a949bf | 376 | on_line = 1; |
phillippsm | 0:d54bb6a949bf | 377 | } |
phillippsm | 0:d54bb6a949bf | 378 | |
phillippsm | 0:d54bb6a949bf | 379 | // only average in values that are above a noise threshold |
phillippsm | 0:d54bb6a949bf | 380 | if(value > 50) { |
phillippsm | 0:d54bb6a949bf | 381 | avg += (long)(value) * (i * 1000); |
phillippsm | 0:d54bb6a949bf | 382 | sum += value; |
phillippsm | 0:d54bb6a949bf | 383 | } |
phillippsm | 0:d54bb6a949bf | 384 | } |
phillippsm | 0:d54bb6a949bf | 385 | |
phillippsm | 1:a664ab7aba8d | 386 | if(!on_line) { |
phillippsm | 0:d54bb6a949bf | 387 | // If it last read to the left of center, return 0. |
phillippsm | 0:d54bb6a949bf | 388 | if(last_value < (_numSensors-1)*1000/2) |
phillippsm | 0:d54bb6a949bf | 389 | return 0; |
phillippsm | 0:d54bb6a949bf | 390 | |
phillippsm | 0:d54bb6a949bf | 391 | // If it last read to the right of center, return the max. |
phillippsm | 0:d54bb6a949bf | 392 | else |
phillippsm | 0:d54bb6a949bf | 393 | return (_numSensors-1)*1000; |
phillippsm | 0:d54bb6a949bf | 394 | |
phillippsm | 0:d54bb6a949bf | 395 | } |
phillippsm | 0:d54bb6a949bf | 396 | |
phillippsm | 0:d54bb6a949bf | 397 | last_value = avg/sum; |
phillippsm | 0:d54bb6a949bf | 398 | |
phillippsm | 0:d54bb6a949bf | 399 | return last_value; |
phillippsm | 0:d54bb6a949bf | 400 | } |
phillippsm | 0:d54bb6a949bf | 401 | |
phillippsm | 0:d54bb6a949bf | 402 | |
phillippsm | 0:d54bb6a949bf | 403 | |
phillippsm | 0:d54bb6a949bf | 404 | // Derived RC class constructors |
phillippsm | 0:d54bb6a949bf | 405 | QTRSensorsRC::QTRSensorsRC() |
phillippsm | 0:d54bb6a949bf | 406 | { |
phillippsm | 0:d54bb6a949bf | 407 | calibratedMinimumOn = 0; |
phillippsm | 0:d54bb6a949bf | 408 | calibratedMaximumOn = 0; |
phillippsm | 0:d54bb6a949bf | 409 | calibratedMinimumOff = 0; |
phillippsm | 0:d54bb6a949bf | 410 | calibratedMaximumOff = 0; |
phillippsm | 0:d54bb6a949bf | 411 | _pins = 0; |
phillippsm | 0:d54bb6a949bf | 412 | } |
phillippsm | 0:d54bb6a949bf | 413 | |
phillippsm | 0:d54bb6a949bf | 414 | QTRSensorsRC::QTRSensorsRC(PinName* pins, |
phillippsm | 1:a664ab7aba8d | 415 | unsigned char numSensors, unsigned int timeout, PinName emitterPin) |
phillippsm | 0:d54bb6a949bf | 416 | { |
phillippsm | 0:d54bb6a949bf | 417 | _pins = 0; |
phillippsm | 0:d54bb6a949bf | 418 | |
phillippsm | 0:d54bb6a949bf | 419 | init(pins, numSensors, timeout, emitterPin); |
phillippsm | 0:d54bb6a949bf | 420 | } |
phillippsm | 0:d54bb6a949bf | 421 | |
phillippsm | 0:d54bb6a949bf | 422 | |
phillippsm | 0:d54bb6a949bf | 423 | // The array 'pins' contains the Arduino pin number for each sensor. |
phillippsm | 0:d54bb6a949bf | 424 | |
phillippsm | 0:d54bb6a949bf | 425 | // 'numSensors' specifies the length of the 'pins' array (i.e. the |
phillippsm | 0:d54bb6a949bf | 426 | // number of QTR-RC sensors you are using). numSensors must be |
phillippsm | 0:d54bb6a949bf | 427 | // no greater than 16. |
phillippsm | 0:d54bb6a949bf | 428 | |
phillippsm | 0:d54bb6a949bf | 429 | // 'timeout' specifies the length of time in microseconds beyond |
phillippsm | 0:d54bb6a949bf | 430 | // which you consider the sensor reading completely black. That is to say, |
phillippsm | 0:d54bb6a949bf | 431 | // if the pulse length for a pin exceeds 'timeout', pulse timing will stop |
phillippsm | 0:d54bb6a949bf | 432 | // and the reading for that pin will be considered full black. |
phillippsm | 0:d54bb6a949bf | 433 | // It is recommended that you set timeout to be between 1000 and |
phillippsm | 0:d54bb6a949bf | 434 | // 3000 us, depending on things like the height of your sensors and |
phillippsm | 0:d54bb6a949bf | 435 | // ambient lighting. Using timeout allows you to shorten the |
phillippsm | 0:d54bb6a949bf | 436 | // duration of a sensor-reading cycle while still maintaining |
phillippsm | 0:d54bb6a949bf | 437 | // useful analog measurements of reflectance |
phillippsm | 0:d54bb6a949bf | 438 | |
phillippsm | 0:d54bb6a949bf | 439 | // 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC |
phillippsm | 0:d54bb6a949bf | 440 | // modules. If you are using a 1RC (i.e. if there is no emitter pin), |
phillippsm | 0:d54bb6a949bf | 441 | // or if you just want the emitters on all the time and don't want to |
phillippsm | 0:d54bb6a949bf | 442 | // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN). |
phillippsm | 0:d54bb6a949bf | 443 | void QTRSensorsRC::init(PinName* pins, |
phillippsm | 1:a664ab7aba8d | 444 | unsigned char numSensors, |
phillippsm | 1:a664ab7aba8d | 445 | unsigned int timeout, PinName emitterPin) |
phillippsm | 0:d54bb6a949bf | 446 | { |
phillippsm | 0:d54bb6a949bf | 447 | QTRSensors::init(pins, numSensors, emitterPin, false); |
phillippsm | 0:d54bb6a949bf | 448 | |
phillippsm | 0:d54bb6a949bf | 449 | _maxValue = timeout; |
phillippsm | 0:d54bb6a949bf | 450 | } |
phillippsm | 0:d54bb6a949bf | 451 | |
phillippsm | 0:d54bb6a949bf | 452 | |
phillippsm | 0:d54bb6a949bf | 453 | // Reads the sensor values into an array. There *MUST* be space |
phillippsm | 0:d54bb6a949bf | 454 | // for as many values as there were sensors specified in the constructor. |
phillippsm | 0:d54bb6a949bf | 455 | // Example usage: |
phillippsm | 0:d54bb6a949bf | 456 | // unsigned int sensor_values[8]; |
phillippsm | 0:d54bb6a949bf | 457 | // sensors.read(sensor_values); |
phillippsm | 0:d54bb6a949bf | 458 | // ... |
phillippsm | 0:d54bb6a949bf | 459 | // The values returned are in microseconds and range from 0 to |
phillippsm | 0:d54bb6a949bf | 460 | // timeout (as specified in the constructor). |
phillippsm | 0:d54bb6a949bf | 461 | void QTRSensorsRC::readPrivate(unsigned int *sensor_values) |
phillippsm | 0:d54bb6a949bf | 462 | { |
phillippsm | 0:d54bb6a949bf | 463 | unsigned char i; |
phillippsm | 0:d54bb6a949bf | 464 | |
phillippsm | 0:d54bb6a949bf | 465 | if (_pins == 0) |
phillippsm | 0:d54bb6a949bf | 466 | return; |
phillippsm | 1:a664ab7aba8d | 467 | |
phillippsm | 0:d54bb6a949bf | 468 | |
phillippsm | 1:a664ab7aba8d | 469 | for(i = 0; i < _numSensors; i++) { |
phillippsm | 0:d54bb6a949bf | 470 | sensor_values[i] = _maxValue; |
phillippsm | 1:a664ab7aba8d | 471 | _qtrPins[i]->output(); |
phillippsm | 1:a664ab7aba8d | 472 | _qtrPins[i]->write(HIGH); // make sensor line an output and drive high |
phillippsm | 0:d54bb6a949bf | 473 | } |
phillippsm | 0:d54bb6a949bf | 474 | |
phillippsm | 1:a664ab7aba8d | 475 | wait_us(10); // charge lines for 10 us |
phillippsm | 0:d54bb6a949bf | 476 | |
phillippsm | 1:a664ab7aba8d | 477 | for(i = 0; i < _numSensors; i++) { |
phillippsm | 1:a664ab7aba8d | 478 | // important: disable internal pull-up! |
phillippsm | 1:a664ab7aba8d | 479 | // ??? do we need to change the mode: _qtrPins[i]->mode(OpenDrain); |
phillippsm | 1:a664ab7aba8d | 480 | // or just change mode to input |
phillippsm | 1:a664ab7aba8d | 481 | // mbed documentation is not clear and I do not have a test sensor |
phillippsm | 1:a664ab7aba8d | 482 | _qtrPins[i]->write(LOW); |
phillippsm | 1:a664ab7aba8d | 483 | _qtrPins[i]->input(); |
phillippsm | 1:a664ab7aba8d | 484 | |
phillippsm | 0:d54bb6a949bf | 485 | } |
phillippsm | 0:d54bb6a949bf | 486 | |
phillippsm | 0:d54bb6a949bf | 487 | timer.start(); |
phillippsm | 0:d54bb6a949bf | 488 | unsigned long startTime = timer.read_ms(); |
phillippsm | 1:a664ab7aba8d | 489 | while ((timer.read_ms() - startTime) < _maxValue) { |
phillippsm | 0:d54bb6a949bf | 490 | unsigned int time = timer.read_ms() - startTime; |
phillippsm | 1:a664ab7aba8d | 491 | for (i = 0; i < _numSensors; i++) { |
phillippsm | 0:d54bb6a949bf | 492 | if (_qtrPins[i]->read() == LOW && time < sensor_values[i]) |
phillippsm | 0:d54bb6a949bf | 493 | sensor_values[i] = time; |
phillippsm | 0:d54bb6a949bf | 494 | } |
phillippsm | 0:d54bb6a949bf | 495 | } |
phillippsm | 0:d54bb6a949bf | 496 | |
phillippsm | 0:d54bb6a949bf | 497 | timer.stop(); |
phillippsm | 0:d54bb6a949bf | 498 | } |
phillippsm | 0:d54bb6a949bf | 499 | |
phillippsm | 0:d54bb6a949bf | 500 | |
phillippsm | 0:d54bb6a949bf | 501 | |
phillippsm | 0:d54bb6a949bf | 502 | // Derived Analog class constructors |
phillippsm | 0:d54bb6a949bf | 503 | QTRSensorsAnalog::QTRSensorsAnalog() |
phillippsm | 0:d54bb6a949bf | 504 | { |
phillippsm | 0:d54bb6a949bf | 505 | calibratedMinimumOn = 0; |
phillippsm | 0:d54bb6a949bf | 506 | calibratedMaximumOn = 0; |
phillippsm | 0:d54bb6a949bf | 507 | calibratedMinimumOff = 0; |
phillippsm | 0:d54bb6a949bf | 508 | calibratedMaximumOff = 0; |
phillippsm | 0:d54bb6a949bf | 509 | _pins = 0; |
phillippsm | 0:d54bb6a949bf | 510 | } |
phillippsm | 0:d54bb6a949bf | 511 | |
phillippsm | 0:d54bb6a949bf | 512 | QTRSensorsAnalog::QTRSensorsAnalog(PinName* pins, |
phillippsm | 1:a664ab7aba8d | 513 | unsigned char numSensors, |
phillippsm | 1:a664ab7aba8d | 514 | unsigned char numSamplesPerSensor, |
phillippsm | 1:a664ab7aba8d | 515 | PinName emitterPin) |
phillippsm | 0:d54bb6a949bf | 516 | { |
phillippsm | 0:d54bb6a949bf | 517 | _pins = 0; |
phillippsm | 1:a664ab7aba8d | 518 | |
phillippsm | 0:d54bb6a949bf | 519 | // this is analog - so use analog = true as a parameter |
phillippsm | 0:d54bb6a949bf | 520 | |
phillippsm | 0:d54bb6a949bf | 521 | init(pins, numSensors, numSamplesPerSensor, emitterPin); |
phillippsm | 0:d54bb6a949bf | 522 | } |
phillippsm | 0:d54bb6a949bf | 523 | |
phillippsm | 0:d54bb6a949bf | 524 | |
phillippsm | 0:d54bb6a949bf | 525 | // the array 'pins' contains the Arduino analog pin assignment for each |
phillippsm | 0:d54bb6a949bf | 526 | // sensor. For example, if pins is {0, 1, 7}, sensor 1 is on |
phillippsm | 0:d54bb6a949bf | 527 | // Arduino analog input 0, sensor 2 is on Arduino analog input 1, |
phillippsm | 0:d54bb6a949bf | 528 | // and sensor 3 is on Arduino analog input 7. |
phillippsm | 0:d54bb6a949bf | 529 | |
phillippsm | 0:d54bb6a949bf | 530 | // 'numSensors' specifies the length of the 'analogPins' array (i.e. the |
phillippsm | 0:d54bb6a949bf | 531 | // number of QTR-A sensors you are using). numSensors must be |
phillippsm | 0:d54bb6a949bf | 532 | // no greater than 16. |
phillippsm | 0:d54bb6a949bf | 533 | |
phillippsm | 0:d54bb6a949bf | 534 | // 'numSamplesPerSensor' indicates the number of 10-bit analog samples |
phillippsm | 0:d54bb6a949bf | 535 | // to average per channel (i.e. per sensor) for each reading. The total |
phillippsm | 0:d54bb6a949bf | 536 | // number of analog-to-digital conversions performed will be equal to |
phillippsm | 0:d54bb6a949bf | 537 | // numSensors*numSamplesPerSensor. Note that it takes about 100 us to |
phillippsm | 0:d54bb6a949bf | 538 | // perform a single analog-to-digital conversion, so: |
phillippsm | 0:d54bb6a949bf | 539 | // if numSamplesPerSensor is 4 and numSensors is 6, it will take |
phillippsm | 0:d54bb6a949bf | 540 | // 4 * 6 * 100 us = ~2.5 ms to perform a full readLine(). |
phillippsm | 0:d54bb6a949bf | 541 | // Increasing this parameter increases noise suppression at the cost of |
phillippsm | 0:d54bb6a949bf | 542 | // sample rate. The recommended value is 4. |
phillippsm | 0:d54bb6a949bf | 543 | |
phillippsm | 0:d54bb6a949bf | 544 | // 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC |
phillippsm | 0:d54bb6a949bf | 545 | // modules. If you are using a 1RC (i.e. if there is no emitter pin), |
phillippsm | 0:d54bb6a949bf | 546 | // or if you just want the emitters on all the time and don't want to |
phillippsm | 0:d54bb6a949bf | 547 | // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN). |
phillippsm | 0:d54bb6a949bf | 548 | void QTRSensorsAnalog::init(PinName* pins, |
phillippsm | 1:a664ab7aba8d | 549 | unsigned char numSensors, |
phillippsm | 1:a664ab7aba8d | 550 | unsigned char numSamplesPerSensor, |
phillippsm | 1:a664ab7aba8d | 551 | PinName emitterPin) |
phillippsm | 0:d54bb6a949bf | 552 | { |
phillippsm | 0:d54bb6a949bf | 553 | QTRSensors::init(pins, numSensors, emitterPin, true); |
phillippsm | 0:d54bb6a949bf | 554 | |
phillippsm | 0:d54bb6a949bf | 555 | _numSamplesPerSensor = numSamplesPerSensor; |
phillippsm | 0:d54bb6a949bf | 556 | _maxValue = 1023; // this is the maximum returned by the A/D conversion |
phillippsm | 0:d54bb6a949bf | 557 | } |
phillippsm | 0:d54bb6a949bf | 558 | |
phillippsm | 0:d54bb6a949bf | 559 | |
phillippsm | 0:d54bb6a949bf | 560 | // Reads the sensor values into an array. There *MUST* be space |
phillippsm | 0:d54bb6a949bf | 561 | // for as many values as there were sensors specified in the constructor. |
phillippsm | 0:d54bb6a949bf | 562 | // Example usage: |
phillippsm | 0:d54bb6a949bf | 563 | // unsigned int sensor_values[8]; |
phillippsm | 0:d54bb6a949bf | 564 | // sensors.read(sensor_values); |
phillippsm | 0:d54bb6a949bf | 565 | // The values returned are a measure of the reflectance in terms of a |
phillippsm | 0:d54bb6a949bf | 566 | // 10-bit ADC average with higher values corresponding to lower |
phillippsm | 0:d54bb6a949bf | 567 | // reflectance (e.g. a black surface or a void). |
phillippsm | 0:d54bb6a949bf | 568 | void QTRSensorsAnalog::readPrivate(unsigned int *sensor_values) |
phillippsm | 0:d54bb6a949bf | 569 | { |
phillippsm | 0:d54bb6a949bf | 570 | unsigned char i, j; |
phillippsm | 0:d54bb6a949bf | 571 | |
phillippsm | 0:d54bb6a949bf | 572 | if (_pins == 0) |
phillippsm | 0:d54bb6a949bf | 573 | return; |
phillippsm | 0:d54bb6a949bf | 574 | |
phillippsm | 0:d54bb6a949bf | 575 | // reset the values |
phillippsm | 0:d54bb6a949bf | 576 | for(i = 0; i < _numSensors; i++) |
phillippsm | 0:d54bb6a949bf | 577 | sensor_values[i] = 0; |
phillippsm | 0:d54bb6a949bf | 578 | |
phillippsm | 1:a664ab7aba8d | 579 | for (j = 0; j < _numSamplesPerSensor; j++) { |
phillippsm | 1:a664ab7aba8d | 580 | for (i = 0; i < _numSensors; i++) { |
phillippsm | 0:d54bb6a949bf | 581 | sensor_values[i] += (unsigned int) _qtrAIPins[i]->read_u16(); // add the conversion result |
phillippsm | 0:d54bb6a949bf | 582 | } |
phillippsm | 0:d54bb6a949bf | 583 | } |
phillippsm | 0:d54bb6a949bf | 584 | |
phillippsm | 0:d54bb6a949bf | 585 | // get the rounded average of the readings for each sensor |
phillippsm | 0:d54bb6a949bf | 586 | for (i = 0; i < _numSensors; i++) |
phillippsm | 0:d54bb6a949bf | 587 | sensor_values[i] = (sensor_values[i] + (_numSamplesPerSensor >> 1)) / |
phillippsm | 1:a664ab7aba8d | 588 | _numSamplesPerSensor; |
phillippsm | 0:d54bb6a949bf | 589 | } |
phillippsm | 0:d54bb6a949bf | 590 | |
phillippsm | 0:d54bb6a949bf | 591 | // the destructor frees up allocated memory |
phillippsm | 0:d54bb6a949bf | 592 | QTRSensors::~QTRSensors() |
phillippsm | 0:d54bb6a949bf | 593 | { |
phillippsm | 0:d54bb6a949bf | 594 | if(calibratedMinimumOff) |
phillippsm | 0:d54bb6a949bf | 595 | free(calibratedMinimumOff); |
phillippsm | 0:d54bb6a949bf | 596 | if(calibratedMinimumOn) |
phillippsm | 0:d54bb6a949bf | 597 | free(calibratedMinimumOn); |
phillippsm | 0:d54bb6a949bf | 598 | if(calibratedMaximumOff) |
phillippsm | 0:d54bb6a949bf | 599 | free(calibratedMaximumOff); |
phillippsm | 0:d54bb6a949bf | 600 | if(calibratedMaximumOn) |
phillippsm | 0:d54bb6a949bf | 601 | free(calibratedMaximumOn); |
phillippsm | 0:d54bb6a949bf | 602 | if (_pins) |
phillippsm | 0:d54bb6a949bf | 603 | free(_pins); |
phillippsm | 0:d54bb6a949bf | 604 | unsigned int i; |
phillippsm | 0:d54bb6a949bf | 605 | for (i = 0; i < _numSensors; i++) { |
phillippsm | 0:d54bb6a949bf | 606 | if (_analog) { |
phillippsm | 0:d54bb6a949bf | 607 | delete _qtrAIPins[i]; |
phillippsm | 0:d54bb6a949bf | 608 | } else { |
phillippsm | 0:d54bb6a949bf | 609 | delete _qtrPins[i]; |
phillippsm | 0:d54bb6a949bf | 610 | } |
phillippsm | 0:d54bb6a949bf | 611 | } |
phillippsm | 0:d54bb6a949bf | 612 | if (_analog) { |
phillippsm | 0:d54bb6a949bf | 613 | _qtrAIPins.clear(); |
phillippsm | 0:d54bb6a949bf | 614 | vector<AnalogIn *>().swap(_qtrAIPins); |
phillippsm | 0:d54bb6a949bf | 615 | } else { |
phillippsm | 1:a664ab7aba8d | 616 | _qtrPins.clear(); |
phillippsm | 1:a664ab7aba8d | 617 | vector<DigitalInOut *>().swap(_qtrPins); |
phillippsm | 0:d54bb6a949bf | 618 | } |
phillippsm | 0:d54bb6a949bf | 619 | } |