Team 9 / LineScan

Moved to Team 9.

Fork of LineScan by Nicholas Gan

LineScan.cpp@37:b76faff78354, 2015-11-19 (annotated)

Committer:
ng3600
Date:
Thu Nov 19 23:00:20 2015 +0000
Revision:
37:b76faff78354
Parent:
20:e9f0d1483ba1
Child:
38:1ac5f0ab5ae6
Trimmed comments

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 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 14:928254a609cb 130 l_walker = l_idx_ary;
ng3600 14:928254a609cb 131 h_walker = h_idx_ary;
ng3600 16:aaac67b2bce4 132
ng3600 14:928254a609cb 133 while(*l_walker != -1 && *h_walker != -1){
ng3600 14:928254a609cb 134 //evaluate out and advance if line is white on black and returns center of smallest white band
ng3600 14:928254a609cb 135 //if interval is black on white, advance the pointer for the peak array
ng3600 18:8a65598abf2f 136 //width needs to be larger than 4 pixels wide to be a good read.
ng3600 18:8a65598abf2f 137
ng3600 20:e9f0d1483ba1 138 if(*h_walker > *l_walker && (*h_walker - *l_walker) < out_width && (*h_walker - *l_walker) > 3){
ng3600 14:928254a609cb 139 out_width = *h_walker - *l_walker;
ng3600 15:4bd1c1d2cf94 140 out = ((float)(*h_walker + *l_walker)) / (2.0 * (float)frameLen); //0.5 is center
ng3600 37:b76faff78354 141 return out;
ng3600 37:b76faff78354 142
ng3600 14:928254a609cb 143 l_walker++;
ng3600 14:928254a609cb 144 }
ng3600 14:928254a609cb 145 h_walker++;
ng3600 18:8a65598abf2f 146
ng3600 18:8a65598abf2f 147 //serial.printf("%d %d\r\n", *h_walker, *l_walker);
ng3600 18:8a65598abf2f 148 //serial.printf("%.2f\r\n", out);
ng3600 14:928254a609cb 149 }
ng3600 14:928254a609cb 150
ng3600 14:928254a609cb 151 return out;
ng3600 0:2d546112b0b8 152 }
ng3600 0:2d546112b0b8 153
ng3600 3:f31986cb68fd 154 //call after integration time is done, returns index of array line is expected to be at and new exposure time
ng3600 14:928254a609cb 155 float getLinePos(AnalogIn cam,
ng3600 14:928254a609cb 156 DigitalOut *camSi,
ng3600 14:928254a609cb 157 DigitalOut *camClk,
ng3600 14:928254a609cb 158 int *exposure,
ng3600 19:5a29c887c8eb 159 telemetry::NumericArray<uint16_t, NUM_PIX> &tele_linescan){//,
ng3600 19:5a29c887c8eb 160 //telemetry::NumericArray<uint16_t, NUM_PIX> &tele_linescan_diff){
ng3600 3:f31986cb68fd 161 uint16_t lineAry[NUM_PIX];
ng3600 8:b9ec2f3e12b6 162 float position;
ng3600 0:2d546112b0b8 163
ng3600 0:2d546112b0b8 164 //read
ng3600 0:2d546112b0b8 165 startRead(camSi, camClk);
ng3600 0:2d546112b0b8 166 for(int i = 0; i < NUM_PIX; i++){
ng3600 0:2d546112b0b8 167 lineAry[i] = read1Bit(cam, camClk);
ng3600 12:ce6d9f7dc76e 168 tele_linescan[i] = lineAry[i];
ng3600 0:2d546112b0b8 169 }
ng3600 0:2d546112b0b8 170
ng3600 0:2d546112b0b8 171 //process line scan data
ng3600 19:5a29c887c8eb 172 position = processFn(lineAry, NUM_PIX, exposure);//, tele_linescan_diff);
ng3600 18:8a65598abf2f 173 //serial.printf("%.2f\r\n", position);
ng3600 0:2d546112b0b8 174
ng3600 0:2d546112b0b8 175 return position;
ng3600 0:2d546112b0b8 176 }
ng3600 0:2d546112b0b8 177
ng3600 0:2d546112b0b8 178 /*
ng3600 0:2d546112b0b8 179 sample call (handler thread):
ng3600 0:2d546112b0b8 180
ng3600 0:2d546112b0b8 181 while(1){
ng3600 3:f31986cb68fd 182 linePos = getLinePos(cameraIn1, si, clk, &exposureTime); //volatile linePos, replace with steering angle and read 2 cameras?
ng3600 3:f31986cb68fd 183 Thread::wait(exposureTime); //sleep for 14 us
ng3600 0:2d546112b0b8 184 }
ng3600 2:6a87b2348245 185 */