Team 9 / LineScan

Moved to Team 9.

Fork of LineScan by Nicholas Gan

LineScan.cpp@13:c17cf029d899, 2015-04-10 (annotated)

Committer:
ng3600
Date:
Fri Apr 10 00:38:31 2015 +0000
Revision:
13:c17cf029d899
Parent:
12:ce6d9f7dc76e
Child:
14:928254a609cb
Child:
21:68db1d0a5534
Overhaul of AGC. Now can tell difference between overexposed and underexposed.

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
ng3600 9:5226617d2019 11 #define CAM_CTRL_GAIN 0.0005 //should be small
ng3600 3:f31986cb68fd 12
ng3600 0:2d546112b0b8 13 uint16_t read1Bit(AnalogIn cam, DigitalOut *camClk){
ng3600 0:2d546112b0b8 14 uint16_t pixel;
ng3600 0:2d546112b0b8 15
ng3600 0:2d546112b0b8 16 //read pixel n
ng3600 0:2d546112b0b8 17 pixel = cam.read_u16();
ng3600 0:2d546112b0b8 18
ng3600 0:2d546112b0b8 19 //clock pulse for next pixel n + 1
ng3600 0:2d546112b0b8 20 *camClk = 1;
ikrase 1:f10ec868cd71 21 *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 22
ng3600 0:2d546112b0b8 23 return pixel; //return result as an uint16_t (16 bit integer)
ng3600 0:2d546112b0b8 24 }
ng3600 0:2d546112b0b8 25
ng3600 0:2d546112b0b8 26 void startRead(DigitalOut *camSi, DigitalOut *camClk){
ng3600 0:2d546112b0b8 27 //pulse first clock cycle and start integral pin
ng3600 0:2d546112b0b8 28 *camSi = 1;
ng3600 0:2d546112b0b8 29 *camClk = 1;
ng3600 0:2d546112b0b8 30 *camSi = 0;
ng3600 0:2d546112b0b8 31 *camClk = 0;
ng3600 0:2d546112b0b8 32 }
ng3600 0:2d546112b0b8 33
ng3600 8:b9ec2f3e12b6 34 float processFn(uint16_t *array, int arraySz, int* exposure){
ng3600 13:c17cf029d899 35 const static int frameLen = NUM_PIX - 2 * SKIP;
ng3600 8:b9ec2f3e12b6 36 int avg[frameLen];
ng3600 8:b9ec2f3e12b6 37 int diff[frameLen];
ng3600 2:6a87b2348245 38 int highest = 0;
ng3600 2:6a87b2348245 39 int lowest = 0;
ng3600 13:c17cf029d899 40 int total = 0;
ng3600 8:b9ec2f3e12b6 41 int h_idx = frameLen/2;
ng3600 8:b9ec2f3e12b6 42 int l_idx = frameLen/2;
ng3600 0:2d546112b0b8 43
ng3600 3:f31986cb68fd 44 //for AGC
ng3600 3:f31986cb68fd 45 float exposureChange;
ng3600 3:f31986cb68fd 46
ng3600 3:f31986cb68fd 47 //Just finds line center, does not track crossings (needs this feature)
ng3600 13:c17cf029d899 48 if(array){
ng3600 8:b9ec2f3e12b6 49 avg[0] = array[SKIP - 1]/2 + array[SKIP]/2;
ng3600 2:6a87b2348245 50 diff[0] = 0;
ng3600 2:6a87b2348245 51
ng3600 13:c17cf029d899 52 total += avg[0]; //AGC
ng3600 13:c17cf029d899 53
ng3600 8:b9ec2f3e12b6 54 for(int i = 1; i < frameLen; i++){
ng3600 8:b9ec2f3e12b6 55 avg[i] = array[i - 1 + SKIP]/2 + array[i + SKIP]/2; //smoothing
ng3600 3:f31986cb68fd 56 diff[i] = avg[i - 1] - avg[i]; //differential
ng3600 2:6a87b2348245 57
ng3600 13:c17cf029d899 58 total += avg[i]; //AGC
ng3600 13:c17cf029d899 59
ng3600 2:6a87b2348245 60 if(diff[i] > highest){
ng3600 2:6a87b2348245 61 highest = diff[i];
ng3600 2:6a87b2348245 62 h_idx = i;
ng3600 2:6a87b2348245 63 }
ng3600 2:6a87b2348245 64 else if(diff[i] < lowest){
ng3600 2:6a87b2348245 65 lowest = diff[i];
ng3600 2:6a87b2348245 66 l_idx = i;
ng3600 2:6a87b2348245 67 }
ng3600 2:6a87b2348245 68 }
ng3600 13:c17cf029d899 69 //AGC, simple proportional controller
ng3600 13:c17cf029d899 70 total = total / frameLen;
ng3600 13:c17cf029d899 71 exposureChange = ((float)(MEAN_REF - total)) * CAM_CTRL_GAIN;
ng3600 3:f31986cb68fd 72 *exposure += (int)exposureChange;
ng3600 11:5e66d0531053 73 if(*exposure < 1)
ng3600 11:5e66d0531053 74 *exposure = 1;
ng3600 9:5226617d2019 75 else if(*exposure > 30)
ng3600 9:5226617d2019 76 *exposure = 30;
ng3600 0:2d546112b0b8 77 }
ng3600 13:c17cf029d899 78 //valid if white line on black and not blinded
ng3600 13:c17cf029d899 79 if(h_idx > l_idx && (highest - lowest) > THRESH)
ng3600 13:c17cf029d899 80 return ((float)(h_idx + l_idx)) / (2.0 * (float)frameLen); //0.5 is center
ng3600 13:c17cf029d899 81 else
ng3600 13:c17cf029d899 82 return -1.0; //invalid read
ng3600 0:2d546112b0b8 83 }
ng3600 0:2d546112b0b8 84
ng3600 3:f31986cb68fd 85 //call after integration time is done, returns index of array line is expected to be at and new exposure time
ng3600 12:ce6d9f7dc76e 86 float getLinePos(AnalogIn cam, DigitalOut *camSi, DigitalOut *camClk, int *exposure, telemetry::NumericArray<uint16_t, 128> &tele_linescan){
ng3600 3:f31986cb68fd 87 uint16_t lineAry[NUM_PIX];
ng3600 8:b9ec2f3e12b6 88 float position;
ng3600 0:2d546112b0b8 89
ng3600 0:2d546112b0b8 90 //read
ng3600 0:2d546112b0b8 91 startRead(camSi, camClk);
ng3600 0:2d546112b0b8 92 for(int i = 0; i < NUM_PIX; i++){
ng3600 0:2d546112b0b8 93 lineAry[i] = read1Bit(cam, camClk);
ng3600 12:ce6d9f7dc76e 94 tele_linescan[i] = lineAry[i];
ng3600 0:2d546112b0b8 95 }
ng3600 0:2d546112b0b8 96
ng3600 0:2d546112b0b8 97 //process line scan data
ng3600 3:f31986cb68fd 98 position = processFn(lineAry, NUM_PIX, exposure);
ng3600 0:2d546112b0b8 99
ng3600 0:2d546112b0b8 100 return position;
ng3600 0:2d546112b0b8 101 }
ng3600 0:2d546112b0b8 102
ng3600 0:2d546112b0b8 103 /*
ng3600 0:2d546112b0b8 104 sample call (handler thread):
ng3600 0:2d546112b0b8 105
ng3600 0:2d546112b0b8 106 while(1){
ng3600 3:f31986cb68fd 107 linePos = getLinePos(cameraIn1, si, clk, &exposureTime); //volatile linePos, replace with steering angle and read 2 cameras?
ng3600 3:f31986cb68fd 108 Thread::wait(exposureTime); //sleep for 14 us
ng3600 0:2d546112b0b8 109 }
ng3600 2:6a87b2348245 110 */