TRSensors.cpp

00001 /*  QTRSensors.h - Library for using Pololu QTR reflectance
00002     sensors and reflectance sensor arrays
00003  * Copyright (c) 2008-2012 waveshare Corporation. For more information, see
00004  *
00005  *   http://www.waveshare.com
00006  *
00007  * You may freely modify and share this code, as long as you keep this
00008  * notice intact.  
00009  *
00010  * Disclaimer: To the extent permitted by law, waveshare pr
00011  ovides this work
00012  * without any warranty.  It might be defective, in which case you agree
00013  * to be responsible for all resulting costs and damages.
00014  */
00015 
00016 
00017 #include "mbed.h"
00018 #include "TRSensors.h"
00019 
00020 #define NUMSENSORS 5
00021 
00022 SPI spi(D11, D12, D13);     // D11, D12, D13
00023 DigitalOut spi_cs(D10,1);   // PB_6: D10
00024 
00025 // Base class data member initialization (called by derived class init())
00026 TRSensors::TRSensors()
00027 {
00028     spi.format(16,0);       // 16-bit mode, mode 0
00029     //spi.frequency(2000000);
00030     spi.frequency(4000000); // 2 MHz
00031 
00032     _numSensors = NUMSENSORS;
00033     
00034     calibratedMin = (unsigned int*)malloc(sizeof(unsigned int) * _numSensors);
00035     calibratedMax = (unsigned int*)malloc(sizeof(unsigned int) * _numSensors);
00036     
00037     for(int i=0;i<_numSensors;i++)
00038     {
00039         calibratedMin[i] = 1023;
00040         calibratedMax[i] = 0;
00041     }
00042 }
00043 
00044 
00045 // Reads the sensor values using TLC1543 ADC chip into an array. 
00046 // The values returned are a measure of the reflectance in abstract units,
00047 // with higher values corresponding to lower reflectance (e.g. a black
00048 // surface or a void).
00049 void TRSensors::AnalogRead(unsigned int *sensor_values)
00050 {
00051     char i,j;
00052     unsigned int channel = 0;
00053     unsigned int values[] = {0,0,0,0,0,0,0,0}; // array
00054 
00055     for(j = 0;j < _numSensors + 1;j++) 
00056     {
00057         spi_cs = 0;
00058         wait_us(2);
00059         values[j] = spi.write(j << 12);
00060         spi_cs = 1;
00061         wait_us(21);
00062     }
00063 
00064     for(i = 0;i < _numSensors;i++)
00065     {
00066         sensor_values[i] = values[i+1];
00067     }
00068 }
00069 
00070 // Reads the sensors 10 times and uses the results for
00071 // calibration.  The sensor values are not returned; instead, the
00072 // maximum and minimum values found over time are stored internally
00073 // and used for the readCalibrated() method.
00074 void TRSensors::calibrate()
00075 {
00076     int i;
00077     unsigned int sensor_values[_numSensors];
00078     unsigned int max_sensor_values[_numSensors];
00079     unsigned int min_sensor_values[_numSensors];
00080 
00081     int j;
00082     for(j=0;j<10;j++)
00083     { 
00084         AnalogRead(sensor_values);
00085         for(i=0;i<_numSensors;i++)
00086         {
00087             // set the max we found THIS time
00088             // j==0일때 무조건 true니까 초기에 max-sensor-value 값이 sensor-values값으로 세팅된다.
00089             // 이후에 sensor_values의 값이 이전 데이터보다 클 경우에만 max-sensor-value가 sensor-values 값으로 세팅된다.
00090             // i가 Sensor 넘버
00091             // 근데 j를 0에서부터 9까지 돌리는 이유? ★★★★★★ --> 총 10번
00092             if(j == 0 || max_sensor_values[i] < sensor_values[i])
00093                 max_sensor_values[i] = sensor_values[i];
00094 
00095             // set the min we found THIS time
00096             // 위와 동일한 원리로 적용
00097             if(j == 0 || min_sensor_values[i] > sensor_values[i])
00098                 min_sensor_values[i] = sensor_values[i];
00099         }
00100     }
00101   
00102     //위 코드 다 돌아가면 나온다.
00103     //위의 코드가 각 센서마다 값이 설정되는건가보다.
00104     //위 코드 다 돌아가면 max_sensor_values 값과 min_sensor_values 값이 결정된다.
00105   // record the min and max calibration values
00106   for(i=0;i<_numSensors;i++)
00107   {
00108     if(min_sensor_values[i] > calibratedMax[i])
00109       calibratedMax[i] = min_sensor_values[i];
00110     if(max_sensor_values[i] < calibratedMin[i])
00111       calibratedMin[i] = max_sensor_values[i];
00112   }
00113 }
00114 
00115 
00116 // Returns values calibrated to a value between 0 and 1000, where
00117 // 0 corresponds to the minimum value read by calibrate() and 1000
00118 // corresponds to the maximum value.  Calibration values are
00119 // stored separately for each sensor, so that differences in the
00120 // sensors are accounted for automatically.
00121 void TRSensors::readCalibrated(unsigned int *sensor_values)
00122 {
00123     int i;
00124 
00125     // read the needed values
00126     AnalogRead(sensor_values);
00127 
00128     for(i=0;i<_numSensors;i++)
00129     {
00130         unsigned int calmin,calmax;
00131         unsigned int denominator;
00132 
00133         denominator = calibratedMax[i] - calibratedMin[i];
00134 
00135         signed int x = 0;
00136         if(denominator != 0)
00137             x = (((signed long)sensor_values[i]) - calibratedMin[i])
00138                 * 1000 / denominator;
00139         if(x < 0)
00140             x = 0;
00141         else if(x > 1000)
00142             x = 1000;
00143         sensor_values[i] = x;
00144         // sensor_values[i]는 0-1000사이의 값으로 scaling되어 나온다.
00145     }
00146 
00147 }
00148 
00149 
00150 // Operates the same as read calibrated, but also returns an
00151 // estimated position of the robot with respect to a line. The
00152 // estimate is made using a weighted average of the sensor indices
00153 // multiplied by 1000, so that a return value of 0 indicates that
00154 // the line is directly below sensor 0, a return value of 1000
00155 // indicates that the line is directly below sensor 1, 2000
00156 // indicates that it's below sensor 2000, etc.  Intermediate
00157 // values indicate that the line is between two sensors.  The
00158 // formula is:
00159 // 
00160 //    0*value0 + 1000*value1 + 2000*value2 + ...
00161 //   --------------------------------------------
00162 //         value0  +  value1  +  value2 + ...
00163 //
00164 // By default, this function assumes a dark line (high values)
00165 // surrounded by white (low values).  If your line is light on
00166 // black, set the optional second argument white_line to true.  In
00167 // this case, each sensor value will be replaced by (1000-value)
00168 // before the averaging.
00169 int TRSensors::readLine(unsigned int *sensor_values, unsigned char white_line)
00170 {
00171     unsigned char i, on_line = 0;
00172     unsigned long avg; // this is for the weighted total, which is long
00173                        // before division
00174     unsigned int sum; // this is for the denominator which is <= 64000
00175     static int last_value=0; // assume initially that the line is left.
00176 
00177     readCalibrated(sensor_values);
00178 
00179     avg = 0;
00180     sum = 0;
00181   
00182     for(i=0;i<_numSensors;i++) {
00183         int value = sensor_values[i];
00184 
00185         if(!white_line)
00186             value = 1000-value;
00187         sensor_values[i] = value;
00188         // keep track of whether we see the line at all
00189         if(value > 500) {
00190             on_line = 1;
00191         }
00192         
00193         // only average in values that are above a noise threshold
00194         if(value > 50) {
00195             avg += (long)(value) * (i * 1000);
00196             sum += value;
00197         }
00198     }
00199 
00200     if(!on_line)
00201     {
00202         // If it last read to the left of center, return 0.
00203          if(last_value < (_numSensors-1)*1000/2)    // 2000보다 last value가 작으면 0 리턴 / last value 최종 위치
00204              return 0;
00205         
00206         // If it last read to the right of center, return the max.
00207          else
00208              return (_numSensors-1)*1000;   // 2000보다 last value가 크면 4000 리턴
00209     }
00210 
00211     last_value = avg/sum;
00212 
00213     return last_value;
00214 }