Welcome, Guest. Please Login.
Surveyor Corporation Surveyor SRV-1
Home Help Search Login

Surveyor Robotics Forum

Welcome to the user support forum for Surveyor SRV-1 robots, SRV-1 robot controllers and SVS stereo vision systems. To register for this forum, please send an email to support@surveyor.com which includes your desired forum user name, your registration email address, and a brief explanation of why you wish to join, and we will create a forum account for you.

Please note that there is a Search button in the forum toolbar for forum topics. Another effective search method for the entire surveyor.com site is to use Google, e.g. "xyz site://www.surveyor.com" where "xyz" is the search topic.



Pages: 1
Send Topic Print
srv-blackfin-112608 - new Lisp + laser ranging (Read 7216 times)
admin
YaBB Administrator
*****




Posts: 3676
srv-blackfin-112608 - new Lisp + laser ranging
11/16/08 at 9:20am
 
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);
}
 
Back to top
 
« Last Edit: 11/26/08 at 8:30am by admin »  

SRV-1 Development Team
Surveyor Corporation
Email WWW   IP Logged
Lefteris
Senior Member
****




Posts: 274
Re: using the laser pointers for ranging
Reply #1 - 11/16/08 at 10:35am
 
Certainly a very interesting way to use the lasers. As you might remember I have been using the lasers for real time obstacle detection (almost 95% accurate) but not for ranging with a value. I guess a neural network could be trained to do that.  
 
Since I needed it real time I just used color segmenntation with these values :  
 
Code:
if((cY >= 250 && cY <=255 )&&
					(cU >= 120 && cU <=130)&&//if it's laser
					(cV >= 120 && cV <=130)) 


 
My values take only the bright , very bright spot of the laser beam, so I can be sure it is the laser and not something else. I can see that the color bin you are using allows for many more values to get recorded? Then you are checking them all to see which is the brigthest and return that as a laser?
 
Just asking to see if I got it right. Any chance such a routine could work while moving?
Back to top
 
 

http://www.realintelligence.net - The real intelligence project || http://lefteris.realintelligence.net - My blog
  IP Logged
admin
YaBB Administrator
*****




Posts: 3676
Re: using the laser pointers for ranging
Reply #2 - 11/16/08 at 10:47am
 
Filtering based on a very bright spot might work while moving for close range - I think the dynamic threshold approach would be useful for this.
    for(ymin[16]=250; ymin[16]>0; ymin[16]-=5) {
       vblob((unsigned char *)FRAME_BUF, 16);  // use the brightest blob
I will have to give this a try.
 
The color bin ranges I'm using are different because of frame differencing.  Without differencing, V should probably be in the range of 150-180, and U in the range of 116-140
 
For computing a range, alignment of the lasers is somewhat critical.  We might actually get better results by using a single laser.
Back to top
 
« Last Edit: 11/16/08 at 10:51am by admin »  

SRV-1 Development Team
Surveyor Corporation
Email WWW   IP Logged
Lefteris
Senior Member
****




Posts: 274
Re: using the laser pointers for ranging
Reply #3 - 11/16/08 at 11:44am
 
Frame differencing is what exactly? I saw something like that in the original firmware but I just commented out since I did not quite understand it.
 
But it sounds computationally expensive, would it work while moving?  
Back to top
 
 

http://www.realintelligence.net - The real intelligence project || http://lefteris.realintelligence.net - My blog
  IP Logged
admin
YaBB Administrator
*****




Posts: 3676
Re: using the laser pointers for ranging
Reply #4 - 11/16/08 at 11:57am
 
It is computationally cheap, but does not work if there is any motion.  Essentially, it captures a reference frame and then computes the difference on a pixel-by-pixel basis between the reference frame and a newly captured frame.  This is a standard technique for motion detection, so it doesn't work if the camera is moving.  However, it works quite well for locating the laser light if the camera is fixed because everything else in the frame has been subtracted out.
 
Back to top
 
« Last Edit: 11/16/08 at 11:58am by admin »  

SRV-1 Development Team
Surveyor Corporation
Email WWW   IP Logged
Lefteris
Senior Member
****




Posts: 274
Re: using the laser pointers for ranging
Reply #5 - 11/16/08 at 11:59am
 
oh I see , smart way to find the laser fast and accurately. Would work like a charm if you just stop for a moment and check!
Back to top
 
 

http://www.realintelligence.net - The real intelligence project || http://lefteris.realintelligence.net - My blog
  IP Logged
admin
YaBB Administrator
*****




Posts: 3676
Re: using the laser pointers for ranging
Reply #6 - 11/16/08 at 12:03pm
 
The motor stop is built into the laser_range() function.  Hopefully 250ms is a long enough wait for the robot to stop.  The various thresholds will need to be optimized.
 
Back to top
 
« Last Edit: 11/16/08 at 12:04pm by admin »  

SRV-1 Development Team
Surveyor Corporation
Email WWW   IP Logged
admin
YaBB Administrator
*****




Posts: 3676
Re: srv-blackfin-112608 - new Lisp + laser ranging
Reply #7 - 11/21/08 at 1:59pm
 
Another update has been posted.   Left and right lasers are sampled separately, which is yielding much better results.  The 'R' range command diagnostic output is currently silenced, but we added a command 'r' to dump the range diagnostic data.  
 
Besides addition of the (range) command to Lisp, we now have (color) and (blob) in Lisp.  I'll post a tutorial shortly, but an example calling sequence is  
 
> (color 1 '(50 250 100 140 150 250))
(50 250 100 140 150 250)
 
this will set color 1 to ymin 50, ymax 250, umin 100, umax 140, vmin 150, vmax 250 - which is a range of reds
 
> (blob 1)
(1546 145 118)
 
this will display the number of pixels in the largest blob along with the x and y midpoints
 
Here's the code -
    http://www.surveyor.com/blackfin/srv-blackfin-112608.zip
or
    svn http://srv.transterpreter.org/
Back to top
 
« Last Edit: 11/26/08 at 8:31am by admin »  

SRV-1 Development Team
Surveyor Corporation
Email WWW   IP Logged
admin
YaBB Administrator
*****




Posts: 3676
Re: srv-blackfin-112608 - new Lisp + laser ranging
Reply #8 - 11/22/08 at 12:40pm
 
I posted code samples using the laser_range() function from C and Lisp.  Code is here -
 
==================================
 
/* C version */
main()
{
    int x;
    char ch;
 
    ch = 0;
    while (ch == 0) {
       x = range();   /* user laser pointer ranging */
       print("range = " x);
       if (x < 30) {
           motors(-50 , 50);
       } else {
           motors(50, 50);
       }
       delay(500);
       ch = input();  /* continue until any console input */
    }  
    motors(0, 0);
}
 
====================================
 
; Lisp version
 
(define (stop) (robot 5))
(define (left) (robot 4))
(define (right) (robot 6))
(define (forward)
    (begin
       (robot 8)
       (delay 500)
       (robot 5)
    )
)
(define (back)
    (begin
       (robot 2)
       (delay 500)
       (robot 5)
    )
)
 
(define (branch)     ;if range<30 go right else go forward
    (begin
       (set! x (range))
       (print x)
       (if (< x 30) (right) (forward))  
    )
)
 
(while (not (signal))
    (branch)
)
 
==================================
 
I'll be adding more code samples shortly using the blob and color functions.  Note that while the Lisp version is longer, most of the code is creating some library functions that can easily be used later.
 
Back to top
 
« Last Edit: 11/26/08 at 8:42am by admin »  

SRV-1 Development Team
Surveyor Corporation
Email WWW   IP Logged
admin
YaBB Administrator
*****




Posts: 3676
Re: srv-blackfin-112608 - new Lisp + laser ranging
Reply #9 - 11/26/08 at 8:41am
 
Made just a few more minor tweaks and bug fixes, so latest version is  
    http://www.surveyor.com/blackfin/srv-blackfin-112608.zip
 
There was a bug in atoi() which was causing problems with negative number inputs to Lisp, and a divide-by-zero error in laser_range().  Also cleaned up more compiler warnings and fixed some dependency issues in Makefile.
 
Next on the agenda is to integrate neural net based image pattern training and matching functions, and add on-board SVS depth map composer using the edge + color segmentation functions.  Also, we need to create a tutorial for using the Lisp interpreter with the various new functions.
 
Back to top
 
 

SRV-1 Development Team
Surveyor Corporation
Email WWW   IP Logged
Pages: 1
Send Topic Print