Team 9 / LineScan

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of LineScan by Nicholas Gan

LineScan.cpp@33:cc3810ac5365, 2015-11-13 (annotated)

Committer:
ng3600
Date:
Fri Nov 13 02:23:16 2015 +0000
Revision:
33:cc3810ac5365
Parent:
28:3006d46dcec5
Child:
34:5c35287ff49e
Child:
35:ac9f45bd5352
Implemented spi comms with car to integrate with the driver

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 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 33:cc3810ac5365 160 if(*h_walker > *l_walker && (*h_walker - *l_walker) < 10 && (*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 */