Team 9 / LineScan

Moved to Team 9.

Fork of LineScan by Nicholas Gan

LineScan.cpp@16:aaac67b2bce4, 2015-04-18 (annotated)

Committer:
ng3600
Date:
Sat Apr 18 08:46:22 2015 +0000
Revision:
16:aaac67b2bce4
Parent:
15:4bd1c1d2cf94
Child:
17:4ba196fe78ea
Resolved a segmentation fault

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 14:928254a609cb 8 #define THRESH 5000 //may need adjusting
ng3600 8:b9ec2f3e12b6 9
ng3600 13:c17cf029d899 10 #define MEAN_REF 20000 //ideal mean we should see
ng3600 14:928254a609cb 11 #define CAM_CTRL_GAIN 0.0009 //should be small
ng3600 14:928254a609cb 12
ng3600 14:928254a609cb 13 #define NEUTRAL 0
ng3600 14:928254a609cb 14 #define FIND_PEAK 1
ng3600 14:928254a609cb 15 #define FIND_TROUGH 2
ng3600 14:928254a609cb 16
ng3600 15:4bd1c1d2cf94 17 //#define FRAMELEN NUM_PIX - 2 * SKIP
ng3600 3:f31986cb68fd 18
ng3600 0:2d546112b0b8 19 uint16_t read1Bit(AnalogIn cam, DigitalOut *camClk){
ng3600 0:2d546112b0b8 20 uint16_t pixel;
ng3600 0:2d546112b0b8 21
ng3600 0:2d546112b0b8 22 //read pixel n
ng3600 0:2d546112b0b8 23 pixel = cam.read_u16();
ng3600 0:2d546112b0b8 24
ng3600 0:2d546112b0b8 25 //clock pulse for next pixel n + 1
ng3600 0:2d546112b0b8 26 *camClk = 1;
ng3600 14:928254a609cb 27 *camClk = 0;
ng3600 0:2d546112b0b8 28
ng3600 0:2d546112b0b8 29 return pixel; //return result as an uint16_t (16 bit integer)
ng3600 0:2d546112b0b8 30 }
ng3600 0:2d546112b0b8 31
ng3600 0:2d546112b0b8 32 void startRead(DigitalOut *camSi, DigitalOut *camClk){
ng3600 0:2d546112b0b8 33 //pulse first clock cycle and start integral pin
ng3600 0:2d546112b0b8 34 *camSi = 1;
ng3600 0:2d546112b0b8 35 *camClk = 1;
ng3600 0:2d546112b0b8 36 *camSi = 0;
ng3600 0:2d546112b0b8 37 *camClk = 0;
ng3600 0:2d546112b0b8 38 }
ng3600 0:2d546112b0b8 39
ng3600 14:928254a609cb 40 float processFn(uint16_t *array, int arraySz, int* exposure, telemetry::NumericArray<uint16_t, NUM_PIX - 2 * SKIP> &tele_linescan_diff){ //consider adding diff to telemetry
ng3600 15:4bd1c1d2cf94 41 const static int frameLen = NUM_PIX - 2 * SKIP;
ng3600 15:4bd1c1d2cf94 42 int avg[frameLen];
ng3600 15:4bd1c1d2cf94 43 int diff[frameLen];
ng3600 2:6a87b2348245 44 int highest = 0;
ng3600 2:6a87b2348245 45 int lowest = 0;
ng3600 13:c17cf029d899 46 int total = 0;
ng3600 15:4bd1c1d2cf94 47 int h_idx_ary[frameLen];
ng3600 14:928254a609cb 48 int h_idx_pos = 0;
ng3600 15:4bd1c1d2cf94 49 int l_idx_ary[frameLen];
ng3600 14:928254a609cb 50 int l_idx_pos = 0;
ng3600 14:928254a609cb 51 int state = NEUTRAL;
ng3600 14:928254a609cb 52 int *l_walker, *h_walker;
ng3600 15:4bd1c1d2cf94 53 int out_width = frameLen;
ng3600 14:928254a609cb 54 float out = -1.0;
ng3600 0:2d546112b0b8 55
ng3600 3:f31986cb68fd 56 //for AGC
ng3600 3:f31986cb68fd 57 float exposureChange;
ng3600 3:f31986cb68fd 58
ng3600 16:aaac67b2bce4 59 memset (h_idx_ary, -1, frameLen);
ng3600 16:aaac67b2bce4 60 memset (l_idx_ary, -1, frameLen);
ng3600 14:928254a609cb 61
ng3600 16:aaac67b2bce4 62 if(array){
ng3600 8:b9ec2f3e12b6 63 avg[0] = array[SKIP - 1]/2 + array[SKIP]/2;
ng3600 2:6a87b2348245 64 diff[0] = 0;
ng3600 14:928254a609cb 65 tele_linescan_diff[0] = 0;
ng3600 2:6a87b2348245 66
ng3600 13:c17cf029d899 67 total += avg[0]; //AGC
ng3600 13:c17cf029d899 68
ng3600 15:4bd1c1d2cf94 69 for(int i = 1; i < frameLen; i++){
ng3600 8:b9ec2f3e12b6 70 avg[i] = array[i - 1 + SKIP]/2 + array[i + SKIP]/2; //smoothing
ng3600 3:f31986cb68fd 71 diff[i] = avg[i - 1] - avg[i]; //differential
ng3600 14:928254a609cb 72 tele_linescan_diff[i] = diff[i];
ng3600 2:6a87b2348245 73
ng3600 13:c17cf029d899 74 total += avg[i]; //AGC
ng3600 13:c17cf029d899 75
ng3600 14:928254a609cb 76 //Finite State Machine
ng3600 14:928254a609cb 77 switch(state){
ng3600 14:928254a609cb 78 case NEUTRAL:
ng3600 14:928254a609cb 79 if( diff[i] > THRESH ){
ng3600 14:928254a609cb 80 state = FIND_PEAK;
ng3600 14:928254a609cb 81 highest = diff[i];
ng3600 14:928254a609cb 82 }else if( diff[i] < -THRESH ){
ng3600 14:928254a609cb 83 state = FIND_TROUGH;
ng3600 14:928254a609cb 84 lowest = diff[i];
ng3600 14:928254a609cb 85 }
ng3600 14:928254a609cb 86 break;
ng3600 14:928254a609cb 87
ng3600 14:928254a609cb 88 case FIND_PEAK:
ng3600 14:928254a609cb 89 if( diff[i] <= THRESH )
ng3600 14:928254a609cb 90 state = NEUTRAL;
ng3600 14:928254a609cb 91 else if(diff[i] > highest){
ng3600 14:928254a609cb 92 highest = diff[i];
ng3600 14:928254a609cb 93 h_idx_ary[h_idx_pos] = i; //insert to array
ng3600 14:928254a609cb 94 h_idx_pos++;
ng3600 14:928254a609cb 95 }
ng3600 14:928254a609cb 96 break;
ng3600 14:928254a609cb 97
ng3600 14:928254a609cb 98 case FIND_TROUGH:
ng3600 14:928254a609cb 99 if( diff[i] >= -THRESH )
ng3600 14:928254a609cb 100 state = NEUTRAL;
ng3600 14:928254a609cb 101 else if(diff[i] < lowest){
ng3600 14:928254a609cb 102 lowest = diff[i];
ng3600 14:928254a609cb 103 l_idx_ary[l_idx_pos] = i; //insert to array
ng3600 14:928254a609cb 104 l_idx_pos++;
ng3600 14:928254a609cb 105 }
ng3600 14:928254a609cb 106 break;
ng3600 14:928254a609cb 107
ng3600 14:928254a609cb 108 default:
ng3600 14:928254a609cb 109 //exit case if exception happened
ng3600 14:928254a609cb 110 return out;
ng3600 2:6a87b2348245 111 }
ng3600 2:6a87b2348245 112 }
ng3600 13:c17cf029d899 113 //AGC, simple proportional controller
ng3600 15:4bd1c1d2cf94 114 total = total / frameLen;
ng3600 13:c17cf029d899 115 exposureChange = ((float)(MEAN_REF - total)) * CAM_CTRL_GAIN;
ng3600 3:f31986cb68fd 116 *exposure += (int)exposureChange;
ng3600 14:928254a609cb 117 if(*exposure < 0)
ng3600 14:928254a609cb 118 *exposure = 0;
ng3600 14:928254a609cb 119 else if(*exposure > UPDATE_RATE)
ng3600 14:928254a609cb 120 *exposure = UPDATE_RATE;
ng3600 0:2d546112b0b8 121 }
ng3600 14:928254a609cb 122
ng3600 14:928254a609cb 123 l_walker = l_idx_ary;
ng3600 14:928254a609cb 124 h_walker = h_idx_ary;
ng3600 16:aaac67b2bce4 125
ng3600 14:928254a609cb 126 while(*l_walker != -1 && *h_walker != -1){
ng3600 14:928254a609cb 127 //evaluate out and advance if line is white on black and returns center of smallest white band
ng3600 14:928254a609cb 128 //if interval is black on white, advance the pointer for the peak array
ng3600 14:928254a609cb 129 if(*h_walker > *l_walker && (*h_walker - *l_walker) < out_width){
ng3600 14:928254a609cb 130 out_width = *h_walker - *l_walker;
ng3600 15:4bd1c1d2cf94 131 out = ((float)(*h_walker + *l_walker)) / (2.0 * (float)frameLen); //0.5 is center
ng3600 14:928254a609cb 132 l_walker++;
ng3600 14:928254a609cb 133 }
ng3600 14:928254a609cb 134 h_walker++;
ng3600 14:928254a609cb 135 }
ng3600 14:928254a609cb 136
ng3600 14:928254a609cb 137 return out;
ng3600 0:2d546112b0b8 138 }
ng3600 0:2d546112b0b8 139
ng3600 3:f31986cb68fd 140 //call after integration time is done, returns index of array line is expected to be at and new exposure time
ng3600 14:928254a609cb 141 float getLinePos(AnalogIn cam,
ng3600 14:928254a609cb 142 DigitalOut *camSi,
ng3600 14:928254a609cb 143 DigitalOut *camClk,
ng3600 14:928254a609cb 144 int *exposure,
ng3600 14:928254a609cb 145 telemetry::NumericArray<uint16_t, 128> &tele_linescan,
ng3600 14:928254a609cb 146 telemetry::NumericArray<uint16_t, NUM_PIX - 2 * SKIP> &tele_linescan_diff){
ng3600 3:f31986cb68fd 147 uint16_t lineAry[NUM_PIX];
ng3600 8:b9ec2f3e12b6 148 float position;
ng3600 0:2d546112b0b8 149
ng3600 0:2d546112b0b8 150 //read
ng3600 0:2d546112b0b8 151 startRead(camSi, camClk);
ng3600 0:2d546112b0b8 152 for(int i = 0; i < NUM_PIX; i++){
ng3600 0:2d546112b0b8 153 lineAry[i] = read1Bit(cam, camClk);
ng3600 12:ce6d9f7dc76e 154 tele_linescan[i] = lineAry[i];
ng3600 0:2d546112b0b8 155 }
ng3600 0:2d546112b0b8 156
ng3600 0:2d546112b0b8 157 //process line scan data
ng3600 14:928254a609cb 158 position = processFn(lineAry, NUM_PIX, exposure, tele_linescan_diff);
ng3600 0:2d546112b0b8 159
ng3600 0:2d546112b0b8 160 return position;
ng3600 0:2d546112b0b8 161 }
ng3600 0:2d546112b0b8 162
ng3600 0:2d546112b0b8 163 /*
ng3600 0:2d546112b0b8 164 sample call (handler thread):
ng3600 0:2d546112b0b8 165
ng3600 0:2d546112b0b8 166 while(1){
ng3600 3:f31986cb68fd 167 linePos = getLinePos(cameraIn1, si, clk, &exposureTime); //volatile linePos, replace with steering angle and read 2 cameras?
ng3600 3:f31986cb68fd 168 Thread::wait(exposureTime); //sleep for 14 us
ng3600 0:2d546112b0b8 169 }
ng3600 2:6a87b2348245 170 */