QTRSensors.cpp

00001 /*
00002   QTRSensors.cpp - Arduino library for using Pololu QTR reflectance
00003     sensors and reflectance sensor arrays: QTR-1A, QTR-8A, QTR-1RC, and
00004     QTR-8RC.  The object used will determine the type of the sensor (either
00005     QTR-xA or QTR-xRC).  Then simply specify in the constructor which
00006     Arduino I/O pins are connected to a QTR sensor, and the read() method
00007     will obtain reflectance measurements for those sensors.  Smaller sensor
00008     values correspond to higher reflectance (e.g. white) while larger
00009     sensor values correspond to lower reflectance (e.g. black or a void).
00010 
00011     * QTRSensorsRC should be used for QTR-1RC and QTR-8RC sensors.
00012     * QTRSensorsAnalog should be used for QTR-1A and QTR-8A sensors.
00013 */
00014 
00015 /*
00016  * Written by Ben Schmidel et al., October 4, 2010.
00017  * Copyright (c) 2008-2012 Pololu Corporation. For more information, see
00018  *
00019  *   http://www.pololu.com
00020  *   http://forum.pololu.com
00021  *   http://www.pololu.com/docs/0J19
00022  *
00023  * You may freely modify and share this code, as long as you keep this
00024  * notice intact (including the two links above).  Licensed under the
00025  * Creative Commons BY-SA 3.0 license:
00026  *
00027  *   http://creativecommons.org/licenses/by-sa/3.0/
00028  *
00029  * Disclaimer: To the extent permitted by law, Pololu provides this work
00030  * without any warranty.  It might be defective, in which case you agree
00031  * to be responsible for all resulting costs and damages.
00032  *
00033  * Modified by Matthew Phillipps, August 24, 2015
00034  * Adapted to mbed platform (especially STM Nucleo boards)
00035  * Some changes to memory management
00036  */
00037 
00038 #include <stdlib.h>
00039 #include "QTRSensors.h"
00040 #include "mbed.h"
00041 
00042 
00043 Timer timer;
00044 
00045 
00046 // Base class data member initialization (called by derived class init())
00047 void QTRSensors::init(PinName *pins, unsigned char numSensors,
00048                       PinName emitterPin, bool analog = false)
00049 {
00050     calibratedMinimumOn=0;
00051     calibratedMaximumOn=0;
00052     calibratedMinimumOff=0;
00053     calibratedMaximumOff=0;
00054 
00055     if (numSensors > QTR_MAX_SENSORS)
00056         _numSensors = QTR_MAX_SENSORS;
00057     else
00058         _numSensors = numSensors;
00059 
00060 
00061     if (_pins == 0) {
00062         _pins = (PinName *)malloc(sizeof(PinName)*_numSensors);
00063         if (_pins == 0)
00064             return;
00065     }
00066     
00067     unsigned char i;
00068     // Copy parameter values to local storage
00069     for (i = 0; i < _numSensors; i++) {
00070         _pins[i] = pins[i];
00071     }
00072 
00073     // Allocate the arrays
00074     // Allocate Space for Calibrated Maximum On Values   
00075     calibratedMaximumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
00076 
00077     // If the malloc failed, don't continue.
00078     if(calibratedMaximumOn == 0)
00079         return;
00080 
00081     // Initialize the max and min calibrated values to values that
00082     // will cause the first reading to update them.
00083 
00084     for(i=0; i<_numSensors; i++)
00085         calibratedMaximumOn[i] = 0;
00086 
00087     // Allocate Space for Calibrated Maximum Off Values
00088     calibratedMaximumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
00089 
00090     // If the malloc failed, don't continue.
00091     if(calibratedMaximumOff == 0)
00092         return;
00093 
00094     // Initialize the max and min calibrated values to values that
00095     // will cause the first reading to update them.
00096 
00097     for(i=0; i<_numSensors; i++)
00098         calibratedMaximumOff[i] = 0;
00099         
00100     // Allocate Space for Calibrated Minimum On Values
00101     calibratedMinimumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
00102 
00103     // If the malloc failed, don't continue.
00104     if(calibratedMinimumOn == 0)
00105         return;
00106 
00107     for(i=0; i<_numSensors; i++)
00108         calibratedMinimumOn[i] = _maxValue;
00109 
00110     // Allocate Space for Calibrated Minimum Off Values
00111     calibratedMinimumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
00112 
00113     // If the malloc failed, don't continue.
00114     if(calibratedMinimumOff == 0)
00115         return;
00116 
00117     for(i=0; i<_numSensors; i++)
00118         calibratedMinimumOff[i] = _maxValue;
00119 
00120     // emitter pin is used for DigitalOut
00121     // So we create a DigitalOut on that pin
00122     _emitterPin = emitterPin;
00123     _emitter = new DigitalOut(emitterPin);
00124 
00125     // If we have an Analog Sensor then we wil used AnalogIn on the pins provided
00126     // We use a Vector for our collection of pins
00127     // Here we reserve space for the pins
00128     _analog = analog;
00129     if (_analog) {
00130         _qtrAIPins.reserve(_numSensors);
00131     } else {
00132         // Not analog - so we use _qtrPins (which is a Vector on DigitalInOut)
00133         _qtrPins.reserve(_numSensors);
00134     }
00135     // Create the pins and push onto the vectors
00136     for (i = 0; i < _numSensors; i++) {
00137         if (_analog) {
00138             _qtrAIPins.push_back(new AnalogIn(pins[i]));
00139         } else {
00140             _qtrPins.push_back(new DigitalInOut(pins[i]));
00141         }
00142     }
00143 
00144 }
00145 
00146 
00147 // Reads the sensor values into an array. There *MUST* be space
00148 // for as many values as there were sensors specified in the constructor.
00149 // Example usage:
00150 // unsigned int sensor_values[8];
00151 // sensors.read(sensor_values);
00152 // The values returned are a measure of the reflectance in abstract units,
00153 // with higher values corresponding to lower reflectance (e.g. a black
00154 // surface or a void).
00155 void QTRSensors::read(unsigned int *sensor_values, unsigned char readMode)
00156 {
00157     unsigned int off_values[QTR_MAX_SENSORS];
00158     unsigned char i;
00159 
00160 
00161     if(readMode == QTR_EMITTERS_ON || readMode == QTR_EMITTERS_ON_AND_OFF) {
00162         emittersOn();
00163     } else {
00164         emittersOff();
00165     }
00166 
00167 
00168     readPrivate(sensor_values);
00169 
00170     emittersOff();
00171 
00172     if(readMode == QTR_EMITTERS_ON_AND_OFF) {
00173         readPrivate(off_values);
00174 
00175         for(i=0; i<_numSensors; i++) {
00176             sensor_values[i] += _maxValue - off_values[i];
00177         }
00178     }
00179 }
00180 
00181 
00182 // Turn the IR LEDs off and on.  This is mainly for use by the
00183 // read method, and calling these functions before or
00184 // after the reading the sensors will have no effect on the
00185 // readings, but you may wish to use these for testing purposes.
00186 void QTRSensors::emittersOff()
00187 {
00188     if (_emitterPin == QTR_NO_EMITTER_PIN)
00189         return;
00190 
00191     _emitter->write(LOW);
00192     wait_us(200); // wait 200 microseconds for the emitters to settle
00193 }
00194 
00195 void QTRSensors::emittersOn()
00196 {
00197     if (_emitterPin == QTR_NO_EMITTER_PIN)
00198         return;
00199 
00200     _emitter->write(HIGH);
00201     wait_us(200); // wait 200 microseconds for the emitters to settle
00202 }
00203 
00204 // Resets the calibration.
00205 void QTRSensors::resetCalibration()
00206 {
00207     unsigned char i;
00208     for(i=0; i<_numSensors; i++) {
00209         if(calibratedMinimumOn)
00210             calibratedMinimumOn[i] = _maxValue;
00211         if(calibratedMinimumOff)
00212             calibratedMinimumOff[i] = _maxValue;
00213         if(calibratedMaximumOn)
00214             calibratedMaximumOn[i] = 0;
00215         if(calibratedMaximumOff)
00216             calibratedMaximumOff[i] = 0;
00217     }
00218 }
00219 
00220 // Reads the sensors 10 times and uses the results for
00221 // calibration.  The sensor values are not returned; instead, the
00222 // maximum and minimum values found over time are stored internally
00223 // and used for the readCalibrated() method.
00224 void QTRSensors::calibrate(unsigned char readMode)
00225 {
00226     if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON) {
00227         calibrateOnOrOff(&calibratedMinimumOn,
00228                          &calibratedMaximumOn,
00229                          QTR_EMITTERS_ON);
00230     }
00231 
00232 
00233     if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF) {
00234         calibrateOnOrOff(&calibratedMinimumOff,
00235                          &calibratedMaximumOff,
00236                          QTR_EMITTERS_OFF);
00237     }
00238 }
00239 
00240 void QTRSensors::calibrateOnOrOff(unsigned int **calibratedMinimum,
00241                                   unsigned int **calibratedMaximum,
00242                                   unsigned char readMode)
00243 {
00244     int i;
00245     unsigned int sensor_values[16];
00246     unsigned int max_sensor_values[16];
00247     unsigned int min_sensor_values[16];
00248 
00249     // initialisation of calibrated sensor values moved to init()
00250 
00251     int j;
00252     for(j=0; j<10; j++) {
00253         read(sensor_values,readMode);
00254         for(i=0; i<_numSensors; i++) {
00255             // set the max we found THIS time
00256             if(j == 0 || max_sensor_values[i] < sensor_values[i])
00257                 max_sensor_values[i] = sensor_values[i];
00258 
00259             // set the min we found THIS time
00260             if(j == 0 || min_sensor_values[i] > sensor_values[i])
00261                 min_sensor_values[i] = sensor_values[i];
00262         }
00263     }
00264 
00265     // record the min and max calibration values
00266     for(i=0; i<_numSensors; i++) {
00267         if(min_sensor_values[i] > (*calibratedMaximum)[i]) // this was min_sensor_values[i] > (*calibratedMaximum)[i]
00268             (*calibratedMaximum)[i] = min_sensor_values[i];
00269         if(max_sensor_values[i] < (*calibratedMinimum)[i])
00270             (*calibratedMinimum)[i] = max_sensor_values[i];
00271     }
00272 
00273 }
00274 
00275 
00276 // Returns values calibrated to a value between 0 and 1000, where
00277 // 0 corresponds to the minimum value read by calibrate() and 1000
00278 // corresponds to the maximum value.  Calibration values are
00279 // stored separately for each sensor, so that differences in the
00280 // sensors are accounted for automatically.
00281 void QTRSensors::readCalibrated(unsigned int *sensor_values, unsigned char readMode)
00282 {
00283     int i;
00284 
00285     // if not calibrated, do nothing
00286     if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF)
00287         if(!calibratedMinimumOff || !calibratedMaximumOff)
00288             return;
00289     if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON)
00290         if(!calibratedMinimumOn || !calibratedMaximumOn)
00291             return;
00292 
00293     // read the needed values
00294     read(sensor_values,readMode);
00295 
00296     for(i=0; i<_numSensors; i++) {
00297         unsigned int calmin,calmax;
00298         unsigned int denominator;
00299 
00300         // find the correct calibration
00301         if(readMode == QTR_EMITTERS_ON) {
00302             calmax = calibratedMaximumOn[i];
00303             calmin = calibratedMinimumOn[i];
00304         } else if(readMode == QTR_EMITTERS_OFF) {
00305             calmax = calibratedMaximumOff[i];
00306             calmin = calibratedMinimumOff[i];
00307         } else { // QTR_EMITTERS_ON_AND_OFF
00308 
00309             if(calibratedMinimumOff[i] < calibratedMinimumOn[i]) // no meaningful signal
00310                 calmin = _maxValue;
00311             else
00312                 calmin = calibratedMinimumOn[i] + _maxValue - calibratedMinimumOff[i]; // this won't go past _maxValue
00313 
00314             if(calibratedMaximumOff[i] < calibratedMaximumOn[i]) // no meaningful signal
00315                 calmax = _maxValue;
00316             else
00317                 calmax = calibratedMaximumOn[i] + _maxValue - calibratedMaximumOff[i]; // this won't go past _maxValue
00318         }
00319 
00320         denominator = calmax - calmin;
00321 
00322         signed int x = 0;
00323         if(denominator != 0)
00324             x = (((signed long)sensor_values[i]) - calmin)
00325                 * 1000 / denominator;
00326         if(x < 0)
00327             x = 0;
00328         else if(x > 1000)
00329             x = 1000;
00330         sensor_values[i] = x;
00331     }
00332 
00333 }
00334 
00335 
00336 // Operates the same as read calibrated, but also returns an
00337 // estimated position of the robot with respect to a line. The
00338 // estimate is made using a weighted average of the sensor indices
00339 // multiplied by 1000, so that a return value of 0 indicates that
00340 // the line is directly below sensor 0, a return value of 1000
00341 // indicates that the line is directly below sensor 1, 2000
00342 // indicates that it's below sensor 2000, etc.  Intermediate
00343 // values indicate that the line is between two sensors.  The
00344 // formula is:
00345 //
00346 //    0*value0 + 1000*value1 + 2000*value2 + ...
00347 //   --------------------------------------------
00348 //         value0  +  value1  +  value2 + ...
00349 //
00350 // By default, this function assumes a dark line (high values)
00351 // surrounded by white (low values).  If your line is light on
00352 // black, set the optional second argument white_line to true.  In
00353 // this case, each sensor value will be replaced by (1000-value)
00354 // before the averaging.
00355 int QTRSensors::readLine(unsigned int *sensor_values,
00356                          unsigned char readMode, unsigned char white_line)
00357 {
00358     unsigned char i, on_line = 0;
00359     unsigned long avg; // this is for the weighted total, which is long
00360     // before division
00361     unsigned int sum; // this is for the denominator which is <= 64000
00362     static int last_value=0; // assume initially that the line is left.
00363 
00364     readCalibrated(sensor_values, readMode);
00365 
00366     avg = 0;
00367     sum = 0;
00368 
00369     for(i=0; i<_numSensors; i++) {
00370         int value = sensor_values[i];
00371         if(white_line)
00372             value = 1000-value;
00373 
00374         // keep track of whether we see the line at all
00375         if(value > 200) {
00376             on_line = 1;
00377         }
00378 
00379         // only average in values that are above a noise threshold
00380         if(value > 50) {
00381             avg += (long)(value) * (i * 1000);
00382             sum += value;
00383         }
00384     }
00385 
00386     if(!on_line) {
00387         // If it last read to the left of center, return 0.
00388         if(last_value < (_numSensors-1)*1000/2)
00389             return 0;
00390 
00391         // If it last read to the right of center, return the max.
00392         else
00393             return (_numSensors-1)*1000;
00394 
00395     }
00396 
00397     last_value = avg/sum;
00398 
00399     return last_value;
00400 }
00401 
00402 
00403 
00404 // Derived RC class constructors
00405 QTRSensorsRC::QTRSensorsRC()
00406 {
00407     calibratedMinimumOn = 0;
00408     calibratedMaximumOn = 0;
00409     calibratedMinimumOff = 0;
00410     calibratedMaximumOff = 0;
00411     _pins = 0;
00412 }
00413 
00414 QTRSensorsRC::QTRSensorsRC(PinName* pins,
00415                            unsigned char numSensors, unsigned int timeout, PinName emitterPin)
00416 {
00417     _pins = 0;
00418 
00419     init(pins, numSensors, timeout, emitterPin);
00420 }
00421 
00422 
00423 // The array 'pins' contains the Arduino pin number for each sensor.
00424 
00425 // 'numSensors' specifies the length of the 'pins' array (i.e. the
00426 // number of QTR-RC sensors you are using).  numSensors must be
00427 // no greater than 16.
00428 
00429 // 'timeout' specifies the length of time in microseconds beyond
00430 // which you consider the sensor reading completely black.  That is to say,
00431 // if the pulse length for a pin exceeds 'timeout', pulse timing will stop
00432 // and the reading for that pin will be considered full black.
00433 // It is recommended that you set timeout to be between 1000 and
00434 // 3000 us, depending on things like the height of your sensors and
00435 // ambient lighting.  Using timeout allows you to shorten the
00436 // duration of a sensor-reading cycle while still maintaining
00437 // useful analog measurements of reflectance
00438 
00439 // 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
00440 // modules.  If you are using a 1RC (i.e. if there is no emitter pin),
00441 // or if you just want the emitters on all the time and don't want to
00442 // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
00443 void QTRSensorsRC::init(PinName* pins,
00444                         unsigned char numSensors,
00445                         unsigned int timeout, PinName emitterPin)
00446 {
00447     QTRSensors::init(pins, numSensors, emitterPin, false);
00448 
00449     _maxValue = timeout;
00450 }
00451 
00452 
00453 // Reads the sensor values into an array. There *MUST* be space
00454 // for as many values as there were sensors specified in the constructor.
00455 // Example usage:
00456 // unsigned int sensor_values[8];
00457 // sensors.read(sensor_values);
00458 // ...
00459 // The values returned are in microseconds and range from 0 to
00460 // timeout (as specified in the constructor).
00461 void QTRSensorsRC::readPrivate(unsigned int *sensor_values)
00462 {
00463     unsigned char i;
00464 
00465     if (_pins == 0)
00466         return;
00467 
00468 
00469     for(i = 0; i < _numSensors; i++) {
00470         sensor_values[i] = _maxValue;
00471         _qtrPins[i]->output();
00472         _qtrPins[i]->write(HIGH);   // make sensor line an output and drive high
00473     }
00474 
00475     wait_us(10);              // charge lines for 10 us
00476 
00477     for(i = 0; i < _numSensors; i++) {
00478         // important: disable internal pull-up!
00479         // ??? do we need to change the mode: _qtrPins[i]->mode(OpenDrain);
00480         //     or just change mode to input
00481         //     mbed documentation is not clear and I do not have a test sensor
00482         _qtrPins[i]->write(LOW);
00483         _qtrPins[i]->input();
00484 
00485     }
00486 
00487     timer.start();
00488     unsigned long startTime = timer.read_ms();
00489     while ((timer.read_ms() - startTime) < _maxValue) {
00490         unsigned int time = timer.read_ms() - startTime;
00491         for (i = 0; i < _numSensors; i++) {
00492             if (_qtrPins[i]->read() == LOW && time < sensor_values[i])
00493                 sensor_values[i] = time;
00494         }
00495     }
00496 
00497     timer.stop();
00498 }
00499 
00500 
00501 
00502 // Derived Analog class constructors
00503 QTRSensorsAnalog::QTRSensorsAnalog()
00504 {
00505     calibratedMinimumOn = 0;
00506     calibratedMaximumOn = 0;
00507     calibratedMinimumOff = 0;
00508     calibratedMaximumOff = 0;
00509     _pins = 0;
00510 }
00511 
00512 QTRSensorsAnalog::QTRSensorsAnalog(PinName* pins,
00513                                    unsigned char numSensors,
00514                                    unsigned char numSamplesPerSensor,
00515                                    PinName emitterPin)
00516 {
00517     _pins = 0;
00518 
00519     // this is analog - so use analog = true as a parameter
00520 
00521     init(pins, numSensors, numSamplesPerSensor, emitterPin);
00522 }
00523 
00524 
00525 // the array 'pins' contains the Arduino analog pin assignment for each
00526 // sensor.  For example, if pins is {0, 1, 7}, sensor 1 is on
00527 // Arduino analog input 0, sensor 2 is on Arduino analog input 1,
00528 // and sensor 3 is on Arduino analog input 7.
00529 
00530 // 'numSensors' specifies the length of the 'analogPins' array (i.e. the
00531 // number of QTR-A sensors you are using).  numSensors must be
00532 // no greater than 16.
00533 
00534 // 'numSamplesPerSensor' indicates the number of 10-bit analog samples
00535 // to average per channel (i.e. per sensor) for each reading.  The total
00536 // number of analog-to-digital conversions performed will be equal to
00537 // numSensors*numSamplesPerSensor.  Note that it takes about 100 us to
00538 // perform a single analog-to-digital conversion, so:
00539 // if numSamplesPerSensor is 4 and numSensors is 6, it will take
00540 // 4 * 6 * 100 us = ~2.5 ms to perform a full readLine().
00541 // Increasing this parameter increases noise suppression at the cost of
00542 // sample rate.  The recommended value is 4.
00543 
00544 // 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
00545 // modules.  If you are using a 1RC (i.e. if there is no emitter pin),
00546 // or if you just want the emitters on all the time and don't want to
00547 // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
00548 void QTRSensorsAnalog::init(PinName* pins,
00549                             unsigned char numSensors,
00550                             unsigned char numSamplesPerSensor,
00551                             PinName emitterPin)
00552 {
00553     QTRSensors::init(pins, numSensors, emitterPin, true);
00554 
00555     _numSamplesPerSensor = numSamplesPerSensor;
00556     _maxValue = 1023; // this is the maximum returned by the A/D conversion
00557 }
00558 
00559 
00560 // Reads the sensor values into an array. There *MUST* be space
00561 // for as many values as there were sensors specified in the constructor.
00562 // Example usage:
00563 // unsigned int sensor_values[8];
00564 // sensors.read(sensor_values);
00565 // The values returned are a measure of the reflectance in terms of a
00566 // 10-bit ADC average with higher values corresponding to lower
00567 // reflectance (e.g. a black surface or a void).
00568 void QTRSensorsAnalog::readPrivate(unsigned int *sensor_values)
00569 {
00570     unsigned char i, j;
00571 
00572     if (_pins == 0)
00573         return;
00574 
00575     // reset the values
00576     for(i = 0; i < _numSensors; i++)
00577         sensor_values[i] = 0;
00578 
00579     for (j = 0; j < _numSamplesPerSensor; j++) {
00580         for (i = 0; i < _numSensors; i++) {
00581             sensor_values[i] += (unsigned int) _qtrAIPins[i]->read_u16();   // add the conversion result
00582         }
00583     }
00584 
00585     // get the rounded average of the readings for each sensor
00586     for (i = 0; i < _numSensors; i++)
00587         sensor_values[i] = (sensor_values[i] + (_numSamplesPerSensor >> 1)) /
00588                            _numSamplesPerSensor;
00589 }
00590 
00591 // the destructor frees up allocated memory
00592 QTRSensors::~QTRSensors()
00593 {
00594     if(calibratedMinimumOff)
00595         free(calibratedMinimumOff);
00596     if(calibratedMinimumOn)
00597         free(calibratedMinimumOn);
00598     if(calibratedMaximumOff)
00599         free(calibratedMaximumOff);
00600     if(calibratedMaximumOn)
00601         free(calibratedMaximumOn);
00602     if (_pins)
00603         free(_pins);
00604     unsigned int i;
00605     for (i = 0; i < _numSensors; i++) {
00606         if (_analog) {
00607             delete _qtrAIPins[i];
00608         } else {
00609             delete _qtrPins[i];
00610         }
00611     }
00612     if (_analog) {
00613         _qtrAIPins.clear();
00614         vector<AnalogIn *>().swap(_qtrAIPins);
00615     } else {
00616         _qtrPins.clear();
00617         vector<DigitalInOut *>().swap(_qtrPins);
00618     }
00619 }