IRreflection.cpp

00001 #include "IRreflection.h"
00002 
00003  
00004 
00005 // Base class data member initialization (called by derived class init())
00006 
00007 TRSensors::TRSensors(PinName MOSI,PinName MISO, PinName CLK, PinName CS) : _spi(MOSI,MISO,CLK), _spi_cs(CS)
00008 
00009 {   
00010 
00011     _spi.format(16,0);
00012 
00013     _spi.frequency(2000000);
00014 
00015     _spi_cs = 1;
00016 
00017     _numSensors = NUMSENSORS;
00018 
00019 }
00020 
00021  
00022 
00023  
00024 
00025  
00026 
00027 void TRSensors::AnalogRead(int *sensor_values)
00028 
00029 {
00030 
00031     int i,j,ch;
00032 
00033     int values[] = {0,0,0,0,0,0};
00034 
00035  
00036 
00037     for(j = 0;j < _numSensors + 1;j++)
00038 
00039     {
00040 
00041      ch =j;
00042 
00043      _spi_cs = 0;
00044 
00045      wait_us(2);
00046 
00047      values[j] = _spi.write(ch<<12);
00048 
00049      _spi_cs = 1;
00050 
00051      wait_us(21);
00052 
00053      values[j] = (values[j]>>6);
00054 
00055     }
00056 
00057     
00058 
00059     for(i = 0;i < _numSensors;i++)
00060 
00061     {
00062 
00063         sensor_values[i] = values[i+1];
00064 
00065     }
00066 
00067  
00068 
00069 }
00070 
00071  
00072 
00073 // Reads the sensors 10 times and uses the results for
00074 
00075 // calibration.  The sensor values are not returned; instead, the
00076 
00077 // maximum and minimum values found over time are stored internally
00078 
00079 // and used for the readCalibrated() method.
00080 
00081 void TRSensors::calibrate_init(int *calibratedMin, int *calibratedMax)
00082 
00083 {
00084 
00085         for(int i=0;i<_numSensors;i++)
00086 
00087     {
00088 
00089         calibratedMin[i] = 1023;
00090 
00091         calibratedMax[i] = 0;
00092 
00093     }
00094 
00095 }
00096 
00097 void TRSensors::calibrate(int *sensor_values, int *calibratedMin, int *calibratedMax)
00098 
00099 {
00100 
00101     int i=0;
00102 
00103     int j=0;
00104 
00105     
00106 
00107     for(j=0;j<10;j++)
00108 
00109     {
00110 
00111         AnalogRead(sensor_values);
00112 
00113         for(i=0;i<_numSensors;i++)
00114 
00115         {
00116 
00117             // set the max we found THIS time
00118 
00119             if(j == 0 || max_sensor_values[i] < sensor_values[i])
00120 
00121                 max_sensor_values[i] = sensor_values[i];
00122 
00123  
00124 
00125             // set the min we found THIS time
00126 
00127             if(j == 0 || min_sensor_values[i] > sensor_values[i])
00128 
00129                 min_sensor_values[i] = sensor_values[i];
00130 
00131         }
00132 
00133     }
00134 
00135     
00136 
00137   // record the min and max calibration values
00138 
00139   for(i=0;i<_numSensors;i++)
00140 
00141   {
00142 
00143     if(min_sensor_values[i] > calibratedMax[i])
00144 
00145       calibratedMax[i] = min_sensor_values[i];
00146 
00147     if(max_sensor_values[i] < calibratedMin[i])
00148 
00149       calibratedMin[i] = max_sensor_values[i];
00150 
00151   }
00152 
00153 }
00154 
00155  
00156 
00157  
00158 
00159 // Returns values calibrated to a value between 0 and 1000, where
00160 
00161 // 0 corresponds to the minimum value read by calibrate() and 1000
00162 
00163 // corresponds to the maximum value.  Calibration values are
00164 
00165 // stored separately for each sensor, so that differences in the
00166 
00167 // sensors are accounted for automatically.
00168 
00169 void TRSensors::readCalibrated(int *sensor_values, int *calibratedMin, int *calibratedMax)
00170 
00171 {
00172 
00173     int i;
00174 
00175  
00176 
00177     // read the needed values
00178 
00179     AnalogRead(sensor_values);
00180 
00181  
00182 
00183     for(i=0;i<_numSensors;i++)
00184 
00185     {
00186 
00187         int denominator;
00188 
00189  
00190 
00191         denominator = calibratedMax[i] - calibratedMin[i];
00192 
00193  
00194 
00195         int x = 0;
00196 
00197         
00198 
00199         
00200 
00201         if(denominator != 0){
00202 
00203             if(((signed int)sensor_values[i] - (signed int)calibratedMin[i])<0){
00204 
00205                 x = 0;}
00206 
00207             else {
00208 
00209                 x = ((sensor_values[i] - calibratedMin[i])*1000/denominator);
00210 
00211                 }
00212 
00213         }
00214 
00215         
00216 
00217         if( x > 1000) x = 1000;
00218 
00219         
00220 
00221         sensor_values[i] = x;
00222 
00223     
00224 
00225  
00226 
00227 }
00228 
00229  
00230 
00231 }
00232 
00233  
00234 
00235  
00236 
00237  
00238 
00239 // Operates the same as read calibrated, but also returns an
00240 
00241 // estimated position of the robot with respect to a line. The
00242 
00243 // estimate is made using a weighted average of the sensor indices
00244 
00245 // multiplied by 1000, so that a return value of 0 indicates that
00246 
00247 // the line is directly below sensor 0, a return value of 1000
00248 
00249 // indicates that the line is directly below sensor 1, 2000
00250 
00251 // indicates that it's below sensor 2000, etc.  Intermediate
00252 
00253 // values indicate that the line is between two sensors.  The
00254 
00255 // formula is:
00256 
00257 // 
00258 
00259 //    0*value0 + 1000*value1 + 2000*value2 + ...
00260 
00261 //   --------------------------------------------
00262 
00263 //         value0  +  value1  +  value2 + ...
00264 
00265 //
00266 
00267 // By default, this function assumes a dark line (high values)
00268 
00269 // surrounded by white (low values).  If your line is light on
00270 
00271 // black, set the optional second argument white_line to true.  In
00272 
00273 // this case, each sensor value will be replaced by (1000-value)
00274 
00275 // before the averaging.
00276 
00277  
00278 
00279 int TRSensors::readLine(int *sensor_values , int *calibratedMin, int *calibratedMax, int *online, char white_line)
00280 
00281 {
00282 
00283     char i, on_line = 0;
00284 
00285     long int avg; // this is for the weighted total, which is long
00286 
00287                        // before division
00288 
00289     int sum; // this is for the denominator which is <= 64000
00290 
00291     static int last_value=0; // assume initially that the line is left.
00292 
00293  
00294 
00295     readCalibrated(sensor_values, calibratedMin, calibratedMax);
00296 
00297     
00298 
00299     online[0] = 0;
00300 
00301     avg = 0;
00302 
00303     sum = 0;
00304 
00305   
00306 
00307     for(i=0;i<_numSensors;i++) {
00308 
00309         
00310 
00311         long int value = sensor_values[i];
00312 
00313  
00314 
00315         //if(!white_line) value = 1000-value;
00316 
00317         if(white_line){
00318 
00319         value = 1000-value;
00320 
00321         //sensor_values[i] = (1000 - sensor_values[i])/100;
00322 
00323         }
00324 
00325         
00326 
00327         //sensor_values[i] = value;
00328 
00329         // keep track of whether we see the line at all
00330 
00331         if(value > 300) {
00332 
00333             online[0] = 1;
00334 
00335         }
00336 
00337         
00338 
00339         // only average in values that are above a noise threshold
00340         
00341 
00342         if(value > 30) {
00343 
00344   
00345 
00346             avg += value*(1000*i);//(value/100)*(1000*i)     // * sqrt(100,i);
00347 
00348             sum += value; //(value/100);
00349 
00350         }
00351 
00352     }
00353 
00354     /*
00355 
00356     if(!on_line)
00357 
00358     {
00359 
00360         // If it last read to the left of center, return 0.
00361 
00362          if(last_value < (_numSensors-1)*1000/2)
00363 
00364              return 0;
00365 
00366         
00367 
00368         // If it last read to the right of center, return the max.
00369 
00370          else
00371 
00372              return (_numSensors-1)*1000;
00373 
00374     }
00375 
00376     */
00377 
00378     
00379 
00380     last_value = avg/sum;
00381 
00382     
00383 
00384    // if(sum == 0) last_value = 0;
00385 
00386  
00387 
00388     return last_value;
00389 
00390 }
00391 
00392  
00393 
00394 int TRSensors::sqrt(int m, int k){
00395 
00396    int temp = m;
00397 
00398    if(k == 0) m = 1;
00399 
00400    else{
00401 
00402    for(int i=0; i<(k-1); i++){
00403 
00404      m = temp *m;
00405 
00406     }
00407 
00408     }
00409 
00410    return  m;
00411 
00412 }