Team 9 / LineScan

Moved to Team 9.

Fork of LineScan by Nicholas Gan

LineScan.cpp@29:c832523d7d96, 2015-05-21 (annotated)

Committer:
ikrase
Date:
Thu May 21 23:22:50 2015 +0000
Revision:
29:c832523d7d96
Parent:
28:3006d46dcec5
Child:
31:25cec6b92c59
Added telemetry for maximum diff for threshold debugging.

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