Doug Leppard (DLeppard@CCCI.Org
Benjamin Leppard (Benjamin@Leppard.Com)
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.
"Setting the Stage for Building a Robot," we set goals for the robot and did research. From the goals and research we will make fundamental decisions on what our robot will be like.
"Choosing the CPU, Language and Basic Shape," using our goals we choose what microprocessor, language and what the fundamental shape of the robot.
"Motorolas S-Record" looking at the S-Record format the Motorola uses to load data and programs.
"Booting up the 68HC912B32 for the first time" goes into detail how to boot the B32 up and run a program.
"State of the Union" gives an overview of what state the robot was in August 1999.
"Multiplexing Five Sonar Modules" goes into how tos and whys of putting five Polaroid sonar transducers and wiring them together.
In my previous article I described wiring five Polaroid transducers together hooked to one sonar module giving a possible 360 degree view of your surroundings. This article describes the software used to interface those modules to the MCU. You need to read my previous article and the articles I suggested for background reading. Without understanding those this will not be as helpful.
The sonar driver is based on Kevin Ross article "The 68HC12 Timer Module". This is a must read. Without Kevin's article I could not have gotten my sonar working. Thanks Kevin and SRS for all the articles that I can build on.
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 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 is the actual code. I will put in additional comments in bold in the source.
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
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.
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.
case 0 'do not turn off or on any relays, they are preset
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
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
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
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
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
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
If last relay used is different then delay 3 ms to make sure it has time to
close. Relays should close within 1 ms.
This routine below is a general routine that gives me timing delays etc.
It is good to about 1ms accurazy.
do loop until usr(compare_intcnt,wait)>3 'delay 3 ms to let relay latch
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
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
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
do loop until peek(tcnt)>*temp_lib
pokeb portb, peekb(portb) or %100 'Binh go high, now can take reading
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.
if peekb(tflg1) and $08<>0 'read found
if tof_count>2 'something happened, will get us out of loop
loop until s_ok=1 'waituntil flag goes high
Now ready to calculate how far the object is.
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
Reset conditions and save things for next time.
pokeb PORTB, peekb(PORTB) and %11111001 ; clear PB1 (init) and PB2 (Binh)
waitsonar=intcnt 'wil be used to make sure not firing too soon next 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.
'timer overflow, page 77
pokeb tflg2, $80 'clear TOF flag , timer overflow flag
If you have a boot loader on your B32 like I do, then you have to redirect the interrupt to this routine. See Kevin's article "Interrupts on the 68HC12", that will help you understand things. Also see my article "Booting up the 68HC912B32 for the first time" that explains the boot loader a little, also in that article see the memory map document I made.
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.