=== 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 [[madoverlord:robots-pwm|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: