Team 9 / LineScan

Moved to Team 9.

Fork of LineScan by Nicholas Gan

LineScan.cpp@39:ea0660f7d724, 2015-11-23 (annotated)

Committer:
ng3600
Date:
Mon Nov 23 23:57:58 2015 +0000
Revision:
39:ea0660f7d724
Parent:
38:1ac5f0ab5ae6 
Bugfix, does not start right-to-left search at last element in array anymore. Start at last entry in array.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ng3600 0:2d546112b0b8 1 #include "LineScan.h"
ng3600 18:8a65598abf2f 2 #include <string.h>
ng3600 0:2d546112b0b8 3
ng3600 20:e9f0d1483ba1 4 #define THRESH 2000
ng3600 8:b9ec2f3e12b6 5
ng3600 20:e9f0d1483ba1 6 #define MEAN_REF 10000 //ideal mean we should see
ng3600 20:e9f0d1483ba1 7 #define CAM_CTRL_GAIN 0.0003 //should be small
ng3600 14:928254a609cb 8
ng3600 14:928254a609cb 9 #define NEUTRAL 0
ng3600 14:928254a609cb 10 #define FIND_PEAK 1
ng3600 14:928254a609cb 11 #define FIND_TROUGH 2
ng3600 14:928254a609cb 12
ng3600 38:1ac5f0ab5ae6 13 #define IDX_ARY_LEN 10
ng3600 38:1ac5f0ab5ae6 14
ng3600 0:2d546112b0b8 15 uint16_t read1Bit(AnalogIn cam, DigitalOut *camClk){
ng3600 0:2d546112b0b8 16 uint16_t pixel;
ng3600 0:2d546112b0b8 17
ng3600 0:2d546112b0b8 18 //read pixel n
ng3600 0:2d546112b0b8 19 pixel = cam.read_u16();
ng3600 0:2d546112b0b8 20
ng3600 0:2d546112b0b8 21 //clock pulse for next pixel n + 1
ng3600 0:2d546112b0b8 22 *camClk = 1;
ng3600 14:928254a609cb 23 *camClk = 0;
ng3600 0:2d546112b0b8 24
ng3600 0:2d546112b0b8 25 return pixel; //return result as an uint16_t (16 bit integer)
ng3600 0:2d546112b0b8 26 }
ng3600 0:2d546112b0b8 27
ng3600 0:2d546112b0b8 28 void startRead(DigitalOut *camSi, DigitalOut *camClk){
ng3600 0:2d546112b0b8 29 //pulse first clock cycle and start integral pin
ng3600 0:2d546112b0b8 30 *camSi = 1;
ng3600 0:2d546112b0b8 31 *camClk = 1;
ng3600 0:2d546112b0b8 32 *camSi = 0;
ng3600 0:2d546112b0b8 33 *camClk = 0;
ng3600 0:2d546112b0b8 34 }
ng3600 0:2d546112b0b8 35
ng3600 38:1ac5f0ab5ae6 36 float processFn(uint16_t *array, int arraySz, int* exposure, bool preferLeftLine, int *numLines){
ng3600 15:4bd1c1d2cf94 37 const static int frameLen = NUM_PIX - 2 * SKIP;
ng3600 15:4bd1c1d2cf94 38 int avg[frameLen];
ng3600 15:4bd1c1d2cf94 39 int diff[frameLen];
ng3600 2:6a87b2348245 40 int highest = 0;
ng3600 2:6a87b2348245 41 int lowest = 0;
ng3600 13:c17cf029d899 42 int total = 0;
ng3600 38:1ac5f0ab5ae6 43 int h_idx_ary[IDX_ARY_LEN];
ng3600 38:1ac5f0ab5ae6 44 int h_idx_pos = 1;
ng3600 38:1ac5f0ab5ae6 45 int l_idx_ary[IDX_ARY_LEN];
ng3600 38:1ac5f0ab5ae6 46 int l_idx_pos = 1;
ng3600 14:928254a609cb 47 int state = NEUTRAL;
ng3600 14:928254a609cb 48 int *l_walker, *h_walker;
ng3600 38:1ac5f0ab5ae6 49 //int out_width = frameLen;
ng3600 18:8a65598abf2f 50 int i;
ng3600 14:928254a609cb 51 float out = -1.0;
ng3600 0:2d546112b0b8 52
ng3600 3:f31986cb68fd 53 //for AGC
ng3600 3:f31986cb68fd 54 float exposureChange;
ng3600 3:f31986cb68fd 55
ng3600 38:1ac5f0ab5ae6 56 //line counter initialization
ng3600 38:1ac5f0ab5ae6 57 *numLines = 0;
ng3600 38:1ac5f0ab5ae6 58
ng3600 18:8a65598abf2f 59 h_idx_ary[0] = -1;
ng3600 18:8a65598abf2f 60 l_idx_ary[0] = -1;
ng3600 38:1ac5f0ab5ae6 61 h_idx_ary[1] = -1;
ng3600 38:1ac5f0ab5ae6 62 l_idx_ary[1] = -1;
ng3600 14:928254a609cb 63
ng3600 16:aaac67b2bce4 64 if(array){
ng3600 8:b9ec2f3e12b6 65 avg[0] = array[SKIP - 1]/2 + array[SKIP]/2;
ng3600 2:6a87b2348245 66 diff[0] = 0;
ng3600 19:5a29c887c8eb 67 //tele_linescan_diff[0] = diff[0];
ng3600 2:6a87b2348245 68
ng3600 13:c17cf029d899 69 total += avg[0]; //AGC
ng3600 13:c17cf029d899 70
ng3600 18:8a65598abf2f 71 for(i = 1; i < frameLen; i++){
ng3600 8:b9ec2f3e12b6 72 avg[i] = array[i - 1 + SKIP]/2 + array[i + SKIP]/2; //smoothing
ng3600 3:f31986cb68fd 73 diff[i] = avg[i - 1] - avg[i]; //differential
ng3600 19:5a29c887c8eb 74 //tele_linescan_diff[i] = diff[i];
ng3600 2:6a87b2348245 75
ng3600 13:c17cf029d899 76 total += avg[i]; //AGC
ng3600 13:c17cf029d899 77
ng3600 18:8a65598abf2f 78
ng3600 14:928254a609cb 79 //Finite State Machine
ng3600 18:8a65598abf2f 80 //problem in this section, rewrite? find peaks and troughs
ng3600 14:928254a609cb 81 switch(state){
ng3600 14:928254a609cb 82 case NEUTRAL:
ng3600 18:8a65598abf2f 83 //serial.printf("Neutral case in processFn\r\n");
ng3600 14:928254a609cb 84 if( diff[i] > THRESH ){
ng3600 14:928254a609cb 85 state = FIND_PEAK;
ng3600 14:928254a609cb 86 highest = diff[i];
ng3600 18:8a65598abf2f 87 h_idx_ary[h_idx_pos] = i;
ng3600 14:928254a609cb 88 }else if( diff[i] < -THRESH ){
ng3600 14:928254a609cb 89 state = FIND_TROUGH;
ng3600 14:928254a609cb 90 lowest = diff[i];
ng3600 18:8a65598abf2f 91 l_idx_ary[l_idx_pos] = i;
ng3600 14:928254a609cb 92 }
ng3600 14:928254a609cb 93 break;
ng3600 14:928254a609cb 94
ng3600 14:928254a609cb 95 case FIND_PEAK:
ng3600 18:8a65598abf2f 96 //serial.printf("Peak case in processFn\r\n");
ng3600 18:8a65598abf2f 97 if( diff[i] <= THRESH ){
ng3600 14:928254a609cb 98 state = NEUTRAL;
ng3600 18:8a65598abf2f 99 h_idx_pos++;
ng3600 18:8a65598abf2f 100 h_idx_ary[h_idx_pos] = -1; //last element is always -1
ng3600 18:8a65598abf2f 101 }
ng3600 14:928254a609cb 102 else if(diff[i] > highest){
ng3600 14:928254a609cb 103 highest = diff[i];
ng3600 18:8a65598abf2f 104 h_idx_ary[h_idx_pos] = i; //insert to array, clobbers
ng3600 14:928254a609cb 105 }
ng3600 14:928254a609cb 106 break;
ng3600 14:928254a609cb 107
ng3600 14:928254a609cb 108 case FIND_TROUGH:
ng3600 18:8a65598abf2f 109 //serial.printf("Trough case in processFn\r\n");
ng3600 18:8a65598abf2f 110 if( diff[i] >= -THRESH ){
ng3600 14:928254a609cb 111 state = NEUTRAL;
ng3600 18:8a65598abf2f 112 l_idx_pos++;
ng3600 18:8a65598abf2f 113 l_idx_ary[l_idx_pos] = -1; //last element is always -1
ng3600 18:8a65598abf2f 114 }
ng3600 14:928254a609cb 115 else if(diff[i] < lowest){
ng3600 14:928254a609cb 116 lowest = diff[i];
ng3600 18:8a65598abf2f 117 l_idx_ary[l_idx_pos] = i; //insert to array, clobbers
ng3600 14:928254a609cb 118 }
ng3600 14:928254a609cb 119 break;
ng3600 14:928254a609cb 120
ng3600 14:928254a609cb 121 default:
ng3600 14:928254a609cb 122 //exit case if exception happened
ng3600 18:8a65598abf2f 123 serial.printf("Default case in processFn\r\n");
ng3600 14:928254a609cb 124 return out;
ng3600 2:6a87b2348245 125 }
ng3600 2:6a87b2348245 126 }
ng3600 13:c17cf029d899 127 //AGC, simple proportional controller
ng3600 15:4bd1c1d2cf94 128 total = total / frameLen;
ng3600 13:c17cf029d899 129 exposureChange = ((float)(MEAN_REF - total)) * CAM_CTRL_GAIN;
ng3600 3:f31986cb68fd 130 *exposure += (int)exposureChange;
ng3600 14:928254a609cb 131 if(*exposure < 0)
ng3600 14:928254a609cb 132 *exposure = 0;
ng3600 14:928254a609cb 133 else if(*exposure > UPDATE_RATE)
ng3600 14:928254a609cb 134 *exposure = UPDATE_RATE;
ng3600 0:2d546112b0b8 135 }
ng3600 14:928254a609cb 136
ng3600 38:1ac5f0ab5ae6 137 if(preferLeftLine){
ng3600 39:ea0660f7d724 138 l_walker = l_idx_ary + l_idx_pos - 1;
ng3600 39:ea0660f7d724 139 h_walker = h_idx_ary + h_idx_pos - 1;
ng3600 18:8a65598abf2f 140
ng3600 38:1ac5f0ab5ae6 141 while(*l_walker != -1 && *h_walker != -1){
ng3600 38:1ac5f0ab5ae6 142 //evaluate out and advance if line is white on black and returns center of smallest white band
ng3600 38:1ac5f0ab5ae6 143 //if interval is black on white, advance the pointer for the peak array
ng3600 38:1ac5f0ab5ae6 144 //width needs to be larger than 4 pixels wide to be a good read.
ng3600 38:1ac5f0ab5ae6 145
ng3600 38:1ac5f0ab5ae6 146 if(*h_walker > *l_walker /*&& (*h_walker - *l_walker) < out_width*/ && (*h_walker - *l_walker) > 3){
ng3600 38:1ac5f0ab5ae6 147 //out_width = *h_walker - *l_walker;
ng3600 38:1ac5f0ab5ae6 148 out = ((float)(*h_walker + *l_walker)) / (2.0 * (float)frameLen); //0.5 is center
ng3600 38:1ac5f0ab5ae6 149
ng3600 38:1ac5f0ab5ae6 150 (*numLines)++;
ng3600 38:1ac5f0ab5ae6 151 h_walker--;
ng3600 38:1ac5f0ab5ae6 152 }
ng3600 38:1ac5f0ab5ae6 153 l_walker--;
ng3600 14:928254a609cb 154 }
ng3600 38:1ac5f0ab5ae6 155 } else {
ng3600 38:1ac5f0ab5ae6 156 l_walker = l_idx_ary + 1;
ng3600 38:1ac5f0ab5ae6 157 h_walker = h_idx_ary + 1;
ng3600 18:8a65598abf2f 158
ng3600 38:1ac5f0ab5ae6 159 while(*l_walker != -1 && *h_walker != -1){
ng3600 38:1ac5f0ab5ae6 160 //evaluate out and advance if line is white on black and returns center of smallest white band
ng3600 38:1ac5f0ab5ae6 161 //if interval is black on white, advance the pointer for the peak array
ng3600 38:1ac5f0ab5ae6 162 //width needs to be larger than 4 pixels wide to be a good read.
ng3600 38:1ac5f0ab5ae6 163
ng3600 38:1ac5f0ab5ae6 164 if(*h_walker > *l_walker /*&& (*h_walker - *l_walker) < out_width*/ && (*h_walker - *l_walker) > 3){
ng3600 38:1ac5f0ab5ae6 165 //out_width = *h_walker - *l_walker;
ng3600 38:1ac5f0ab5ae6 166 out = ((float)(*h_walker + *l_walker)) / (2.0 * (float)frameLen); //0.5 is center
ng3600 38:1ac5f0ab5ae6 167
ng3600 38:1ac5f0ab5ae6 168 (*numLines)++;
ng3600 38:1ac5f0ab5ae6 169 l_walker++;
ng3600 38:1ac5f0ab5ae6 170 }
ng3600 38:1ac5f0ab5ae6 171 h_walker++;
ng3600 38:1ac5f0ab5ae6 172 }
ng3600 14:928254a609cb 173 }
ng3600 14:928254a609cb 174
ng3600 14:928254a609cb 175 return out;
ng3600 0:2d546112b0b8 176 }
ng3600 0:2d546112b0b8 177
ng3600 3:f31986cb68fd 178 //call after integration time is done, returns index of array line is expected to be at and new exposure time
ng3600 14:928254a609cb 179 float getLinePos(AnalogIn cam,
ng3600 14:928254a609cb 180 DigitalOut *camSi,
ng3600 14:928254a609cb 181 DigitalOut *camClk,
ng3600 14:928254a609cb 182 int *exposure,
ng3600 38:1ac5f0ab5ae6 183 bool preferLeft,
ng3600 38:1ac5f0ab5ae6 184 int *lineCt,
ng3600 38:1ac5f0ab5ae6 185 telemetry::NumericArray<uint16_t, NUM_PIX> &tele_linescan){
ng3600 38:1ac5f0ab5ae6 186
ng3600 3:f31986cb68fd 187 uint16_t lineAry[NUM_PIX];
ng3600 8:b9ec2f3e12b6 188 float position;
ng3600 0:2d546112b0b8 189
ng3600 0:2d546112b0b8 190 //read
ng3600 0:2d546112b0b8 191 startRead(camSi, camClk);
ng3600 0:2d546112b0b8 192 for(int i = 0; i < NUM_PIX; i++){
ng3600 0:2d546112b0b8 193 lineAry[i] = read1Bit(cam, camClk);
ng3600 12:ce6d9f7dc76e 194 tele_linescan[i] = lineAry[i];
ng3600 0:2d546112b0b8 195 }
ng3600 0:2d546112b0b8 196
ng3600 0:2d546112b0b8 197 //process line scan data
ng3600 38:1ac5f0ab5ae6 198 position = processFn(lineAry, NUM_PIX, exposure, preferLeft, lineCt);//, tele_linescan_diff);
ng3600 18:8a65598abf2f 199 //serial.printf("%.2f\r\n", position);
ng3600 0:2d546112b0b8 200
ng3600 0:2d546112b0b8 201 return position;
ng3600 0:2d546112b0b8 202 }
ng3600 0:2d546112b0b8 203
ng3600 0:2d546112b0b8 204 /*
ng3600 0:2d546112b0b8 205 sample call (handler thread):
ng3600 0:2d546112b0b8 206
ng3600 0:2d546112b0b8 207 while(1){
ng3600 3:f31986cb68fd 208 linePos = getLinePos(cameraIn1, si, clk, &exposureTime); //volatile linePos, replace with steering angle and read 2 cameras?
ng3600 3:f31986cb68fd 209 Thread::wait(exposureTime); //sleep for 14 us
ng3600 0:2d546112b0b8 210 }
ng3600 2:6a87b2348245 211 */