ss
Dependencies: WS2812 PixelArray Adafruit_GFX
IRreflection.cpp
- Committer:
- eunsong
- Date:
- 2019-06-15
- Revision:
- 0:27e31cadeb36
File content as of revision 0:27e31cadeb36:
#include "IRreflection.h" // Base class data member initialization (called by derived class init()) TRSensors::TRSensors(PinName MOSI,PinName MISO, PinName CLK, PinName CS) : _spi(MOSI,MISO,CLK), _spi_cs(CS) { _spi.format(16,0); _spi.frequency(2000000); _spi_cs = 1; _numSensors = NUMSENSORS; } void TRSensors::AnalogRead(int *sensor_values) { int i,j,ch; int values[] = {0,0,0,0,0,0}; for(j = 0;j < _numSensors + 1;j++) { ch =j; _spi_cs = 0; wait_us(2); values[j] = _spi.write(ch<<12); _spi_cs = 1; wait_us(21); values[j] = (values[j]>>6); } for(i = 0;i < _numSensors;i++) { sensor_values[i] = values[i+1]; } } // Reads the sensors 10 times and uses the results for // calibration. The sensor values are not returned; instead, the // maximum and minimum values found over time are stored internally // and used for the readCalibrated() method. void TRSensors::calibrate_init(int *calibratedMin, int *calibratedMax) { for(int i=0;i<_numSensors;i++) { calibratedMin[i] = 1023; calibratedMax[i] = 0; } } void TRSensors::calibrate(int *sensor_values, int *calibratedMin, int *calibratedMax) { int i=0; int j=0; for(j=0;j<10;j++) { AnalogRead(sensor_values); 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]; // set the min we found THIS time if(j == 0 || min_sensor_values[i] > sensor_values[i]) min_sensor_values[i] = sensor_values[i]; } } // record the min and max calibration values for(i=0;i<_numSensors;i++) { if(min_sensor_values[i] > calibratedMax[i]) calibratedMax[i] = min_sensor_values[i]; if(max_sensor_values[i] < calibratedMin[i]) calibratedMin[i] = max_sensor_values[i]; } } // Returns values calibrated to a value between 0 and 1000, where // 0 corresponds to the minimum value read by calibrate() and 1000 // corresponds to the maximum value. Calibration values are // stored separately for each sensor, so that differences in the // sensors are accounted for automatically. void TRSensors::readCalibrated(int *sensor_values, int *calibratedMin, int *calibratedMax) { int i; // read the needed values AnalogRead(sensor_values); for(i=0;i<_numSensors;i++) { int denominator; denominator = calibratedMax[i] - calibratedMin[i]; int x = 0; if(denominator != 0){ if(((signed int)sensor_values[i] - (signed int)calibratedMin[i])<0){ x = 0;} else { x = ((sensor_values[i] - calibratedMin[i])*1000/denominator); } } if( x > 1000) x = 1000; sensor_values[i] = x; } } // Operates the same as read calibrated, but also returns an // estimated position of the robot with respect to a line. The // estimate is made using a weighted average of the sensor indices // multiplied by 1000, so that a return value of 0 indicates that // the line is directly below sensor 0, a return value of 1000 // indicates that the line is directly below sensor 1, 2000 // indicates that it's below sensor 2000, etc. Intermediate // values indicate that the line is between two sensors. The // formula is: // // 0*value0 + 1000*value1 + 2000*value2 + ... // -------------------------------------------- // value0 + value1 + value2 + ... // // By default, this function assumes a dark line (high values) // surrounded by white (low values). If your line is light on // black, set the optional second argument white_line to true. In // this case, each sensor value will be replaced by (1000-value) // before the averaging. int TRSensors::readLine(int *sensor_values , int *calibratedMin, int *calibratedMax, int *online, char white_line) { char i, on_line = 0; long int avg; // this is for the weighted total, which is long // before division int sum; // this is for the denominator which is <= 64000 static int last_value=0; // assume initially that the line is left. readCalibrated(sensor_values, calibratedMin, calibratedMax); online[0] = 0; avg = 0; sum = 0; for(i=0;i<_numSensors;i++) { long int value = sensor_values[i]; //if(!white_line) value = 1000-value; if(white_line){ value = 1000-value; //sensor_values[i] = (1000 - sensor_values[i])/100; } //sensor_values[i] = value; // keep track of whether we see the line at all if(value > 300) { online[0] = 1; } // only average in values that are above a noise threshold if(value > 30) { avg += value*(1000*i);//(value/100)*(1000*i) // * sqrt(100,i); sum += value; //(value/100); } } /* if(!on_line) { // If it last read to the left of center, return 0. if(last_value < (_numSensors-1)*1000/2) return 0; // If it last read to the right of center, return the max. else return (_numSensors-1)*1000; } */ last_value = avg/sum; // if(sum == 0) last_value = 0; return last_value; } int TRSensors::sqrt(int m, int k){ int temp = m; if(k == 0) m = 1; else{ for(int i=0; i<(k-1); i++){ m = temp *m; } } return m; }