Andrew Schaeffer / QTR_8A

Part of the OU_Davis_Old_Robot Library

QTR_8A.cpp@0:c4f495ae2ec6, 2017-11-01 (annotated)

Committer:
DrewSchaef
Date:
Wed Nov 01 15:55:29 2017 +0000
Revision:
0:c4f495ae2ec6
Committed to allow full program to be published

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DrewSchaef 0:c4f495ae2ec6 1 //ported from arduino with some changes
DrewSchaef 0:c4f495ae2ec6 2 //by Austin Saunders
DrewSchaef 0:c4f495ae2ec6 3
DrewSchaef 0:c4f495ae2ec6 4 #include "mbed.h"
DrewSchaef 0:c4f495ae2ec6 5 #include "QTR_8A.h"
DrewSchaef 0:c4f495ae2ec6 6 #include "MCP3008.h"
DrewSchaef 0:c4f495ae2ec6 7
DrewSchaef 0:c4f495ae2ec6 8 //6 sensors
DrewSchaef 0:c4f495ae2ec6 9
DrewSchaef 0:c4f495ae2ec6 10 /*
DrewSchaef 0:c4f495ae2ec6 11 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)
DrewSchaef 0:c4f495ae2ec6 12 {
DrewSchaef 0:c4f495ae2ec6 13 calibratedMinimum=0;
DrewSchaef 0:c4f495ae2ec6 14 calibratedMaximum=0;
DrewSchaef 0:c4f495ae2ec6 15 }
DrewSchaef 0:c4f495ae2ec6 16 */
DrewSchaef 0:c4f495ae2ec6 17 MCP3008 Ain(p11,p12,p13,p14);
DrewSchaef 0:c4f495ae2ec6 18
DrewSchaef 0:c4f495ae2ec6 19 QTR_8A::QTR_8A(PinName E) : _E(E)
DrewSchaef 0:c4f495ae2ec6 20 {
DrewSchaef 0:c4f495ae2ec6 21 calibratedMinimum=0;
DrewSchaef 0:c4f495ae2ec6 22 calibratedMaximum=0;
DrewSchaef 0:c4f495ae2ec6 23 emittersOff();
DrewSchaef 0:c4f495ae2ec6 24 }
DrewSchaef 0:c4f495ae2ec6 25
DrewSchaef 0:c4f495ae2ec6 26
DrewSchaef 0:c4f495ae2ec6 27
DrewSchaef 0:c4f495ae2ec6 28 QTR_8A::~QTR_8A()
DrewSchaef 0:c4f495ae2ec6 29 {
DrewSchaef 0:c4f495ae2ec6 30
DrewSchaef 0:c4f495ae2ec6 31 }
DrewSchaef 0:c4f495ae2ec6 32
DrewSchaef 0:c4f495ae2ec6 33
DrewSchaef 0:c4f495ae2ec6 34 void QTR_8A::emittersOn()
DrewSchaef 0:c4f495ae2ec6 35 {
DrewSchaef 0:c4f495ae2ec6 36 _E = 1;
DrewSchaef 0:c4f495ae2ec6 37 wait(0.0004);//wait 300us so that the analogin doesn't read faster than the speed of light!!
DrewSchaef 0:c4f495ae2ec6 38 }
DrewSchaef 0:c4f495ae2ec6 39
DrewSchaef 0:c4f495ae2ec6 40 void QTR_8A::emittersOff()
DrewSchaef 0:c4f495ae2ec6 41 {
DrewSchaef 0:c4f495ae2ec6 42 _E = 0;
DrewSchaef 0:c4f495ae2ec6 43 wait(0.0004);//wait 300us so that the analogin doesn't read faster than the speed of light!!
DrewSchaef 0:c4f495ae2ec6 44 }
DrewSchaef 0:c4f495ae2ec6 45
DrewSchaef 0:c4f495ae2ec6 46 void QTR_8A::read(unsigned int *sensor_values)
DrewSchaef 0:c4f495ae2ec6 47 {
DrewSchaef 0:c4f495ae2ec6 48 int numSamplesPerSensor = 4;
DrewSchaef 0:c4f495ae2ec6 49 //reset the values
DrewSchaef 0:c4f495ae2ec6 50 for(int i=0; i<8; i++) {
DrewSchaef 0:c4f495ae2ec6 51 sensor_values[i] = 0;
DrewSchaef 0:c4f495ae2ec6 52 }
DrewSchaef 0:c4f495ae2ec6 53 emittersOn();
DrewSchaef 0:c4f495ae2ec6 54 for (int j = 0; j < numSamplesPerSensor; j++) {
DrewSchaef 0:c4f495ae2ec6 55 sensor_values[0]+=Ain.read(0);
DrewSchaef 0:c4f495ae2ec6 56 sensor_values[1]+=Ain.read(1);
DrewSchaef 0:c4f495ae2ec6 57 sensor_values[2]+=Ain.read(2);
DrewSchaef 0:c4f495ae2ec6 58 sensor_values[3]+=Ain.read(3);
DrewSchaef 0:c4f495ae2ec6 59 sensor_values[4]+=Ain.read(4);
DrewSchaef 0:c4f495ae2ec6 60 sensor_values[5]+=Ain.read(5);
DrewSchaef 0:c4f495ae2ec6 61 sensor_values[6]+=Ain.read(6);
DrewSchaef 0:c4f495ae2ec6 62 sensor_values[7]+=Ain.read(7);
DrewSchaef 0:c4f495ae2ec6 63 }
DrewSchaef 0:c4f495ae2ec6 64 emittersOff();
DrewSchaef 0:c4f495ae2ec6 65 // get the rounded average of the readings for each sensor
DrewSchaef 0:c4f495ae2ec6 66 for (int i = 0; i < 8; i++) {
DrewSchaef 0:c4f495ae2ec6 67 sensor_values[i] = (sensor_values[i]+(numSamplesPerSensor>>1))/numSamplesPerSensor;
DrewSchaef 0:c4f495ae2ec6 68 }
DrewSchaef 0:c4f495ae2ec6 69
DrewSchaef 0:c4f495ae2ec6 70 }
DrewSchaef 0:c4f495ae2ec6 71
DrewSchaef 0:c4f495ae2ec6 72 void QTR_8A::calibrate()
DrewSchaef 0:c4f495ae2ec6 73 {
DrewSchaef 0:c4f495ae2ec6 74 calibrateon(&calibratedMinimum,&calibratedMaximum);
DrewSchaef 0:c4f495ae2ec6 75 }
DrewSchaef 0:c4f495ae2ec6 76
DrewSchaef 0:c4f495ae2ec6 77 void QTR_8A::calibrateon(unsigned int**calibratedMinimum,unsigned int**calibratedMaximum){
DrewSchaef 0:c4f495ae2ec6 78
DrewSchaef 0:c4f495ae2ec6 79 unsigned int sensor_values[8];
DrewSchaef 0:c4f495ae2ec6 80 unsigned int max_sensor_values[8];
DrewSchaef 0:c4f495ae2ec6 81 unsigned int min_sensor_values[8];
DrewSchaef 0:c4f495ae2ec6 82
DrewSchaef 0:c4f495ae2ec6 83 // Allocate the arrays if necessary.
DrewSchaef 0:c4f495ae2ec6 84 if(*calibratedMaximum == 0) {
DrewSchaef 0:c4f495ae2ec6 85 *calibratedMaximum = (unsigned int*)malloc(sizeof(unsigned int)*8);
DrewSchaef 0:c4f495ae2ec6 86
DrewSchaef 0:c4f495ae2ec6 87 // If the malloc failed, don't continue.
DrewSchaef 0:c4f495ae2ec6 88 if(*calibratedMaximum == 0) {
DrewSchaef 0:c4f495ae2ec6 89 return;
DrewSchaef 0:c4f495ae2ec6 90 }
DrewSchaef 0:c4f495ae2ec6 91 // Initialize the max and min calibrated values to values that
DrewSchaef 0:c4f495ae2ec6 92 // will cause the first reading to update them.
DrewSchaef 0:c4f495ae2ec6 93
DrewSchaef 0:c4f495ae2ec6 94 for(int i=0; i<8; i++) {
DrewSchaef 0:c4f495ae2ec6 95 (*calibratedMaximum)[i] = 0;
DrewSchaef 0:c4f495ae2ec6 96 }
DrewSchaef 0:c4f495ae2ec6 97 }
DrewSchaef 0:c4f495ae2ec6 98 if(*calibratedMinimum == 0) {
DrewSchaef 0:c4f495ae2ec6 99 *calibratedMinimum = (unsigned int*)malloc(sizeof(unsigned int)*8);
DrewSchaef 0:c4f495ae2ec6 100
DrewSchaef 0:c4f495ae2ec6 101 // If the malloc failed, don't continue.
DrewSchaef 0:c4f495ae2ec6 102 if(*calibratedMinimum == 0) {
DrewSchaef 0:c4f495ae2ec6 103 return;
DrewSchaef 0:c4f495ae2ec6 104 }
DrewSchaef 0:c4f495ae2ec6 105
DrewSchaef 0:c4f495ae2ec6 106 for(int i=0; i<8; i++) {
DrewSchaef 0:c4f495ae2ec6 107 (*calibratedMinimum)[i] = 1023;
DrewSchaef 0:c4f495ae2ec6 108 }
DrewSchaef 0:c4f495ae2ec6 109 }
DrewSchaef 0:c4f495ae2ec6 110
DrewSchaef 0:c4f495ae2ec6 111 for(int j=0; j<10; j++) {
DrewSchaef 0:c4f495ae2ec6 112 read(sensor_values);
DrewSchaef 0:c4f495ae2ec6 113 for(int i=0; i<8; i++) {
DrewSchaef 0:c4f495ae2ec6 114 // set the max we found THIS time
DrewSchaef 0:c4f495ae2ec6 115 if(j == 0 || max_sensor_values[i] < sensor_values[i])
DrewSchaef 0:c4f495ae2ec6 116 max_sensor_values[i] = sensor_values[i];
DrewSchaef 0:c4f495ae2ec6 117
DrewSchaef 0:c4f495ae2ec6 118 // set the min we found THIS time
DrewSchaef 0:c4f495ae2ec6 119 if(j == 0 || min_sensor_values[i] > sensor_values[i])
DrewSchaef 0:c4f495ae2ec6 120 min_sensor_values[i] = sensor_values[i];
DrewSchaef 0:c4f495ae2ec6 121 }
DrewSchaef 0:c4f495ae2ec6 122 }
DrewSchaef 0:c4f495ae2ec6 123
DrewSchaef 0:c4f495ae2ec6 124 // record the min and max calibration values
DrewSchaef 0:c4f495ae2ec6 125 for(int i=0; i<8; i++) {
DrewSchaef 0:c4f495ae2ec6 126 if(min_sensor_values[i] > (*calibratedMaximum)[i])
DrewSchaef 0:c4f495ae2ec6 127 (*calibratedMaximum)[i] = min_sensor_values[i];
DrewSchaef 0:c4f495ae2ec6 128 if(max_sensor_values[i] < (*calibratedMinimum)[i])
DrewSchaef 0:c4f495ae2ec6 129 (*calibratedMinimum)[i] = max_sensor_values[i];
DrewSchaef 0:c4f495ae2ec6 130 }
DrewSchaef 0:c4f495ae2ec6 131 }
DrewSchaef 0:c4f495ae2ec6 132
DrewSchaef 0:c4f495ae2ec6 133
DrewSchaef 0:c4f495ae2ec6 134
DrewSchaef 0:c4f495ae2ec6 135 void QTR_8A::resetCalibration()
DrewSchaef 0:c4f495ae2ec6 136 {
DrewSchaef 0:c4f495ae2ec6 137 for(int i=0; i<8; i++) {
DrewSchaef 0:c4f495ae2ec6 138 calibratedMinimum[i] = 1023;
DrewSchaef 0:c4f495ae2ec6 139 calibratedMaximum[i] = 0;
DrewSchaef 0:c4f495ae2ec6 140 }
DrewSchaef 0:c4f495ae2ec6 141 }
DrewSchaef 0:c4f495ae2ec6 142
DrewSchaef 0:c4f495ae2ec6 143 void QTR_8A::readCalibrated(unsigned int *sensor_values)
DrewSchaef 0:c4f495ae2ec6 144 {
DrewSchaef 0:c4f495ae2ec6 145 if(!calibratedMinimum || !calibratedMaximum) {
DrewSchaef 0:c4f495ae2ec6 146 return;
DrewSchaef 0:c4f495ae2ec6 147 }
DrewSchaef 0:c4f495ae2ec6 148 read(sensor_values);
DrewSchaef 0:c4f495ae2ec6 149
DrewSchaef 0:c4f495ae2ec6 150 for(int i=0; i<8; i++) {
DrewSchaef 0:c4f495ae2ec6 151 unsigned int calmin,calmax;
DrewSchaef 0:c4f495ae2ec6 152 unsigned int denominator;
DrewSchaef 0:c4f495ae2ec6 153 calmax = calibratedMaximum[i];
DrewSchaef 0:c4f495ae2ec6 154 calmin = calibratedMinimum[i];
DrewSchaef 0:c4f495ae2ec6 155 denominator = calmax - calmin;
DrewSchaef 0:c4f495ae2ec6 156
DrewSchaef 0:c4f495ae2ec6 157 signed int x = 0;
DrewSchaef 0:c4f495ae2ec6 158 if(denominator != 0) {
DrewSchaef 0:c4f495ae2ec6 159 x = (((signed long)sensor_values[i]) - calmin)* 1000 / denominator;
DrewSchaef 0:c4f495ae2ec6 160 }
DrewSchaef 0:c4f495ae2ec6 161 if(x < 0) {
DrewSchaef 0:c4f495ae2ec6 162 x = 0;
DrewSchaef 0:c4f495ae2ec6 163 } else if(x > 1000) {
DrewSchaef 0:c4f495ae2ec6 164 x = 1000;
DrewSchaef 0:c4f495ae2ec6 165 }
DrewSchaef 0:c4f495ae2ec6 166 sensor_values[i] = x;
DrewSchaef 0:c4f495ae2ec6 167
DrewSchaef 0:c4f495ae2ec6 168 }
DrewSchaef 0:c4f495ae2ec6 169 }
DrewSchaef 0:c4f495ae2ec6 170
DrewSchaef 0:c4f495ae2ec6 171 int QTR_8A::readLine(unsigned int *sensor_values)
DrewSchaef 0:c4f495ae2ec6 172 {
DrewSchaef 0:c4f495ae2ec6 173 unsigned int on_line = 0;
DrewSchaef 0:c4f495ae2ec6 174 unsigned long avg; // this is for the weighted total, which is long
DrewSchaef 0:c4f495ae2ec6 175 // before division
DrewSchaef 0:c4f495ae2ec6 176 unsigned int sum; // this is for the denominator which is <= 64000
DrewSchaef 0:c4f495ae2ec6 177 static int last_value=0; // assume initially that the line is left.
DrewSchaef 0:c4f495ae2ec6 178
DrewSchaef 0:c4f495ae2ec6 179 readCalibrated(sensor_values);
DrewSchaef 0:c4f495ae2ec6 180
DrewSchaef 0:c4f495ae2ec6 181 avg = 0;
DrewSchaef 0:c4f495ae2ec6 182 sum = 0;
DrewSchaef 0:c4f495ae2ec6 183
DrewSchaef 0:c4f495ae2ec6 184 for(int i=0; i<8; i++) {
DrewSchaef 0:c4f495ae2ec6 185 int value = sensor_values[i];
DrewSchaef 0:c4f495ae2ec6 186
DrewSchaef 0:c4f495ae2ec6 187
DrewSchaef 0:c4f495ae2ec6 188 // keep track of whether we see the line at all
DrewSchaef 0:c4f495ae2ec6 189 if(value > 350) {
DrewSchaef 0:c4f495ae2ec6 190 on_line = 1;
DrewSchaef 0:c4f495ae2ec6 191 }
DrewSchaef 0:c4f495ae2ec6 192
DrewSchaef 0:c4f495ae2ec6 193 // only average in values that are above a noise threshold
DrewSchaef 0:c4f495ae2ec6 194 if(value > 20) {
DrewSchaef 0:c4f495ae2ec6 195 avg += (long)(value) * (i * 1000);
DrewSchaef 0:c4f495ae2ec6 196 sum += value;
DrewSchaef 0:c4f495ae2ec6 197 }
DrewSchaef 0:c4f495ae2ec6 198 }
DrewSchaef 0:c4f495ae2ec6 199
DrewSchaef 0:c4f495ae2ec6 200 if(!on_line) {
DrewSchaef 0:c4f495ae2ec6 201 // If it last read to the left of center, return 0.
DrewSchaef 0:c4f495ae2ec6 202 if(last_value < 3500) {
DrewSchaef 0:c4f495ae2ec6 203 return 0;
DrewSchaef 0:c4f495ae2ec6 204 }
DrewSchaef 0:c4f495ae2ec6 205
DrewSchaef 0:c4f495ae2ec6 206 // If it last read to the right of center, return the max.
DrewSchaef 0:c4f495ae2ec6 207 else {
DrewSchaef 0:c4f495ae2ec6 208 return 7000;
DrewSchaef 0:c4f495ae2ec6 209 }
DrewSchaef 0:c4f495ae2ec6 210
DrewSchaef 0:c4f495ae2ec6 211 }
DrewSchaef 0:c4f495ae2ec6 212
DrewSchaef 0:c4f495ae2ec6 213 last_value = avg/sum;
DrewSchaef 0:c4f495ae2ec6 214
DrewSchaef 0:c4f495ae2ec6 215 return last_value;
DrewSchaef 0:c4f495ae2ec6 216 }
DrewSchaef 0:c4f495ae2ec6 217
DrewSchaef 0:c4f495ae2ec6 218
DrewSchaef 0:c4f495ae2ec6 219
DrewSchaef 0:c4f495ae2ec6 220