Team 9 / LineScan

Moved to Team 9.

Fork of LineScan by Nicholas Gan

LineScan.cpp@21:68db1d0a5534, 2015-04-18 (annotated)

Committer:
dnleek
Date:
Sat Apr 18 07:03:00 2015 +0000
Revision:
21:68db1d0a5534
Parent:
13:c17cf029d899
Child:
22:0136520fe249
freescale cup branch

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;
dnleek 21:68db1d0a5534 43 int left_bound = SKIP;
dnleek 21:68db1d0a5534 44 int right_bound = NUM_PIX - SKIP;
ng3600 0:2d546112b0b8 45
ng3600 3:f31986cb68fd 46 //for AGC
ng3600 3:f31986cb68fd 47 float exposureChange;
ng3600 3:f31986cb68fd 48
ng3600 3:f31986cb68fd 49 //Just finds line center, does not track crossings (needs this feature)
ng3600 13:c17cf029d899 50 if(array){
ng3600 8:b9ec2f3e12b6 51 avg[0] = array[SKIP - 1]/2 + array[SKIP]/2;
ng3600 2:6a87b2348245 52 diff[0] = 0;
ng3600 2:6a87b2348245 53
ng3600 13:c17cf029d899 54 total += avg[0]; //AGC
ng3600 13:c17cf029d899 55
ng3600 8:b9ec2f3e12b6 56 for(int i = 1; i < frameLen; i++){
ng3600 8:b9ec2f3e12b6 57 avg[i] = array[i - 1 + SKIP]/2 + array[i + SKIP]/2; //smoothing
ng3600 3:f31986cb68fd 58 diff[i] = avg[i - 1] - avg[i]; //differential
ng3600 2:6a87b2348245 59
ng3600 13:c17cf029d899 60 total += avg[i]; //AGC
ng3600 13:c17cf029d899 61
ng3600 2:6a87b2348245 62 if(diff[i] > highest){
ng3600 2:6a87b2348245 63 highest = diff[i];
ng3600 2:6a87b2348245 64 h_idx = i;
ng3600 2:6a87b2348245 65 }
ng3600 2:6a87b2348245 66 else if(diff[i] < lowest){
ng3600 2:6a87b2348245 67 lowest = diff[i];
ng3600 2:6a87b2348245 68 l_idx = i;
ng3600 2:6a87b2348245 69 }
ng3600 2:6a87b2348245 70 }
ng3600 13:c17cf029d899 71 //AGC, simple proportional controller
ng3600 13:c17cf029d899 72 total = total / frameLen;
ng3600 13:c17cf029d899 73 exposureChange = ((float)(MEAN_REF - total)) * CAM_CTRL_GAIN;
ng3600 3:f31986cb68fd 74 *exposure += (int)exposureChange;
ng3600 11:5e66d0531053 75 if(*exposure < 1)
ng3600 11:5e66d0531053 76 *exposure = 1;
ng3600 9:5226617d2019 77 else if(*exposure > 30)
ng3600 9:5226617d2019 78 *exposure = 30;
ng3600 0:2d546112b0b8 79 }
ng3600 13:c17cf029d899 80 //valid if white line on black and not blinded
dnleek 21:68db1d0a5534 81 if (highest > THRESH && -1 * lowest > THRESH)
dnleek 21:68db1d0a5534 82 return ((float) h_idx + (float) l_idx) / (2*(float)frameLen);
dnleek 21:68db1d0a5534 83 else if(highest > THRESH && -1 * lowest < THRESH )
dnleek 21:68db1d0a5534 84 return ((float) h_idx + left_bound) / (2*(float)frameLen); //0.5 is center
dnleek 21:68db1d0a5534 85 else if (-1*lowest > THRESH && highest < THRESH )
dnleek 21:68db1d0a5534 86 return ((float) l_idx + right_bound) / (2*(float)frameLen);
ng3600 13:c17cf029d899 87 else
ng3600 13:c17cf029d899 88 return -1.0; //invalid read
ng3600 0:2d546112b0b8 89 }
ng3600 0:2d546112b0b8 90
ng3600 3:f31986cb68fd 91 //call after integration time is done, returns index of array line is expected to be at and new exposure time
ng3600 12:ce6d9f7dc76e 92 float getLinePos(AnalogIn cam, DigitalOut *camSi, DigitalOut *camClk, int *exposure, telemetry::NumericArray<uint16_t, 128> &tele_linescan){
ng3600 3:f31986cb68fd 93 uint16_t lineAry[NUM_PIX];
ng3600 8:b9ec2f3e12b6 94 float position;
ng3600 0:2d546112b0b8 95
ng3600 0:2d546112b0b8 96 //read
ng3600 0:2d546112b0b8 97 startRead(camSi, camClk);
ng3600 0:2d546112b0b8 98 for(int i = 0; i < NUM_PIX; i++){
ng3600 0:2d546112b0b8 99 lineAry[i] = read1Bit(cam, camClk);
ng3600 12:ce6d9f7dc76e 100 tele_linescan[i] = lineAry[i];
ng3600 0:2d546112b0b8 101 }
ng3600 0:2d546112b0b8 102
ng3600 0:2d546112b0b8 103 //process line scan data
ng3600 3:f31986cb68fd 104 position = processFn(lineAry, NUM_PIX, exposure);
ng3600 0:2d546112b0b8 105
ng3600 0:2d546112b0b8 106 return position;
ng3600 0:2d546112b0b8 107 }
ng3600 0:2d546112b0b8 108
ng3600 0:2d546112b0b8 109 /*
ng3600 0:2d546112b0b8 110 sample call (handler thread):
ng3600 0:2d546112b0b8 111
ng3600 0:2d546112b0b8 112 while(1){
ng3600 3:f31986cb68fd 113 linePos = getLinePos(cameraIn1, si, clk, &exposureTime); //volatile linePos, replace with steering angle and read 2 cameras?
ng3600 3:f31986cb68fd 114 Thread::wait(exposureTime); //sleep for 14 us
ng3600 0:2d546112b0b8 115 }
ng3600 2:6a87b2348245 116 */