Getting a BASIC stamp to sample 8 Ultrasonic sensors at once

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: