Team 9 / LineScan

Moved to Team 9.

Fork of LineScan by Nicholas Gan

LineScan.cpp@19:5a29c887c8eb, 2015-04-23 (annotated)

Committer:
ng3600
Date:
Thu Apr 23 17:40:02 2015 +0000
Revision:
19:5a29c887c8eb
Parent:
18:8a65598abf2f
Child:
20:e9f0d1483ba1
Tuned AGC, handles sudden changes in light well now.

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