First Steps: Drivers for Sonar & Mapping

Author’s note:

This is the sixth  in a series of articles outlining the steps we have taken to build our robot. Both Benjamin and I are newbies (but becoming more expert) at this and thus this article will be written for newbies. These articles are focused on a robot that will compete in the Fire Fighting contest, be able to roam around the house and will be a fun learning tool. The 68HC912B32 is our processor, so articles will be on interfacing to the B32 to various mechanical devices and sensors using SBasic as our programming language.

Previous articles:

I have created libraries for all my basic routines that will be used over and over again.  It uses three of the libraries, first is the Rinit.lib which has all my initialization routines and second is the Rsonar.lib which is the actual drives for the module and last a interrupt routine.

Below is the section that initializes the MCU hardware.  Again to make sense of this you need to read Kevin's article.  I am not going to try to explain it here.  But you can see how I implemented it and boiled it down to what I needed.  The page numbers in the comments are page numbers of the 68HC12B32 manual so I could refer to the page if I needed to make changes.

    '------- timer port setup ----------------
' Setup channels 0 and 1 and 7 to be TOC channels. (servos)
' Setup channels 3 to be TIC channels. (sonar)
    pokeb tios,%10000011    'page 74
    pokeb tctl4,%01000000         'channel 3 rising edge (page 76)

    pokeb tmsk2,%10110010    '32.768 ms so can work with servo, 2000 counts/ms
                    'b7 TOI set Timer Overflow Flag TOF allowed

' Turn on the timer, disable it during background debug mode (page 75)
    pokeb tscr, %10100000

You just need to run this once at the beginning of your program and it will be ready to go.

Below is the library (Rsonar.lib) that I include in all my programs.

You would use this subroutine like this:

distance=usr(read_sonar,right_eye)

Distance is any variable that will be loaded with the distance of the object.  This is the raw number.  Divided by 30 gets you the distance in 10ths of an inch which is what I usually do now. 

read_sonar is the name of the routine.

right_eye is a predefined constant which tells the routine what sonar to look at.  For this routine 1-5 selects a transducer.

Below are the variables that I use in this routine unique to this routine.   If a variable is used only in a routine then I will declare it just before the routine.

'routines to do polaroid sonar
' based on timers1.bas
declare waitsonar            'have it wait to fire again
declare sonar_time(5)    'last recorded time so can go to 6 inches
declare last_relay        'what last command was to tell if wait needed
declare s_ok

read_sonar:        'use is usr(read_sonar,x) where x is sonar number
    'pick(0)    will hold the sonar number to look at

Next resets the sonar module to fire a signal.  It also does this at the end of the program.
    'cleared at end of routine, but cleared again just in case
    pokeb PORTB, peekb(PORTB) and %11111001 ; clear PB1 (init) and PB2 (Binh)

Sonar_time holds the last reading for this transducer.  Need to know this to decide if it is close and allows it to listen early for a signal.
    sonar_total_time=sonar_time(pick(0))

Now we close the relay for that transducer, this is so when the module fires it will go to the right transducer.  Case 0 is to make no change.
    select pick(0)
        case 0             'do not turn off or on any relays, they are preset
        'do nothing
        endcase
        case 1             'right eye
            pokeb PORTB, peekb(PORTB) or %1000 ; set PB3 to turn on relay 1
            pokeb PORTB, peekb(PORTB) and %00001111 ; clear other relays
        endcase
        case 2             'left eye
            pokeb PORTB, peekb(PORTB) or %10000 ; set PB4 to turn on relay 2
            pokeb PORTB, peekb(PORTB) and %00010111 ; clear other relays
        endcase
        case 3             'right ear
            pokeb PORTB, peekb(PORTB) or %100000 ; set PB5 to turn on relay 3
            pokeb PORTB, peekb(PORTB) and %00100111 ; clear other relays
        endcase
        case 4             'left ear
            pokeb PORTB, peekb(PORTB) or %1000000 ; set PB6 to turn on relay 4
            pokeb PORTB, peekb(PORTB) and %01000111 ; clear other relays
        endcase
        case 5             'back
            pokeb PORTB, peekb(PORTB) or %10000000 ; set PB7 to turn on relay 5
            pokeb PORTB, peekb(PORTB) and %10000111 ; clear other relays
        endcase
        case 6         'relay 1 and 2 on to work together, all other relays off       Note I never use this where I fire two at same time.
            pokeb PORTB, peekb(PORTB) or %11000 ; set PB4 to turn on relay 2
            pokeb PORTB, peekb(PORTB) and %00011111 ; clear all relays
        endcase
    endselect

If last relay used is different then delay 3 ms to make sure it has time to close.  Relays should close within 1 ms.
    if last_relay<>pick(0)

This routine below is a general routine that gives me timing delays etc.   It is good to about 1ms accurazy.
        wait=intcnt
        do loop until usr(compare_intcnt,wait)>3     'delay 3 ms to let relay latch
        last_relay=pick(0)
    endif

This will make sure that the module does not fire any faster than every 11 ms.   If it fires faster than that it does not have time to recharge etc.  The specs call for 50 ms but I tested it and 11 ms worked.
    'have it not fire any sooner than 11ms after last firing
    if intcnt>=waitsonar    'then must wait until fire again
        do loop until usr(compare_intcnt,waitsonar)>11    'wait to recharge
    endif

Fire the module and start the timing.
    pokeb PORTB, peekb(PORTB) or %10 ; set PB1 to start sonar
    sonar_start_timing=peek(tcnt)         'get current timing
    tof_count=0

If the last time the reading was close (below 1.3 ft) then suppress the inhibit (Binh).  Need to wait .7 ms before doing that.  I had to also test if the counter overflowed, if it did needed to take that into consideration.
    if sonar_total_time<6000 '5200 was used before 'getting close or below 1.3 ft turn on inhibit
        temp_lib=peek(tcnt)+1450 '200         'delay .725 ms before reading
        if temp_lib<1450                         'must have overflowed
            do loop until peek(tcnt)<*temp_lib    'wait until it over flows
        endif
        do loop until peek(tcnt)>*temp_lib
        pokeb portb, peekb(portb) or %100     'Binh go high, now can take reading
    endif

Now loop until flag goes high.  tof_count is set by an interrupt evey time this counter overflows.  I use that to test if it has overflowed greater than 2, if so then something failed.  If this routine was not there then if something failed then it would be in an infinite loop.
    s_ok=0
    do
        if peekb(tflg1) and $08<>0     'read found
            s_ok=1
        endif
        if tof_count>2    'something happened, will get us out of loop
            s_ok=1
        endif
    loop until s_ok=1    'waituntil flag goes high

Now ready to calculate how far the object is.
    sonar_end_timing=peek(tc3)
    sonar_overflow=tof_count    'number times overflowed

    pokeb tflg1,peekb(tflg1) OR $08    'reset flag

The calculations are very convoluted.  But basically the time is end time - start time.  Divide that by 303 gets you inches or by 30 to get you 10ths of an inch.  I load sonar_inches with inches and sonar_inches10 with 10ths of inch.  I really don't need or use the sonar_inches10 much, will change this someday.
    sonar_total_time=sonar_end_timing-sonar_start_timing     'takes in account of roll over
    sonar_inches=sonar_total_time/303    'sound travels at 13.2 in/ms
                                    '2000 counts/ms or 303 counts per inch away (round trip)
    sonar_inches10=10*(sonar_total_time-(sonar_inches*303))/303         'convert for 1/10 inch

If there was an overflow then need to add 216 inches for that overflow.
    if sonar_overflow>1    'double overflow must be over 16ft
        sonar_inches=sonar_inches+216     '216.3=65k/303
    endif

Reset conditions and save things for next time.
    pokeb PORTB, peekb(PORTB) and %11111001 ; clear PB1 (init) and PB2 (Binh)
    sonar_time(pick(0))=sonar_total_time
    waitsonar=intcnt    'wil be used to make sure not firing too soon next time
    drop 1
return sonar_total_time

I guess I should say something about the timer overflow routine.  When the counter register overflows it fires off a interrupt and the interrupt goes here and I increment the counter. 

interrupt $f7de             'timer overflow, page 77
    pokeb tflg2, $80    'clear TOF flag , timer overflow flag
    tof_count=tof_count+1
end

Now comes the fun part.  Actually using the routine.   I wrote an Sbasic program that dumped the sonar readings and then wrote a PC program that read them in real time using the Rs232 connection and draws a map by the readings.  Now that I have my radio modem going I can do this remotely.

This is a picture of my building area in its early days.  I was too lazy and cramped for time to take another picture.  It is not this clean, has a different table and the robot has changed a lot.  But it will give you an idea of the area the robot is in for the picture below. There are selves on the left and right (not shown here) and window in the back.

Please forgive the bigness of this picture, smaller and we would have lost the clarity I wanted.  I did this picture for someone else so had it available.   You can see the rough outline of the room and even see me sitting at the table as it takes my picture.  The lines that are broken at the top are from the angle the signal hits the window.  It does fine until the signal no longer returns because the angle is too great.  It then bounces off another corner of window and the signal comes back but takes longer and thus the disconnected line.  But the basic shape of the room is recognizable. Not shown here is my latest version that color codes each traducer and correlates the reading from the heat sensor.  The lines at the table would show up red and blue where it hits my arms and senses heat.  Works great!

Well enough for now.  I am working on the bot for the Fire Fighting contest.  Everything went to pot last night.  And now even the MCU doesn't respond and I think it has blown for some unknown reason.  So welcome to robotics.

I may write on the programs that made this picture for next time.  If you are interested let me know.