Mike Panetta / PiSlingers

PiSlingers library for AHRC competitions

IRObjDetector.cpp@2:a9351d7f92b4, 2012-11-03 (annotated)

Committer:
mpanetta
Date:
Sat Nov 03 01:03:16 2012 +0000
Revision:
2:a9351d7f92b4
Parent:
1:695f4f4442d3 
Fixed some bugs in the IR code that allowed negative values through.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mpanetta 0:d4427d048a98 1 #include "IRObjDetector.h"
mpanetta 0:d4427d048a98 2 #include "tlc5916.h"
mpanetta 0:d4427d048a98 3
mpanetta 0:d4427d048a98 4 #define DWELL_TIME 25 // Time to wait between enabling IR transmitter LED and reading ADC result, in us.
mpanetta 0:d4427d048a98 5
mpanetta 0:d4427d048a98 6 void
mpanetta 0:d4427d048a98 7 IRObjDetector::scan(void)
mpanetta 0:d4427d048a98 8 {
mpanetta 0:d4427d048a98 9 int i;
mpanetta 0:d4427d048a98 10 uint32_t max = 0;
mpanetta 1:695f4f4442d3 11 int32_t tmp = 0;
mpanetta 0:d4427d048a98 12
mpanetta 0:d4427d048a98 13 //data[7] = ain.read_u16() >> 8; // Read the ambient level and store in last data slot.
mpanetta 1:695f4f4442d3 14 //data[7] = ain.read_u16(); // Read the ambient level and store in last data slot.
mpanetta 1:695f4f4442d3 15
mpanetta 1:695f4f4442d3 16 // Average 4 readings of ambient.
mpanetta 1:695f4f4442d3 17 tmp += ain.read_u16();
mpanetta 1:695f4f4442d3 18 tmp += ain.read_u16();
mpanetta 1:695f4f4442d3 19 tmp += ain.read_u16();
mpanetta 1:695f4f4442d3 20 tmp += ain.read_u16();
mpanetta 1:695f4f4442d3 21
mpanetta 1:695f4f4442d3 22 // Store ambient level in last data slot.
mpanetta 1:695f4f4442d3 23 data[7] = tmp / 4;
mpanetta 0:d4427d048a98 24
mpanetta 0:d4427d048a98 25 // Set the IR leds one at a time and read the reflection.
mpanetta 0:d4427d048a98 26 for (i = 0; i < 7; i++)
mpanetta 0:d4427d048a98 27 {
mpanetta 0:d4427d048a98 28 // Write the value to enable the LED
mpanetta 0:d4427d048a98 29 tlc5916.write_reg(1 << i);
mpanetta 0:d4427d048a98 30 // Enable the selected LED.
mpanetta 0:d4427d048a98 31 tlc5916.enable();
mpanetta 0:d4427d048a98 32 // Wait for output to stabilize
mpanetta 0:d4427d048a98 33 wait_us(DWELL_TIME);
mpanetta 0:d4427d048a98 34 // Read ADC value and subtract off ambient.
mpanetta 0:d4427d048a98 35 //tmp = (ain.read_u16() >> 8) - data[7];
mpanetta 1:695f4f4442d3 36 //tmp = ain.read_u16() - data[7];
mpanetta 1:695f4f4442d3 37 // Average 4 readings
mpanetta 1:695f4f4442d3 38 tmp = 0;
mpanetta 1:695f4f4442d3 39 tmp += ain.read_u16();
mpanetta 1:695f4f4442d3 40 tmp += ain.read_u16();
mpanetta 1:695f4f4442d3 41 tmp += ain.read_u16();
mpanetta 1:695f4f4442d3 42 tmp += ain.read_u16();
mpanetta 1:695f4f4442d3 43
mpanetta 1:695f4f4442d3 44 // Take reading and subtract off ambient.
mpanetta 2:a9351d7f92b4 45 tmp = (tmp / 4) - data[7];
mpanetta 1:695f4f4442d3 46
mpanetta 2:a9351d7f92b4 47 if (tmp < 0)
mpanetta 2:a9351d7f92b4 48 {
mpanetta 0:d4427d048a98 49 data[i] = 0;
mpanetta 2:a9351d7f92b4 50 tmp = 0;
mpanetta 2:a9351d7f92b4 51 }
mpanetta 0:d4427d048a98 52 else
mpanetta 0:d4427d048a98 53 data[i] = tmp >> 8;
mpanetta 1:695f4f4442d3 54
mpanetta 0:d4427d048a98 55 // Disable the selected LED.
mpanetta 0:d4427d048a98 56 tlc5916.disable();
mpanetta 0:d4427d048a98 57
mpanetta 0:d4427d048a98 58 if (tmp > max) max = tmp;
mpanetta 0:d4427d048a98 59 }
mpanetta 0:d4427d048a98 60
mpanetta 0:d4427d048a98 61 calc_centeroid();
mpanetta 0:d4427d048a98 62
mpanetta 0:d4427d048a98 63 brightness = max;
mpanetta 0:d4427d048a98 64
mpanetta 1:695f4f4442d3 65 #if 0
mpanetta 0:d4427d048a98 66 brightnessBins[currentBin++] = brightness;
mpanetta 0:d4427d048a98 67
mpanetta 0:d4427d048a98 68 // Calculate running average over BRIGHTNESS_BINS bins.
mpanetta 0:d4427d048a98 69 minBrightness = 0xFFFF;
mpanetta 0:d4427d048a98 70 avgBrightness = 0;
mpanetta 0:d4427d048a98 71 maxBrightness = 0;
mpanetta 0:d4427d048a98 72 for (int i = 0; i < BRIGHTNESS_BINS; i++)
mpanetta 0:d4427d048a98 73 {
mpanetta 0:d4427d048a98 74 tmp = brightnessBins[i];
mpanetta 0:d4427d048a98 75 if (minBrightness > tmp) minBrightness = tmp;
mpanetta 0:d4427d048a98 76 avgBrightness += tmp;
mpanetta 0:d4427d048a98 77 if (maxBrightness < tmp) maxBrightness = tmp;
mpanetta 0:d4427d048a98 78 }
mpanetta 0:d4427d048a98 79 avgBrightness = avgBrightness / BRIGHTNESS_BINS;
mpanetta 0:d4427d048a98 80
mpanetta 0:d4427d048a98 81 if (currentBin >= BRIGHTNESS_BINS)
mpanetta 0:d4427d048a98 82 currentBin = 0;
mpanetta 1:695f4f4442d3 83 #else
mpanetta 1:695f4f4442d3 84 avgBrightness = brightness;
mpanetta 1:695f4f4442d3 85 #endif
mpanetta 0:d4427d048a98 86 }
mpanetta 0:d4427d048a98 87
mpanetta 0:d4427d048a98 88 float
mpanetta 0:d4427d048a98 89 IRObjDetector::get_weighted_avg_brightness(void)
mpanetta 0:d4427d048a98 90 {
mpanetta 0:d4427d048a98 91 if (debug != NULL)
mpanetta 0:d4427d048a98 92 {
mpanetta 0:d4427d048a98 93 debug->printf("IRObjDetector: Calculating weighted average.\r\n");
mpanetta 0:d4427d048a98 94 debug->printf("IRObjDetector: minBrightness = 0x%4.4x\r\n", minBrightness);
mpanetta 0:d4427d048a98 95 debug->printf("IRObjDetector: avgBrightness = 0x%4.4x\r\n", avgBrightness);
mpanetta 0:d4427d048a98 96 debug->printf("IRObjDetector: minBrightness = 0x%4.4x\r\n", maxBrightness);
mpanetta 0:d4427d048a98 97 }
mpanetta 0:d4427d048a98 98 return (float)(avgBrightness) / (float)0xffff;
mpanetta 0:d4427d048a98 99 }
mpanetta 0:d4427d048a98 100
mpanetta 0:d4427d048a98 101 void
mpanetta 0:d4427d048a98 102 IRObjDetector::calc_centeroid(void)
mpanetta 0:d4427d048a98 103 {
mpanetta 0:d4427d048a98 104 int i;
mpanetta 0:d4427d048a98 105
mpanetta 0:d4427d048a98 106 uint32_t low;
mpanetta 0:d4427d048a98 107 float sumA, sumB;
mpanetta 0:d4427d048a98 108
mpanetta 0:d4427d048a98 109 low = 0xffff;
mpanetta 0:d4427d048a98 110 for (i = 0; i < 7; i++)
mpanetta 0:d4427d048a98 111 {
mpanetta 0:d4427d048a98 112 if (data[i] < low) low = data[i];
mpanetta 0:d4427d048a98 113 }
mpanetta 0:d4427d048a98 114
mpanetta 0:d4427d048a98 115 sumA = sumB = 0;
mpanetta 0:d4427d048a98 116
mpanetta 0:d4427d048a98 117 // Generate sums for centeroid data
mpanetta 0:d4427d048a98 118 for (i = 0; i < 7; i++)
mpanetta 0:d4427d048a98 119 {
mpanetta 0:d4427d048a98 120 sumB += (data[i] - low) * (i + 1);
mpanetta 0:d4427d048a98 121 sumA += data[i] - low;
mpanetta 0:d4427d048a98 122 }
mpanetta 0:d4427d048a98 123
mpanetta 0:d4427d048a98 124 // Get 'center' bin by taking the highest bin count and dividing by number of bins.
mpanetta 0:d4427d048a98 125 float offset = 28/7;
mpanetta 0:d4427d048a98 126
mpanetta 0:d4427d048a98 127 centeroid = sumB / sumA; // Generate centroid (approximate location of object) which is a number between 1 an nBins (7 in our case)
mpanetta 0:d4427d048a98 128 centeroid -= offset; // Offset the centeroid around center bin, now the centeroid is a number between 1-offset and offset-1 (-3 to 3 in our case)
mpanetta 0:d4427d048a98 129 }
mpanetta 0:d4427d048a98 130
mpanetta 0:d4427d048a98 131 float
mpanetta 0:d4427d048a98 132 IRObjDetector::get_centeroid(void)
mpanetta 0:d4427d048a98 133 {
mpanetta 0:d4427d048a98 134 return centeroid;
mpanetta 0:d4427d048a98 135 }
mpanetta 0:d4427d048a98 136
mpanetta 0:d4427d048a98 137 uint16_t
mpanetta 0:d4427d048a98 138 IRObjDetector::get_raw_brightness(void)
mpanetta 0:d4427d048a98 139 {
mpanetta 0:d4427d048a98 140 return brightness;
mpanetta 0:d4427d048a98 141 }
mpanetta 0:d4427d048a98 142
mpanetta 0:d4427d048a98 143 uint16_t
mpanetta 0:d4427d048a98 144 IRObjDetector::get_min_brightness(void)
mpanetta 0:d4427d048a98 145 {
mpanetta 0:d4427d048a98 146 return minBrightness;
mpanetta 0:d4427d048a98 147 }
mpanetta 0:d4427d048a98 148
mpanetta 0:d4427d048a98 149 uint16_t
mpanetta 0:d4427d048a98 150 IRObjDetector::get_avg_brightness(void)
mpanetta 0:d4427d048a98 151 {
mpanetta 0:d4427d048a98 152 return avgBrightness;
mpanetta 0:d4427d048a98 153 }
mpanetta 0:d4427d048a98 154
mpanetta 0:d4427d048a98 155 uint16_t
mpanetta 0:d4427d048a98 156 IRObjDetector::get_max_brightness(void)
mpanetta 0:d4427d048a98 157 {
mpanetta 0:d4427d048a98 158 return maxBrightness;
mpanetta 0:d4427d048a98 159 }