The latest test version of firmware builds on the addition a few days ago of the Lisp interpreter - see
http://www.surveyor.com/cgi-bin/yabb2/YaBB.pl?num=1226608954 This has been a long time coming - I finally started to create a built-in function for estimating straight-ahead range using the laser pointers. This is not a simple problem, as the intensity and color of the beam vary with different reflecting surfaces, plus there are numerous other artifacts due to reflections.
The approach I am taking is to stop the motors, wait 250ms, grab a reference frame for differencing, turn on the lasers for 500ms, compute a difference, then search for blobs. I am using some dynamic thresholding, and that seems like a good approach, but there are still artifacts which can cause problems. Though the return value isn't calibrated (it's meant to be inches), this seems to work reasonably well up to approx 4 feet range, and then the results are inconsistent.
Note that alignment of the lasers is fairly critical. Some work is still needed on filtering false triggers. This is just a test version - it dumps lots of diagnostic data from the 'R' command, and I will continue to work on this over the next 1-2 days, but comments / suggestions / test results are welcome !
code build here -
http://www.surveyor.com/blackfin/srv-blackfin-112608.zip and SVN
svn://srv.transterpreter.org Here are the code changes -
in colors.h
#define MAX_BLOBS 16
in colors.c, in the vblob() function, change:
for (xx=0; xx<maxx; xx++) {
to
for (xx=0; xx<maxx; xx+=2) {
in main.c, add:
case 'R': // show laser range
show_laser_range();
break;
in srv.c, add:
/* Show laser range
Serial protocol char: R */
void show_laser_range() {
if (!silent_console)
printf("##Range = %d\n\r", laser_range());
}
/* Compute laser range
turn off lasers
stop motors
delay 250ms
grab reference frame
turn on lasers
delay 500ms
grab frame
compute difference
set color #16
use adaptive threshold to filter blobs
range (inches) = imgWidth*2 / vblob(16)
turn lasers off
*/
unsigned int laser_range() {
int ix, iy;
*pPORTHIO &= 0xFC7F; // lasers off
if (pwm1_mode == PWM_PWM) setPWM(0, 0); // motors off
else setPPM1(50, 50);
delayMS(250);
move_image((unsigned char *)DMA_BUF1, (unsigned char *)DMA_BUF2, // grab reference frame
(unsigned char *)FRAME_BUF2, imgWidth, imgHeight);
*pPORTHIO |= 0x0380; // lasers on
delayMS(500);
move_image((unsigned char *)DMA_BUF1, (unsigned char *)DMA_BUF2, // grab new frame
(unsigned char *)FRAME_BUF, imgWidth, imgHeight);
compute_frame_diff((unsigned char *)FRAME_BUF, // compute difference
(unsigned char *)FRAME_BUF2, imgWidth, imgHeight);
*pPORTHIO &= 0xFC7F; // lasers off
umin[16] = 80; umax[16]=144; vmin[16] = 100; vmax[16]=255; ymax[16]=255; // set color bin #16
for(ymin[16]=100; ymin[16]>0; ymin[16]-=5) {
vblob((unsigned char *)FRAME_BUF, 16); // use the brightest blob
printf("blobs: ymin=%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n\r",
ymin[16],
blobcnt[0], blobx1[0], blobx2[0], bloby1[0], bloby2[0],
blobcnt[1], blobx1[1], blobx2[1], bloby1[1], bloby2[1],
blobcnt[2], blobx1[2], blobx2[2], bloby1[2], bloby2[2]);
ix = blobcnt[0];
if (ix < 3)
continue;
if ((bloby2[0]-bloby1[0]) < ix)
break;
ix = 0; // in case loop falls through
}
if (ix) {
if (blobx1[0] > (imgWidth/2))
return (imgWidth*2) / (blobx2[0] - (imgWidth/2)); // right blob
if (blobx2[0] < (imgWidth/2))
return (imgWidth*2) / ((imgWidth/2) - blobx1[0]); // left blob
if ((blobx1[0] < (imgWidth/2)) && (blobx2[0] > (imgWidth/2)))
return (imgWidth*2) / ((blobx2[0]-blobx1[0])/2); // overlap blob
}
return (9999);
}