Team 9 / LineScan

Moved to Team 9.

Fork of LineScan by Nicholas Gan

LineScan.cpp@18:8a65598abf2f, 2015-04-22 (annotated)

Committer:
ng3600
Date:
Wed Apr 22 02:05:47 2015 +0000
Revision:
18:8a65598abf2f
Parent:
17:4ba196fe78ea
Child:
19:5a29c887c8eb
Running new linescan code and slightly tuned PI control. Issue was caused by running out of stack memory.

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 13:c17cf029d899 6 #define MEAN_REF 20000 //ideal mean we should see
ng3600 14:928254a609cb 7 #define CAM_CTRL_GAIN 0.0009 //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 18:8a65598abf2f 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 18:8a65598abf2f 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 14:928254a609cb 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 18:8a65598abf2f 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 18:8a65598abf2f 162 telemetry::NumericArray<uint16_t, NUM_PIX> &tele_linescan,
ng3600 18:8a65598abf2f 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 14:928254a609cb 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 */