Diff: QTRSensors.cpp

Revision:

1:a664ab7aba8d

Parent:

0:d54bb6a949bf

--- a/QTRSensors.cpp	Tue Aug 25 02:44:57 2015 +0000
+++ b/QTRSensors.cpp	Thu Aug 27 01:42:08 2015 +0000
@@ -45,7 +45,7 @@
 
 // Base class data member initialization (called by derived class init())
 void QTRSensors::init(PinName *pins, unsigned char numSensors,
-  PinName emitterPin, bool analog = false)
+                      PinName emitterPin, bool analog = false)
 {
     calibratedMinimumOn=0;
     calibratedMaximumOn=0;
@@ -56,90 +56,88 @@
         _numSensors = QTR_MAX_SENSORS;
     else
         _numSensors = numSensors;
-        
+
 
-    if (_pins == 0)
-    {
+    if (_pins == 0) {
         _pins = (PinName *)malloc(sizeof(PinName)*_numSensors);
         if (_pins == 0)
             return;
     }
+    
     unsigned char i;
-
-    // Allocate the arrays
-    if(calibratedMaximumOn == 0)
-    {
-        calibratedMaximumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
-
-        // If the malloc failed, don't continue.
-        if(calibratedMaximumOn == 0)
-            return;
-
-        // Initialize the max and min calibrated values to values that
-        // will cause the first reading to update them.
-
-        for(i=0;i<_numSensors;i++)
-            calibratedMaximumOn[i] = 0;
-    }
-    if(calibratedMaximumOff == 0)
-    {
-        calibratedMaximumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
-
-        // If the malloc failed, don't continue.
-        if(calibratedMaximumOff == 0)
-            return;
-
-        // Initialize the max and min calibrated values to values that
-        // will cause the first reading to update them.
-
-        for(i=0;i<_numSensors;i++)
-            calibratedMaximumOff[i] = 0;
-    }
-    if(calibratedMinimumOn == 0)
-    {
-        calibratedMinimumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
-
-        // If the malloc failed, don't continue.
-        if(calibratedMinimumOn == 0)
-            return;
-
-        for(i=0;i<_numSensors;i++)
-            calibratedMinimumOn[i] = _maxValue;
-    }
-    if(calibratedMinimumOff == 0)
-    {
-        calibratedMinimumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
-
-        // If the malloc failed, don't continue.
-        if(calibratedMinimumOff == 0)
-            return;
-
-        for(i=0;i<_numSensors;i++)
-            calibratedMinimumOff[i] = _maxValue;
-    }
-
-
-
-    for (i = 0; i < _numSensors; i++)
-    {
+    // Copy parameter values to local storage
+    for (i = 0; i < _numSensors; i++) {
         _pins[i] = pins[i];
     }
 
+    // Allocate the arrays
+    // Allocate Space for Calibrated Maximum On Values   
+    calibratedMaximumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+    // If the malloc failed, don't continue.
+    if(calibratedMaximumOn == 0)
+        return;
+
+    // Initialize the max and min calibrated values to values that
+    // will cause the first reading to update them.
+
+    for(i=0; i<_numSensors; i++)
+        calibratedMaximumOn[i] = 0;
+
+    // Allocate Space for Calibrated Maximum Off Values
+    calibratedMaximumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+    // If the malloc failed, don't continue.
+    if(calibratedMaximumOff == 0)
+        return;
+
+    // Initialize the max and min calibrated values to values that
+    // will cause the first reading to update them.
+
+    for(i=0; i<_numSensors; i++)
+        calibratedMaximumOff[i] = 0;
+        
+    // Allocate Space for Calibrated Minimum On Values
+    calibratedMinimumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+    // If the malloc failed, don't continue.
+    if(calibratedMinimumOn == 0)
+        return;
+
+    for(i=0; i<_numSensors; i++)
+        calibratedMinimumOn[i] = _maxValue;
+
+    // Allocate Space for Calibrated Minimum Off Values
+    calibratedMinimumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+    // If the malloc failed, don't continue.
+    if(calibratedMinimumOff == 0)
+        return;
+
+    for(i=0; i<_numSensors; i++)
+        calibratedMinimumOff[i] = _maxValue;
+
+    // emitter pin is used for DigitalOut
+    // So we create a DigitalOut on that pin
     _emitterPin = emitterPin;
     _emitter = new DigitalOut(emitterPin);
 
+    // If we have an Analog Sensor then we wil used AnalogIn on the pins provided
+    // We use a Vector for our collection of pins
+    // Here we reserve space for the pins
     _analog = analog;
     if (_analog) {
         _qtrAIPins.reserve(_numSensors);
     } else {
-       _qtrPins.reserve(_numSensors);
+        // Not analog - so we use _qtrPins (which is a Vector on DigitalInOut)
+        _qtrPins.reserve(_numSensors);
     }
-    for (i = 0; i < _numSensors; i++)
-    {
+    // Create the pins and push onto the vectors
+    for (i = 0; i < _numSensors; i++) {
         if (_analog) {
-           _qtrAIPins.push_back(new AnalogIn(pins[i]));
+            _qtrAIPins.push_back(new AnalogIn(pins[i]));
         } else {
-           _qtrPins.push_back(new DigitalInOut(pins[i]));
+            _qtrPins.push_back(new DigitalInOut(pins[i]));
         }
     }
 
@@ -158,27 +156,23 @@
 {
     unsigned int off_values[QTR_MAX_SENSORS];
     unsigned char i;
-    
+
 
-    if(readMode == QTR_EMITTERS_ON || readMode == QTR_EMITTERS_ON_AND_OFF)
-     { 
-        emittersOn(); }
-    else
-    {   
+    if(readMode == QTR_EMITTERS_ON || readMode == QTR_EMITTERS_ON_AND_OFF) {
+        emittersOn();
+    } else {
         emittersOff();
     }
 
 
     readPrivate(sensor_values);
-     
+
     emittersOff();
 
-    if(readMode == QTR_EMITTERS_ON_AND_OFF)
-    {
+    if(readMode == QTR_EMITTERS_ON_AND_OFF) {
         readPrivate(off_values);
 
-        for(i=0;i<_numSensors;i++)
-        {
+        for(i=0; i<_numSensors; i++) {
             sensor_values[i] += _maxValue - off_values[i];
         }
     }
@@ -193,32 +187,25 @@
 {
     if (_emitterPin == QTR_NO_EMITTER_PIN)
         return;
-//    pinMode(_emitterPin, OUTPUT);
-//    digitalWrite(_emitterPin, LOW);
-    _emitter->write(LOW); // 0 is low
-//    delayMicroseconds(200);
-    wait_ms(20); //200 was too long
 
+    _emitter->write(LOW);
+    wait_us(200); // wait 200 microseconds for the emitters to settle
 }
 
 void QTRSensors::emittersOn()
 {
     if (_emitterPin == QTR_NO_EMITTER_PIN)
         return;
-        
-    //pinMode(_emitterPin, OUTPUT);
-    //digitalWrite(_emitterPin, HIGH);
+
     _emitter->write(HIGH);
-    //delayMicroseconds(200);
-    wait_ms(20); // 200 was too long
+    wait_us(200); // wait 200 microseconds for the emitters to settle
 }
 
 // Resets the calibration.
 void QTRSensors::resetCalibration()
 {
     unsigned char i;
-    for(i=0;i<_numSensors;i++)
-    {
+    for(i=0; i<_numSensors; i++) {
         if(calibratedMinimumOn)
             calibratedMinimumOn[i] = _maxValue;
         if(calibratedMinimumOff)
@@ -236,16 +223,14 @@
 // and used for the readCalibrated() method.
 void QTRSensors::calibrate(unsigned char readMode)
 {
-    if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON)
-    {
+    if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON) {
         calibrateOnOrOff(&calibratedMinimumOn,
                          &calibratedMaximumOn,
                          QTR_EMITTERS_ON);
     }
 
 
-    if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF)
-    {
+    if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF) {
         calibrateOnOrOff(&calibratedMinimumOff,
                          &calibratedMaximumOff,
                          QTR_EMITTERS_OFF);
@@ -260,18 +245,13 @@
     unsigned int sensor_values[16];
     unsigned int max_sensor_values[16];
     unsigned int min_sensor_values[16];
-    for(i=0;i<_numSensors;i++) {
-        sensor_values[i] = 0;
-        max_sensor_values[i] = 0;
-        min_sensor_values[i] = _maxValue;
-    }
+
+    // initialisation of calibrated sensor values moved to init()
 
     int j;
-    for(j=0;j<10;j++)
-    {
+    for(j=0; j<10; j++) {
         read(sensor_values,readMode);
-        for(i=0;i<_numSensors;i++)
-        {
+        for(i=0; i<_numSensors; i++) {
             // set the max we found THIS time
             if(j == 0 || max_sensor_values[i] < sensor_values[i])
                 max_sensor_values[i] = sensor_values[i];
@@ -283,14 +263,13 @@
     }
 
     // record the min and max calibration values
-    for(i=0;i<_numSensors;i++)
-    {
+    for(i=0; i<_numSensors; i++) {
         if(min_sensor_values[i] > (*calibratedMaximum)[i]) // this was min_sensor_values[i] > (*calibratedMaximum)[i]
             (*calibratedMaximum)[i] = min_sensor_values[i];
         if(max_sensor_values[i] < (*calibratedMinimum)[i])
             (*calibratedMinimum)[i] = max_sensor_values[i];
     }
-    
+
 }
 
 
@@ -314,24 +293,18 @@
     // read the needed values
     read(sensor_values,readMode);
 
-    for(i=0;i<_numSensors;i++)
-    {
+    for(i=0; i<_numSensors; i++) {
         unsigned int calmin,calmax;
         unsigned int denominator;
 
         // find the correct calibration
-        if(readMode == QTR_EMITTERS_ON)
-        {
+        if(readMode == QTR_EMITTERS_ON) {
             calmax = calibratedMaximumOn[i];
             calmin = calibratedMinimumOn[i];
-        }
-        else if(readMode == QTR_EMITTERS_OFF)
-        {
+        } else if(readMode == QTR_EMITTERS_OFF) {
             calmax = calibratedMaximumOff[i];
             calmin = calibratedMinimumOff[i];
-        }
-        else // QTR_EMITTERS_ON_AND_OFF
-        {
+        } else { // QTR_EMITTERS_ON_AND_OFF
 
             if(calibratedMinimumOff[i] < calibratedMinimumOn[i]) // no meaningful signal
                 calmin = _maxValue;
@@ -380,11 +353,11 @@
 // this case, each sensor value will be replaced by (1000-value)
 // before the averaging.
 int QTRSensors::readLine(unsigned int *sensor_values,
-    unsigned char readMode, unsigned char white_line)
+                         unsigned char readMode, unsigned char white_line)
 {
     unsigned char i, on_line = 0;
     unsigned long avg; // this is for the weighted total, which is long
-                       // before division
+    // before division
     unsigned int sum; // this is for the denominator which is <= 64000
     static int last_value=0; // assume initially that the line is left.
 
@@ -393,7 +366,7 @@
     avg = 0;
     sum = 0;
 
-    for(i=0;i<_numSensors;i++) {
+    for(i=0; i<_numSensors; i++) {
         int value = sensor_values[i];
         if(white_line)
             value = 1000-value;
@@ -410,8 +383,7 @@
         }
     }
 
-    if(!on_line)
-    {
+    if(!on_line) {
         // If it last read to the left of center, return 0.
         if(last_value < (_numSensors-1)*1000/2)
             return 0;
@@ -440,12 +412,8 @@
 }
 
 QTRSensorsRC::QTRSensorsRC(PinName* pins,
-  unsigned char numSensors, unsigned int timeout, PinName emitterPin)
+                           unsigned char numSensors, unsigned int timeout, PinName emitterPin)
 {
-    calibratedMinimumOn = 0;
-    calibratedMaximumOn = 0;
-    calibratedMinimumOff = 0;
-    calibratedMaximumOff = 0;
     _pins = 0;
 
     init(pins, numSensors, timeout, emitterPin);
@@ -473,8 +441,8 @@
 // or if you just want the emitters on all the time and don't want to
 // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
 void QTRSensorsRC::init(PinName* pins,
-    unsigned char numSensors,
-    unsigned int timeout, PinName emitterPin)
+                        unsigned char numSensors,
+                        unsigned int timeout, PinName emitterPin)
 {
     QTRSensors::init(pins, numSensors, emitterPin, false);
 
@@ -496,31 +464,31 @@
 
     if (_pins == 0)
         return;
-        
+
 
-    for(i = 0; i < _numSensors; i++)
-    {
+    for(i = 0; i < _numSensors; i++) {
         sensor_values[i] = _maxValue;
-
-        _qtrPins[i]->write(HIGH);   // make sensor line an output
-        //pinMode(_pins[i], OUTPUT);      // drive sensor line high
+        _qtrPins[i]->output();
+        _qtrPins[i]->write(HIGH);   // make sensor line an output and drive high
     }
 
-    wait_ms(10);              // charge lines for 10 us
+    wait_us(10);              // charge lines for 10 us
 
-    for(i = 0; i < _numSensors; i++)
-    {
-// important: disable internal pull-up!
-          _qtrPins[i]->write(LOW);
+    for(i = 0; i < _numSensors; i++) {
+        // important: disable internal pull-up!
+        // ??? do we need to change the mode: _qtrPins[i]->mode(OpenDrain);
+        //     or just change mode to input
+        //     mbed documentation is not clear and I do not have a test sensor
+        _qtrPins[i]->write(LOW);
+        _qtrPins[i]->input();
+
     }
 
     timer.start();
     unsigned long startTime = timer.read_ms();
-    while ((timer.read_ms() - startTime) < _maxValue)
-    {
+    while ((timer.read_ms() - startTime) < _maxValue) {
         unsigned int time = timer.read_ms() - startTime;
-        for (i = 0; i < _numSensors; i++)
-        {
+        for (i = 0; i < _numSensors; i++) {
             if (_qtrPins[i]->read() == LOW && time < sensor_values[i])
                 sensor_values[i] = time;
         }
@@ -542,16 +510,12 @@
 }
 
 QTRSensorsAnalog::QTRSensorsAnalog(PinName* pins,
-  unsigned char numSensors,
-  unsigned char numSamplesPerSensor,
-  PinName emitterPin)
+                                   unsigned char numSensors,
+                                   unsigned char numSamplesPerSensor,
+                                   PinName emitterPin)
 {
-    calibratedMinimumOn = 0;
-    calibratedMaximumOn = 0;
-    calibratedMinimumOff = 0;
-    calibratedMaximumOff = 0;
     _pins = 0;
-    
+
     // this is analog - so use analog = true as a parameter
 
     init(pins, numSensors, numSamplesPerSensor, emitterPin);
@@ -582,9 +546,9 @@
 // or if you just want the emitters on all the time and don't want to
 // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
 void QTRSensorsAnalog::init(PinName* pins,
-    unsigned char numSensors,
-    unsigned char numSamplesPerSensor,
-    PinName emitterPin)
+                            unsigned char numSensors,
+                            unsigned char numSamplesPerSensor,
+                            PinName emitterPin)
 {
     QTRSensors::init(pins, numSensors, emitterPin, true);
 
@@ -612,10 +576,8 @@
     for(i = 0; i < _numSensors; i++)
         sensor_values[i] = 0;
 
-    for (j = 0; j < _numSamplesPerSensor; j++)
-    {
-        for (i = 0; i < _numSensors; i++)
-        {
+    for (j = 0; j < _numSamplesPerSensor; j++) {
+        for (i = 0; i < _numSensors; i++) {
             sensor_values[i] += (unsigned int) _qtrAIPins[i]->read_u16();   // add the conversion result
         }
     }
@@ -623,7 +585,7 @@
     // get the rounded average of the readings for each sensor
     for (i = 0; i < _numSensors; i++)
         sensor_values[i] = (sensor_values[i] + (_numSamplesPerSensor >> 1)) /
-            _numSamplesPerSensor;
+                           _numSamplesPerSensor;
 }
 
 // the destructor frees up allocated memory
@@ -651,7 +613,7 @@
         _qtrAIPins.clear();
         vector<AnalogIn *>().swap(_qtrAIPins);
     } else {
-       _qtrPins.clear();
-       vector<DigitalInOut *>().swap(_qtrPins);
+        _qtrPins.clear();
+        vector<DigitalInOut *>().swap(_qtrPins);
     }
 }