Team 9
Moved to Team 9.
Fork of
LineScan
by 
Nicholas Gan
Diff: LineScan.cpp
- Revision:
- 24:774104c34063
- Parent:
- 20:e9f0d1483ba1
- Child:
- 25:8357ee048ad3
--- a/LineScan.cpp Sat Apr 25 02:42:06 2015 +0000
+++ b/LineScan.cpp Mon Apr 27 23:10:45 2015 +0000
@@ -10,20 +10,22 @@
#define FIND_PEAK 1
#define FIND_TROUGH 2
-uint16_t read1Bit(AnalogIn cam, DigitalOut *camClk){
+uint16_t read1Bit(AnalogIn cam, DigitalOut *camClk)
+{
uint16_t pixel;
-
+
//read pixel n
pixel = cam.read_u16();
-
+
//clock pulse for next pixel n + 1
*camClk = 1;
*camClk = 0;
-
+
return pixel; //return result as an uint16_t (16 bit integer)
}
-void startRead(DigitalOut *camSi, DigitalOut *camClk){
+void startRead(DigitalOut *camSi, DigitalOut *camClk)
+{
//pulse first clock cycle and start integral pin
*camSi = 1;
*camClk = 1;
@@ -31,7 +33,8 @@
*camClk = 0;
}
-float processFn(uint16_t *array, int arraySz, int* exposure){//, telemetry::NumericArray<uint16_t, NUM_PIX> &tele_linescan_diff){
+float processFn(uint16_t *array, int arraySz, int* exposure, telemetry::NumericArray<uint16_t, NUM_PIX> &tele_linescan_diff)
+{
const static int frameLen = NUM_PIX - 2 * SKIP;
int avg[frameLen];
int diff[frameLen];
@@ -47,70 +50,96 @@
int out_width = frameLen;
int i;
float out = -1.0;
-
+ int thinnest = 128;
+ int linestart = -1;
+ int linestop = -1;
+ int tempstart = 0;
+
//for AGC
float exposureChange;
-
+
h_idx_ary[0] = -1;
l_idx_ary[0] = -1;
-
- if(array){
+
+ if(array) {
avg[0] = array[SKIP - 1]/2 + array[SKIP]/2;
diff[0] = 0;
- //tele_linescan_diff[0] = diff[0];
-
+ tele_linescan_diff[0] = diff[0];
+
total += avg[0]; //AGC
-
- for(i = 1; i < frameLen; i++){
+
+ for(i = 1; i < frameLen; i++) {
avg[i] = array[i - 1 + SKIP]/2 + array[i + SKIP]/2; //smoothing
- diff[i] = avg[i - 1] - avg[i]; //differential
- //tele_linescan_diff[i] = diff[i];
-
+ if ((avg[i] - avg[i-1] >= THRESH) || (avg[i] - avg[i-1] <= -THRESH)) {
+ diff[i] = avg[i] - avg[i-1]; //differential
+ } else {
+ diff[i] = 0;
+ }
+ if (diff [i] > 0) {
+ tele_linescan_diff[i] = 10000;
+ } else if (diff[i] < 0) {
+ tele_linescan_diff[i] = 5000;
+ } else {
+ tele_linescan_diff[i] = 0;
+ }
+
total += avg[i]; //AGC
-
-
+
+
//Finite State Machine
//problem in this section, rewrite? find peaks and troughs
- switch(state){
+ switch(state) {
case NEUTRAL:
//serial.printf("Neutral case in processFn\r\n");
- if( diff[i] > THRESH ){
+ if( diff[i] > THRESH ) {
state = FIND_PEAK;
highest = diff[i];
h_idx_ary[h_idx_pos] = i;
- }else if( diff[i] < -THRESH ){
+ tempstart = i;
+ } /*else if( diff[i] < -THRESH ) {
state = FIND_TROUGH;
lowest = diff[i];
l_idx_ary[l_idx_pos] = i;
- }
+ tempstart = i;
+ }*/
break;
-
+
case FIND_PEAK:
//serial.printf("Peak case in processFn\r\n");
- if( diff[i] <= THRESH ){
+ if( diff[i] <= -THRESH ) {
state = NEUTRAL;
h_idx_pos++;
h_idx_ary[h_idx_pos] = -1; //last element is always -1
- }
- else if(diff[i] > highest){
+ if ((i - tempstart > 4) && (i - tempstart < thinnest)) {
+ thinnest = i - tempstart;
+ linestart = tempstart;
+ linestop = i;
+ }
+ } else if(diff[i] > highest) {
highest = diff[i];
h_idx_ary[h_idx_pos] = i; //insert to array, clobbers
+ tempstart = i;
}
break;
-
+
case FIND_TROUGH:
//serial.printf("Trough case in processFn\r\n");
- if( diff[i] >= -THRESH ){
+ if( diff[i] >= THRESH ) {
state = NEUTRAL;
l_idx_pos++;
l_idx_ary[l_idx_pos] = -1; //last element is always -1
- }
- else if(diff[i] < lowest){
+ if ((i - tempstart > 4) && (i - tempstart < thinnest)) {
+ thinnest = i - tempstart;
+ linestart = tempstart;
+ linestop = i;
+ }
+ } else if(diff[i] < lowest) {
lowest = diff[i];
l_idx_ary[l_idx_pos] = i; //insert to array, clobbers
+ tempstart = i;
}
break;
-
+
default:
//exit case if exception happened
serial.printf("Default case in processFn\r\n");
@@ -119,65 +148,221 @@
}
//AGC, simple proportional controller
total = total / frameLen;
- exposureChange = ((float)(MEAN_REF - total)) * CAM_CTRL_GAIN;
+ exposureChange = ((float)(MEAN_REF - total)) * CAM_CTRL_GAIN;
*exposure += (int)exposureChange;
if(*exposure < 0)
*exposure = 0;
else if(*exposure > UPDATE_RATE)
*exposure = UPDATE_RATE;
}
-
+
l_walker = l_idx_ary;
h_walker = h_idx_ary;
-
- while(*l_walker != -1 && *h_walker != -1){
+
+ /*while(*l_walker != -1 && *h_walker != -1){
//evaluate out and advance if line is white on black and returns center of smallest white band
//if interval is black on white, advance the pointer for the peak array
//width needs to be larger than 4 pixels wide to be a good read.
-
+
if(*h_walker > *l_walker && (*h_walker - *l_walker) < out_width && (*h_walker - *l_walker) > 3){
out_width = *h_walker - *l_walker;
out = ((float)(*h_walker + *l_walker)) / (2.0 * (float)frameLen); //0.5 is center
l_walker++;
}
h_walker++;
-
+
//serial.printf("%d %d\r\n", *h_walker, *l_walker);
//serial.printf("%.2f\r\n", out);
+ }*/
+
+ if (linestart != -1) {
+ out = ((float)(linestart + linestop)) / (2.0 * (float)frameLen);
+ } else {
+ out = -1;
}
-
+ return out;
+}
+
+float processFn2(uint16_t *array, int arraySz, int* exposure)
+{
+ const static int frameLen = NUM_PIX - 2 * SKIP;
+ int avg[frameLen];
+ int diff[frameLen];
+ int highest = 0;
+ int lowest = 0;
+ int total = 0;
+ int h_idx_ary[10];
+ int h_idx_pos = 0;
+ int l_idx_ary[10];
+ int l_idx_pos = 0;
+ int state = NEUTRAL;
+ int *l_walker, *h_walker;
+ int out_width = frameLen;
+ int i;
+ float out = -1.0;
+ int thinnest = 128;
+ int linestart = -1;
+ int linestop = -1;
+ int tempstart = 0;
+
+ //for AGC
+ float exposureChange;
+
+ h_idx_ary[0] = -1;
+ l_idx_ary[0] = -1;
+
+ if(array) {
+ avg[0] = array[SKIP - 1]/2 + array[SKIP]/2;
+ diff[0] = 0;
+ //tele_linescan_diff[0] = diff[0];
+
+ total += avg[0]; //AGC
+
+ for(i = 1; i < frameLen; i++) {
+ avg[i] = array[i - 1 + SKIP]/2 + array[i + SKIP]/2; //smoothing
+ diff[i] = avg[i] - avg[i-1]; //differential
+ //tele_linescan_diff[i] = diff[i];
+
+ total += avg[i]; //AGC
+
+
+ //Finite State Machine
+ //problem in this section, rewrite? find peaks and troughs
+ switch(state) {
+ case NEUTRAL:
+ //serial.printf("Neutral case in processFn\r\n");
+ if( diff[i] > THRESH ) {
+ state = FIND_PEAK;
+ highest = diff[i];
+ h_idx_ary[h_idx_pos] = i;
+ tempstart = i;
+ } else if( diff[i] < -THRESH ) {
+ state = FIND_TROUGH;
+ lowest = diff[i];
+ l_idx_ary[l_idx_pos] = i;
+ tempstart = i;
+ }
+ break;
+
+ case FIND_PEAK:
+ //serial.printf("Peak case in processFn\r\n");
+ if( diff[i] <= THRESH ) {
+ state = NEUTRAL;
+ h_idx_pos++;
+ h_idx_ary[h_idx_pos] = -1; //last element is always -1
+ if ((i - tempstart > 4) && (i - tempstart < thinnest)) {
+ thinnest = i - tempstart;
+ linestart = tempstart;
+ linestop = i;
+ }
+ } else if(diff[i] > highest) {
+ highest = diff[i];
+ h_idx_ary[h_idx_pos] = i; //insert to array, clobbers
+ tempstart = i;
+ }
+ break;
+
+ case FIND_TROUGH:
+ //serial.printf("Trough case in processFn\r\n");
+ if( diff[i] >= -THRESH ) {
+ state = NEUTRAL;
+ l_idx_pos++;
+ l_idx_ary[l_idx_pos] = -1; //last element is always -1
+ if ((i - tempstart > 4) && (i - tempstart < thinnest)) {
+ thinnest = i - tempstart;
+ linestart = tempstart;
+ linestop = i;
+ }
+ } else if(diff[i] < lowest) {
+ lowest = diff[i];
+ l_idx_ary[l_idx_pos] = i; //insert to array, clobbers
+ tempstart = i;
+ }
+ break;
+
+ default:
+ //exit case if exception happened
+ serial.printf("Default case in processFn\r\n");
+ return out;
+ }
+ }
+ //AGC, simple proportional controller
+ total = total / frameLen;
+ exposureChange = ((float)(MEAN_REF - total)) * CAM_CTRL_GAIN;
+ *exposure += (int)exposureChange;
+ if(*exposure < 0)
+ *exposure = 0;
+ else if(*exposure > UPDATE_RATE)
+ *exposure = UPDATE_RATE;
+ }
+
+ l_walker = l_idx_ary;
+ h_walker = h_idx_ary;
+
+ /*while(*l_walker != -1 && *h_walker != -1){
+ //evaluate out and advance if line is white on black and returns center of smallest white band
+ //if interval is black on white, advance the pointer for the peak array
+ //width needs to be larger than 4 pixels wide to be a good read.
+
+ if(*h_walker > *l_walker && (*h_walker - *l_walker) < out_width && (*h_walker - *l_walker) > 3){
+ out_width = *h_walker - *l_walker;
+ out = ((float)(*h_walker + *l_walker)) / (2.0 * (float)frameLen); //0.5 is center
+ l_walker++;
+ }
+ h_walker++;
+
+ //serial.printf("%d %d\r\n", *h_walker, *l_walker);
+ //serial.printf("%.2f\r\n", out);
+ }*/
+
+ if (linestart != -1) {
+ out = ((float)(linestart + linestop)) / (2.0 * (float)frameLen);
+ } else {
+ out = -1;
+ }
return out;
}
//call after integration time is done, returns index of array line is expected to be at and new exposure time
-float getLinePos(AnalogIn cam,
- DigitalOut *camSi,
- DigitalOut *camClk,
- int *exposure,
- telemetry::NumericArray<uint16_t, NUM_PIX> &tele_linescan){//,
- //telemetry::NumericArray<uint16_t, NUM_PIX> &tele_linescan_diff){
+float getLinePos(AnalogIn cam,
+ DigitalOut *camSi,
+ DigitalOut *camClk,
+ int *exposure,
+ telemetry::NumericArray<uint16_t, NUM_PIX> &tele_linescan,
+ telemetry::NumericArray<uint16_t, NUM_PIX> &tele_linescan_diff)
+{
uint16_t lineAry[NUM_PIX];
float position;
-
- //read
+
+ //read
startRead(camSi, camClk);
- for(int i = 0; i < NUM_PIX; i++){
+ for(int i = 0; i < NUM_PIX; i++) {
lineAry[i] = read1Bit(cam, camClk);
tele_linescan[i] = lineAry[i];
}
-
+
//process line scan data
- position = processFn(lineAry, NUM_PIX, exposure);//, tele_linescan_diff);
- //serial.printf("%.2f\r\n", position);
-
+ position = processFn(lineAry, NUM_PIX, exposure, tele_linescan_diff);
+
return position;
}
-/*
-sample call (handler thread):
+float getLinePos2(AnalogIn cam,
+ DigitalOut *camSi,
+ DigitalOut *camClk,
+ int *exposure)
+{
+ uint16_t lineAry[NUM_PIX];
+ float position;
-while(1){
- linePos = getLinePos(cameraIn1, si, clk, &exposureTime); //volatile linePos, replace with steering angle and read 2 cameras?
- Thread::wait(exposureTime); //sleep for 14 us
+ //read
+ startRead(camSi, camClk);
+ for(int i = 0; i < NUM_PIX; i++) {
+ lineAry[i] = read1Bit(cam, camClk);
+ }
+
+ //process line scan data
+ position = processFn2(lineAry, NUM_PIX, exposure);//, tele_linescan_diff);
+
+ return position;
}
-*/