Team 9 / LineScan

Moved to Team 9.

Fork of LineScan by Nicholas Gan

LineScan.cpp@38:1ac5f0ab5ae6, 2015-11-21 (annotated)

Committer:
ng3600
Date:
Sat Nov 21 04:19:29 2015 +0000
Revision:
38:1ac5f0ab5ae6
Parent:
37:b76faff78354
Child:
39:ea0660f7d724
Chassis basic functionality stable. Able to follow line well and receive command packets and transmit number of visible lines. Fixed bug where chassis locks up while peeking. Will need to verify if it will actually peek and see the other line.

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 38:1ac5f0ab5ae6 138 l_walker = l_idx_ary + IDX_ARY_LEN - 1;
ng3600 38:1ac5f0ab5ae6 139 h_walker = h_idx_ary + IDX_ARY_LEN - 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 37:b76faff78354 154
ng3600 38:1ac5f0ab5ae6 155 //serial.printf("%d %d\r\n", *h_walker, *l_walker);
ng3600 38:1ac5f0ab5ae6 156 //serial.printf("%.2f\r\n", out);
ng3600 14:928254a609cb 157 }
ng3600 38:1ac5f0ab5ae6 158 } else {
ng3600 38:1ac5f0ab5ae6 159 l_walker = l_idx_ary + 1;
ng3600 38:1ac5f0ab5ae6 160 h_walker = h_idx_ary + 1;
ng3600 18:8a65598abf2f 161
ng3600 38:1ac5f0ab5ae6 162 while(*l_walker != -1 && *h_walker != -1){
ng3600 38:1ac5f0ab5ae6 163 //evaluate out and advance if line is white on black and returns center of smallest white band
ng3600 38:1ac5f0ab5ae6 164 //if interval is black on white, advance the pointer for the peak array
ng3600 38:1ac5f0ab5ae6 165 //width needs to be larger than 4 pixels wide to be a good read.
ng3600 38:1ac5f0ab5ae6 166
ng3600 38:1ac5f0ab5ae6 167 if(*h_walker > *l_walker /*&& (*h_walker - *l_walker) < out_width*/ && (*h_walker - *l_walker) > 3){
ng3600 38:1ac5f0ab5ae6 168 //out_width = *h_walker - *l_walker;
ng3600 38:1ac5f0ab5ae6 169 out = ((float)(*h_walker + *l_walker)) / (2.0 * (float)frameLen); //0.5 is center
ng3600 38:1ac5f0ab5ae6 170
ng3600 38:1ac5f0ab5ae6 171 (*numLines)++;
ng3600 38:1ac5f0ab5ae6 172 l_walker++;
ng3600 38:1ac5f0ab5ae6 173 }
ng3600 38:1ac5f0ab5ae6 174 h_walker++;
ng3600 38:1ac5f0ab5ae6 175
ng3600 38:1ac5f0ab5ae6 176 //serial.printf("%d %d\r\n", *h_walker, *l_walker);
ng3600 38:1ac5f0ab5ae6 177 //serial.printf("%.2f\r\n", out);
ng3600 38:1ac5f0ab5ae6 178 }
ng3600 14:928254a609cb 179 }
ng3600 14:928254a609cb 180
ng3600 14:928254a609cb 181 return out;
ng3600 0:2d546112b0b8 182 }
ng3600 0:2d546112b0b8 183
ng3600 3:f31986cb68fd 184 //call after integration time is done, returns index of array line is expected to be at and new exposure time
ng3600 14:928254a609cb 185 float getLinePos(AnalogIn cam,
ng3600 14:928254a609cb 186 DigitalOut *camSi,
ng3600 14:928254a609cb 187 DigitalOut *camClk,
ng3600 14:928254a609cb 188 int *exposure,
ng3600 38:1ac5f0ab5ae6 189 bool preferLeft,
ng3600 38:1ac5f0ab5ae6 190 int *lineCt,
ng3600 38:1ac5f0ab5ae6 191 telemetry::NumericArray<uint16_t, NUM_PIX> &tele_linescan){
ng3600 38:1ac5f0ab5ae6 192
ng3600 3:f31986cb68fd 193 uint16_t lineAry[NUM_PIX];
ng3600 8:b9ec2f3e12b6 194 float position;
ng3600 0:2d546112b0b8 195
ng3600 0:2d546112b0b8 196 //read
ng3600 0:2d546112b0b8 197 startRead(camSi, camClk);
ng3600 0:2d546112b0b8 198 for(int i = 0; i < NUM_PIX; i++){
ng3600 0:2d546112b0b8 199 lineAry[i] = read1Bit(cam, camClk);
ng3600 12:ce6d9f7dc76e 200 tele_linescan[i] = lineAry[i];
ng3600 0:2d546112b0b8 201 }
ng3600 0:2d546112b0b8 202
ng3600 0:2d546112b0b8 203 //process line scan data
ng3600 38:1ac5f0ab5ae6 204 position = processFn(lineAry, NUM_PIX, exposure, preferLeft, lineCt);//, tele_linescan_diff);
ng3600 18:8a65598abf2f 205 //serial.printf("%.2f\r\n", position);
ng3600 0:2d546112b0b8 206
ng3600 0:2d546112b0b8 207 return position;
ng3600 0:2d546112b0b8 208 }
ng3600 0:2d546112b0b8 209
ng3600 0:2d546112b0b8 210 /*
ng3600 0:2d546112b0b8 211 sample call (handler thread):
ng3600 0:2d546112b0b8 212
ng3600 0:2d546112b0b8 213 while(1){
ng3600 3:f31986cb68fd 214 linePos = getLinePos(cameraIn1, si, clk, &exposureTime); //volatile linePos, replace with steering angle and read 2 cameras?
ng3600 3:f31986cb68fd 215 Thread::wait(exposureTime); //sleep for 14 us
ng3600 0:2d546112b0b8 216 }
ng3600 2:6a87b2348245 217 */