Team 9 / LineScan

Moved to Team 9.

Fork of LineScan by Nicholas Gan

LineScan.cpp@22:0136520fe249, 2015-04-18 (annotated)

Committer:
dnleek
Date:
Sat Apr 18 09:00:19 2015 +0000
Revision:
22:0136520fe249
Parent:
21:68db1d0a5534
Child:
23:a1a671d3c9e5
changes to agc for highest brightness instead of total

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ng3600 0:2d546112b0b8 1 /*
ng3600 0:2d546112b0b8 2 AnalogIn cam1(CAM1_IN);
ng3600 0:2d546112b0b8 3 DigitalOut camSi(CAM_SI);
ikrase 1:f10ec868cd71 4 DigitalOut camClk(CAM_CLK); // Definining camera pins. These are actually defined in Main...
ng3600 0:2d546112b0b8 5 */
ng3600 0:2d546112b0b8 6 #include "LineScan.h"
ng3600 0:2d546112b0b8 7
ng3600 13:c17cf029d899 8 #define THRESH 5000
ng3600 8:b9ec2f3e12b6 9
ng3600 13:c17cf029d899 10 #define MEAN_REF 20000 //ideal mean we should see
dnleek 22:0136520fe249 11 #define BRIGHTEST_REF 40000
ng3600 9:5226617d2019 12 #define CAM_CTRL_GAIN 0.0005 //should be small
ng3600 3:f31986cb68fd 13
ng3600 0:2d546112b0b8 14 uint16_t read1Bit(AnalogIn cam, DigitalOut *camClk){
ng3600 0:2d546112b0b8 15 uint16_t pixel;
ng3600 0:2d546112b0b8 16
ng3600 0:2d546112b0b8 17 //read pixel n
ng3600 0:2d546112b0b8 18 pixel = cam.read_u16();
ng3600 0:2d546112b0b8 19
ng3600 0:2d546112b0b8 20 //clock pulse for next pixel n + 1
ng3600 0:2d546112b0b8 21 *camClk = 1;
ikrase 1:f10ec868cd71 22 *camClk = 0; // Apparently there is no need for any delay. It may be desireable to flatten this, or perhaps the compiler does it.
ng3600 0:2d546112b0b8 23
ng3600 0:2d546112b0b8 24 return pixel; //return result as an uint16_t (16 bit integer)
ng3600 0:2d546112b0b8 25 }
ng3600 0:2d546112b0b8 26
ng3600 0:2d546112b0b8 27 void startRead(DigitalOut *camSi, DigitalOut *camClk){
ng3600 0:2d546112b0b8 28 //pulse first clock cycle and start integral pin
ng3600 0:2d546112b0b8 29 *camSi = 1;
ng3600 0:2d546112b0b8 30 *camClk = 1;
ng3600 0:2d546112b0b8 31 *camSi = 0;
ng3600 0:2d546112b0b8 32 *camClk = 0;
ng3600 0:2d546112b0b8 33 }
ng3600 0:2d546112b0b8 34
ng3600 8:b9ec2f3e12b6 35 float processFn(uint16_t *array, int arraySz, int* exposure){
ng3600 13:c17cf029d899 36 const static int frameLen = NUM_PIX - 2 * SKIP;
ng3600 8:b9ec2f3e12b6 37 int avg[frameLen];
ng3600 8:b9ec2f3e12b6 38 int diff[frameLen];
ng3600 2:6a87b2348245 39 int highest = 0;
ng3600 2:6a87b2348245 40 int lowest = 0;
ng3600 13:c17cf029d899 41 int total = 0;
ng3600 8:b9ec2f3e12b6 42 int h_idx = frameLen/2;
ng3600 8:b9ec2f3e12b6 43 int l_idx = frameLen/2;
dnleek 21:68db1d0a5534 44 int left_bound = SKIP;
dnleek 21:68db1d0a5534 45 int right_bound = NUM_PIX - SKIP;
dnleek 22:0136520fe249 46 uint16_t brightest = 0;
ng3600 0:2d546112b0b8 47
ng3600 3:f31986cb68fd 48 //for AGC
ng3600 3:f31986cb68fd 49 float exposureChange;
ng3600 3:f31986cb68fd 50
ng3600 3:f31986cb68fd 51 //Just finds line center, does not track crossings (needs this feature)
ng3600 13:c17cf029d899 52 if(array){
ng3600 8:b9ec2f3e12b6 53 avg[0] = array[SKIP - 1]/2 + array[SKIP]/2;
ng3600 2:6a87b2348245 54 diff[0] = 0;
dnleek 22:0136520fe249 55 brightest = avg[0];
ng3600 13:c17cf029d899 56 total += avg[0]; //AGC
ng3600 13:c17cf029d899 57
ng3600 8:b9ec2f3e12b6 58 for(int i = 1; i < frameLen; i++){
ng3600 8:b9ec2f3e12b6 59 avg[i] = array[i - 1 + SKIP]/2 + array[i + SKIP]/2; //smoothing
ng3600 3:f31986cb68fd 60 diff[i] = avg[i - 1] - avg[i]; //differential
ng3600 2:6a87b2348245 61
dnleek 22:0136520fe249 62 if (avg[i] > brightest) {
dnleek 22:0136520fe249 63 total = avg[i]; //AGC
dnleek 22:0136520fe249 64 brightest = avg[i];
dnleek 22:0136520fe249 65 }
ng3600 13:c17cf029d899 66
ng3600 2:6a87b2348245 67 if(diff[i] > highest){
ng3600 2:6a87b2348245 68 highest = diff[i];
ng3600 2:6a87b2348245 69 h_idx = i;
ng3600 2:6a87b2348245 70 }
ng3600 2:6a87b2348245 71 else if(diff[i] < lowest){
ng3600 2:6a87b2348245 72 lowest = diff[i];
ng3600 2:6a87b2348245 73 l_idx = i;
ng3600 2:6a87b2348245 74 }
ng3600 2:6a87b2348245 75 }
ng3600 13:c17cf029d899 76 //AGC, simple proportional controller
ng3600 13:c17cf029d899 77 total = total / frameLen;
dnleek 22:0136520fe249 78 exposureChange = ((float)(BRIGHTEST_REF - total)) * CAM_CTRL_GAIN;
ng3600 3:f31986cb68fd 79 *exposure += (int)exposureChange;
ng3600 11:5e66d0531053 80 if(*exposure < 1)
ng3600 11:5e66d0531053 81 *exposure = 1;
ng3600 9:5226617d2019 82 else if(*exposure > 30)
ng3600 9:5226617d2019 83 *exposure = 30;
ng3600 0:2d546112b0b8 84 }
ng3600 13:c17cf029d899 85 //valid if white line on black and not blinded
dnleek 21:68db1d0a5534 86 if (highest > THRESH && -1 * lowest > THRESH)
dnleek 21:68db1d0a5534 87 return ((float) h_idx + (float) l_idx) / (2*(float)frameLen);
dnleek 21:68db1d0a5534 88 else if(highest > THRESH && -1 * lowest < THRESH )
dnleek 22:0136520fe249 89 return ((float) h_idx + (float)left_bound - (float)(right_bound - h_idx)) / (2*(float)frameLen); //0.5 is center
dnleek 21:68db1d0a5534 90 else if (-1*lowest > THRESH && highest < THRESH )
dnleek 22:0136520fe249 91 return ((float) l_idx + (float)right_bound + (float)(l_idx - left_bound)) / (2*(float)frameLen);
ng3600 13:c17cf029d899 92 else
ng3600 13:c17cf029d899 93 return -1.0; //invalid read
ng3600 0:2d546112b0b8 94 }
ng3600 0:2d546112b0b8 95
ng3600 3:f31986cb68fd 96 //call after integration time is done, returns index of array line is expected to be at and new exposure time
ng3600 12:ce6d9f7dc76e 97 float getLinePos(AnalogIn cam, DigitalOut *camSi, DigitalOut *camClk, int *exposure, telemetry::NumericArray<uint16_t, 128> &tele_linescan){
ng3600 3:f31986cb68fd 98 uint16_t lineAry[NUM_PIX];
ng3600 8:b9ec2f3e12b6 99 float position;
ng3600 0:2d546112b0b8 100
ng3600 0:2d546112b0b8 101 //read
ng3600 0:2d546112b0b8 102 startRead(camSi, camClk);
ng3600 0:2d546112b0b8 103 for(int i = 0; i < NUM_PIX; i++){
ng3600 0:2d546112b0b8 104 lineAry[i] = read1Bit(cam, camClk);
ng3600 12:ce6d9f7dc76e 105 tele_linescan[i] = lineAry[i];
ng3600 0:2d546112b0b8 106 }
ng3600 0:2d546112b0b8 107
ng3600 0:2d546112b0b8 108 //process line scan data
ng3600 3:f31986cb68fd 109 position = processFn(lineAry, NUM_PIX, exposure);
ng3600 0:2d546112b0b8 110
ng3600 0:2d546112b0b8 111 return position;
ng3600 0:2d546112b0b8 112 }
ng3600 0:2d546112b0b8 113
ng3600 0:2d546112b0b8 114 /*
ng3600 0:2d546112b0b8 115 sample call (handler thread):
ng3600 0:2d546112b0b8 116
ng3600 0:2d546112b0b8 117 while(1){
ng3600 3:f31986cb68fd 118 linePos = getLinePos(cameraIn1, si, clk, &exposureTime); //volatile linePos, replace with steering angle and read 2 cameras?
ng3600 3:f31986cb68fd 119 Thread::wait(exposureTime); //sleep for 14 us
ng3600 0:2d546112b0b8 120 }
ng3600 2:6a87b2348245 121 */