Part of the OU_Davis_Old_Robot Library
Revision 0:c4f495ae2ec6, committed 2017-11-01
- Comitter:
- DrewSchaef
- Date:
- Wed Nov 01 15:55:29 2017 +0000
- Commit message:
- Committed to allow full program to be published
Changed in this revision
QTR_8A.cpp | Show annotated file Show diff for this revision Revisions of this file |
QTR_8A.h | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QTR_8A.cpp Wed Nov 01 15:55:29 2017 +0000 @@ -0,0 +1,220 @@ +//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; +} + + + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QTR_8A.h Wed Nov 01 15:55:29 2017 +0000 @@ -0,0 +1,36 @@ +#include "mbed.h" +#include "MCP3008.h" +class QTR_8A +{ +public: + + //QTR_8A(PinName L3, PinName L2, PinName L1, PinName R1, PinName R2, PinName R3, PinName E); + QTR_8A(PinName E); + ~QTR_8A(); + void emittersOn(); + void emittersOff(); + void read(unsigned int *sensor_values); + void calibrate(); + void calibrateon(unsigned int **calibratedMinimum, unsigned int **calibratedMaximum); + void resetCalibration(); + void readCalibrated(unsigned int *sensor_values); + int readLine(unsigned int *sensor_values); + + unsigned int *calibratedMinimum; + unsigned int *calibratedMaximum; + + +protected: + +private: + /** + AnalogIn _L3; + AnalogIn _L2; + AnalogIn _L1; + AnalogIn _R1; + AnalogIn _R2; + AnalogIn _R3; + */ + DigitalOut _E; + +}; \ No newline at end of file