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