Team 9 / LineScan

Moved to Team 9.

Fork of LineScan by Nicholas Gan

LineScan.cpp@35:ac9f45bd5352, 2015-11-16 (annotated)

Committer:
ng3600
Date:
Mon Nov 16 21:53:01 2015 +0000
Revision:
35:ac9f45bd5352
Parent:
33:cc3810ac5365
Child:
36:ad1f3321ec84
Semi Functional, able to follow line most of the time

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