This is a sleazy trick. You wire up the 8 trigger lines to one byte
of the Stamp's IO word, and the 8 input lines to the other. Then you
trigger all the sensors, and read bytes -- reading all 8 signals at a
time. You end up with a column of bits, one per sensor, that you can
count to determine the range.
Due to the memory limitations of the stamp, you have to sacrifice
resolution for speed. I was dividing the range into about 14
segments.
The following code does a lot of processing, and ends up with a
"joystick" recommendation that gets sent to the stamp in the
IFI robot controller via the debug port, another sleazy trick.
It also triggers the sensors 4 at a time to minimize crosstalk
between sides of the robot (my 8 sensors were in pairs for
redundancy).
See the code for my PWM reader for
more background on the debug port trick and what the heck
"fracBase" is.
' PROGRAM: BS2SX Sonar Parallel Ping Test
' Written by: Robert J Woodhead
' Date: 31 MAR 02
'
' Define BS2-SX Project Files
'
' {$STAMP BS2SX}
' Pings all 8 devices in parallel. Computes a closeness value
' from 0-15 for each sensor. Then figures out how to twist and
' translate the bot to line up on the target (it's the same if
' attacking or defending!), as well as a movement translation
' suggestion if defending. Sends a $FF,twist,move,checksum
' packet off when it's all done
fracBase con 250 ' fractional range base, same as driver code
fracHalf con 125 ' half of fracBase
fracBaseP1 con fracBase+1 ' utility constants
fracHalfP1 con fracHalf+1
filterCoef con 50 ' 20% of fracBase
filterInv con fracBase-filterCoef ' 80% of fracBase
' Each sensor is connected to the BS2 via 2 lines. Sensor x
' uses line x as trigger and x+8 as echo. So the 8 trigger
' lines are outl and the 8 input lines are inh!
s0 var nib
s1 var nib
s2 var nib
s3 var nib
s4 var nib
s5 var nib
s6 var nib
s7 var nib
s var s0 ' sonar unit 0-15 values
plen con 14 ' length of ping array 0-plen
pmax con 15 ' maximum value a sonar unit can return
p00 var byte
p01 var byte
p02 var byte
p03 var byte
p04 var byte
p05 var byte
p06 var byte
p07 var byte
p08 var byte
p09 var byte
p10 var byte
p11 var byte
p12 var byte
p13 var byte
p14 var byte
p var p00 ' ping array
i var byte ' loop variable
' variable overlay. After we have computed the s() ranges, we don't need
' p00 through p14 anymore. So we can reuse the variable space. However
' if we are doing the alternating left/right pinging (to deal with
' cross reflections, then should retain the lf/rf/lr/rr values
' between loops. We could recompute them but it's faster not to.
'
' Sensor layout is as follows:
'
' 6 7 2 3
' Left Right
' 4 5 0 1
lbits con %11110000 ' bitmask for left sensors
rbits con %00001111 ' bitmask for right sensors
lf var nib ' left front common sensor value
rf var nib ' right front value
lr var nib ' left rear
rr var nib ' right rear
ftwist var p04 ' front twist
rtwist var p05 ' rear twist
fmove var p06 ' front move
rmove var p07 ' rear move
fmoves var p08 ' front move min sensor
rmoves var p09 ' rear move min sensor
twist var p10 ' final twist suggestion
move var p11 ' final move suggestion
csum var p12 ' checksum
twist_filt var byte ' filtered twist value
move_filt var byte ' filtered move value
' finally, we need to keep track of which half we are pinging
half var bit
' initialize all the trigger lines and set them low. Set the initial
' distances of the sensors to timeout level. If we actually do this
' for real, we'll reshuffle the inputs and outputs to all be in a
' single byte
for i = 0 to 7
low i
input i+8
s(i) = 14
next
' initialize filtered output values
twist_filt = fracHalf
move_filt = fracHalf
' initialize initial quadrant values
lf = pmax
rf = pmax
lr = pmax
rr = pmax
' start with one half
half = 1
debug 0 ' remove from final code
' Main loop. Ping all the sensors, read in their results
main:
half = half ^ 1 ' invert half
lookup half,[lbits,rbits],i ' figure out what sensors to ping (half: 0=left, 1=right)
outl = i ' trigger the sensors
outl = i ' trigger the sensors
outl = $00 ' end of pulse
for i = 0 to 200 ' wait for a sensor to go high
p00 = inh ' but time out eventually
if p00 <> $00 then ping_rec:
next
ping_rec:
' note, since plan is not to return raw sensor values anymore,
' we can do more complex sensing if need be, with finer
' resolution
p00 = p00 | inh | inh | inh ' we want to capture the first bit on operational sensors
' Read in the range bytes. The number of inh's we read determines
' the range for a particular byte
p01 = inh | inh | inh | inh | inh | inh | inh | inh | inh | inh | inh
p02 = inh | inh | inh | inh | inh | inh | inh | inh | inh | inh | inh
p03 = inh | inh | inh | inh | inh | inh | inh | inh | inh | inh | inh
p04 = inh | inh | inh | inh | inh | inh | inh | inh | inh | inh | inh
p05 = inh | inh | inh | inh | inh | inh | inh | inh | inh | inh | inh
p06 = inh | inh | inh | inh | inh | inh | inh | inh | inh | inh | inh
p07 = inh | inh | inh | inh | inh | inh | inh | inh | inh | inh | inh
p08 = inh | inh | inh | inh | inh | inh | inh | inh | inh | inh | inh
p09 = inh | inh | inh | inh | inh | inh | inh | inh | inh | inh | inh
p10 = inh | inh | inh | inh | inh | inh | inh | inh | inh | inh | inh
p11 = inh | inh | inh | inh | inh | inh | inh | inh | inh | inh | inh
p12 = inh | inh | inh | inh | inh | inh | inh | inh | inh | inh | inh
p13 = inh | inh | inh | inh | inh | inh | inh | inh | inh | inh | inh
p14 = inh | inh | inh | inh | inh | inh | inh | inh | inh | inh | inh
ping_count:
' debug 1
' for i = 0 to 14
' debug bin8 p(i),11,cr
' next
'
' debug cr
' we only need calculate the ranges for the side we just pinged
if half = 0 then calc_left:
s0 = p00.bit0 + p01.bit0 + p02.bit0 + p03.bit0 + p04.bit0 + p05.bit0 + p06.bit0 + p07.bit0 + p08.bit0 + p09.bit0 + p10.bit0 + p11.bit0 + p12.bit0 + p13.bit0 + p14.bit0
s1 = p00.bit1 + p01.bit1 + p02.bit1 + p03.bit1 + p04.bit1 + p05.bit1 + p06.bit1 + p07.bit1 + p08.bit1 + p09.bit1 + p10.bit1 + p11.bit1 + p12.bit1 + p13.bit1 + p14.bit1
s2 = p00.bit2 + p01.bit2 + p02.bit2 + p03.bit2 + p04.bit2 + p05.bit2 + p06.bit2 + p07.bit2 + p08.bit2 + p09.bit2 + p10.bit2 + p11.bit2 + p12.bit2 + p13.bit2 + p14.bit2
s3 = p00.bit3 + p01.bit3 + p02.bit3 + p03.bit3 + p04.bit3 + p05.bit3 + p06.bit3 + p07.bit3 + p08.bit3 + p09.bit3 + p10.bit3 + p11.bit3 + p12.bit3 + p13.bit3 + p14.bit3
' if a sensor has become disabled, it'll read distance = 0.
' in such a case, set to maximum distance value. Inline
' for speed.
lookup s0,[pmax],s0
lookup s1,[pmax],s1
lookup s2,[pmax],s2
lookup s3,[pmax],s3
' for each of the pairs of sensors, calculate the minimum distance
' returned
rf = s2 max s3
rr = s0 max s1
goto ping_calc:
calc_left:
' ditto for the lefthand side
s4 = p00.bit4 + p01.bit4 + p02.bit4 + p03.bit4 + p04.bit4 + p05.bit4 + p06.bit4 + p07.bit4 + p08.bit4 + p09.bit4 + p10.bit4 + p11.bit4 + p12.bit4 + p13.bit4 + p14.bit4
s5 = p00.bit5 + p01.bit5 + p02.bit5 + p03.bit5 + p04.bit5 + p05.bit5 + p06.bit5 + p07.bit5 + p08.bit5 + p09.bit5 + p10.bit5 + p11.bit5 + p12.bit5 + p13.bit5 + p14.bit5
s6 = p00.bit6 + p01.bit6 + p02.bit6 + p03.bit6 + p04.bit6 + p05.bit6 + p06.bit6 + p07.bit6 + p08.bit6 + p09.bit6 + p10.bit6 + p11.bit6 + p12.bit6 + p13.bit6 + p14.bit6
s7 = p00.bit7 + p01.bit7 + p02.bit7 + p03.bit7 + p04.bit7 + p05.bit7 + p06.bit7 + p07.bit7 + p08.bit7 + p09.bit7 + p10.bit7 + p11.bit7 + p12.bit7 + p13.bit7 + p14.bit7
lookup s4,[pmax],s4
lookup s5,[pmax],s5
lookup s6,[pmax],s6
lookup s7,[pmax],s7
lf = s6 max s7
lr = s4 max s5
ping_calc:
' at this point, the p array is no longer needed, and we
' can reuse the variable space.
' the above variables are now in a range from 1-15. They cannot
' be zero, so we'll have no divide by zero problems below. Next
' for the front and rear pairs, determine how hard we ought to
' twist in order to line up the robot. We generate a number from
' 0..2pmax-2. 0 means full hard counterclockwise, pmax-1 =
' balanced, no twist, and 2pmax-2 means full hard clockwise
lookup ((pmax+lf)-rf)-1,[000,008,017,026,035,044,053,062,071,080,089,098,107,116,125,133,142,151,160,169,178,187,196,205,214,223,232,241,250],ftwist
lookup ((pmax+rr)-lr)-1,[000,008,017,026,035,044,053,062,071,080,089,098,107,116,125,133,142,151,160,169,178,187,196,205,214,223,232,241,250],rtwist
' if twists are in the same direction, use the larger magnitude.
' if in opposite directions, subtract smaller from larger
if (ftwist <= fracHalf) and (rtwist <= fracHalf) then cclockwise:
if (ftwist >= fracHalf) and (rtwist >= fracHalf) then clockwise:
twist = ftwist + rtwist - fracHalf ' our strange math makes this work
goto end_twist:
cclockwise:
twist = ftwist max rtwist ' use smaller of the two
goto end_twist:
clockwise
twist = ftwist min rtwist ' use larger of the two
end_twist:
' figure out the retreat speed. use the minimum distance detected
' on front and rear as an index to determining the proper speed.
' the closer the enemy is, the faster we retreat.
fmoves = lf max rf ' pick the closest sensor
' convert to speed. since these are front sensors, we retreat!
' fmove will never be 0, so first index is doubled. also make
' a little bit of a deadzone around the highest readings, it
' will reduce fluttering a bit
lookup fmoves,[000,000,009,019,028,038,048,057,067,076,086,096,105,115,125,125],fmove
' do the same for the rear
rmoves = lr max rr
lookup rmoves,[250,250,241,231,222,212,202,193,183,174,164,154,145,135,125,125],rmove
' final result is computed similar to twist, but since we always know
' the magnitudes are going to be opposed, we can just do the weird
' calculation
move = fmove + rmove - fracHalf
' compute filtered result
twist_filt = ( (twist * filterCoef) + (twist_filt * filterInv) ) / fracBase
move_filt = ( (move * filterCoef) + (move_filt * filterInv) ) / fracBase
goto packet_send:
' debug sends to the console, for testing
debug 1
debug cr,11,cr," 0 1 2 3 4 5 6 7",11,cr
debug dec2 s0," ",dec2 s1," ",dec2 s2," ",dec2 s3," ",dec2 s4," ",dec2 s5," ",dec2 s6," ",dec2 s7,11,cr
debug 11,cr
debug "lf=",dec3 lf," rf=",dec3 rf," ftwist=",dec3 ftwist,11,cr
debug "lr=",dec3 lr," rr=",dec3 rr," rtwist=",dec3 rtwist," twist=",dec3 twist,11,cr
debug 11,cr
debug "fmoves=",dec3 fmoves," fmove=",dec3 fmove," rmoves=",dec3 rmoves," rmove=",dec3 rmove," move=",dec3 move," ",11,cr
debug "twist_filt=",dec3 twist_filt," move_filt=",dec3 move_filt,11,cr,11,cr
goto main:
' actual packet send.
timeblip:
debug "*"
goto main:
packet_send:
csum = twist ^ move ' compute checksum
if csum < 255 then csum_end:
csum = 254
csum_end:
serout 16,16624,[ $FF,twist,move,csum ] ' send packet
goto main:
