Part of the OU_Davis_Old_Robot Library
QTR_8A.cpp
- Committer:
- DrewSchaef
- Date:
- 2017-11-01
- Revision:
- 0:c4f495ae2ec6
File content as of revision 0:c4f495ae2ec6:
//ported from arduino with some changes //by Austin Saunders #include "mbed.h" #include "QTR_8A.h" #include "MCP3008.h" //6 sensors /* QTR_8A::QTR_8A(PinName L3, PinName L2, PinName L1, PinName R1, PinName R2, PinName R3, PinName E) : _L3(L3), _L2(L2), _L1(L1), _R1(R1), _R2(R2), _R3(R3), _E(E) { calibratedMinimum=0; calibratedMaximum=0; } */ MCP3008 Ain(p11,p12,p13,p14); QTR_8A::QTR_8A(PinName E) : _E(E) { calibratedMinimum=0; calibratedMaximum=0; emittersOff(); } QTR_8A::~QTR_8A() { } void QTR_8A::emittersOn() { _E = 1; wait(0.0004);//wait 300us so that the analogin doesn't read faster than the speed of light!! } void QTR_8A::emittersOff() { _E = 0; wait(0.0004);//wait 300us so that the analogin doesn't read faster than the speed of light!! } void QTR_8A::read(unsigned int *sensor_values) { int numSamplesPerSensor = 4; //reset the values for(int i=0; i<8; i++) { sensor_values[i] = 0; } emittersOn(); for (int j = 0; j < numSamplesPerSensor; j++) { sensor_values[0]+=Ain.read(0); sensor_values[1]+=Ain.read(1); sensor_values[2]+=Ain.read(2); sensor_values[3]+=Ain.read(3); sensor_values[4]+=Ain.read(4); sensor_values[5]+=Ain.read(5); sensor_values[6]+=Ain.read(6); sensor_values[7]+=Ain.read(7); } emittersOff(); // get the rounded average of the readings for each sensor for (int i = 0; i < 8; i++) { sensor_values[i] = (sensor_values[i]+(numSamplesPerSensor>>1))/numSamplesPerSensor; } } void QTR_8A::calibrate() { calibrateon(&calibratedMinimum,&calibratedMaximum); } void QTR_8A::calibrateon(unsigned int**calibratedMinimum,unsigned int**calibratedMaximum){ unsigned int sensor_values[8]; unsigned int max_sensor_values[8]; unsigned int min_sensor_values[8]; // Allocate the arrays if necessary. if(*calibratedMaximum == 0) { *calibratedMaximum = (unsigned int*)malloc(sizeof(unsigned int)*8); // If the malloc failed, don't continue. if(*calibratedMaximum == 0) { return; } // Initialize the max and min calibrated values to values that // will cause the first reading to update them. for(int i=0; i<8; i++) { (*calibratedMaximum)[i] = 0; } } if(*calibratedMinimum == 0) { *calibratedMinimum = (unsigned int*)malloc(sizeof(unsigned int)*8); // If the malloc failed, don't continue. if(*calibratedMinimum == 0) { return; } for(int i=0; i<8; i++) { (*calibratedMinimum)[i] = 1023; } } for(int j=0; j<10; j++) { read(sensor_values); for(int i=0; i<8; 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(int i=0; i<8; i++) { if(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]; } } void QTR_8A::resetCalibration() { for(int i=0; i<8; i++) { calibratedMinimum[i] = 1023; calibratedMaximum[i] = 0; } } void QTR_8A::readCalibrated(unsigned int *sensor_values) { if(!calibratedMinimum || !calibratedMaximum) { return; } read(sensor_values); for(int i=0; i<8; i++) { unsigned int calmin,calmax; unsigned int denominator; calmax = calibratedMaximum[i]; calmin = calibratedMinimum[i]; denominator = calmax - calmin; signed int x = 0; if(denominator != 0) { x = (((signed long)sensor_values[i]) - calmin)* 1000 / denominator; } if(x < 0) { x = 0; } else if(x > 1000) { x = 1000; } sensor_values[i] = x; } } int QTR_8A::readLine(unsigned int *sensor_values) { unsigned int on_line = 0; unsigned long avg; // this is for the weighted total, which is long // 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. readCalibrated(sensor_values); avg = 0; sum = 0; for(int i=0; i<8; i++) { int value = sensor_values[i]; // keep track of whether we see the line at all if(value > 350) { on_line = 1; } // only average in values that are above a noise threshold if(value > 20) { avg += (long)(value) * (i * 1000); sum += value; } } if(!on_line) { // If it last read to the left of center, return 0. if(last_value < 3500) { return 0; } // If it last read to the right of center, return the max. else { return 7000; } } last_value = avg/sum; return last_value; }