Differences

This shows you the differences between two versions of the page.

Link to this comparison view

madoverlord:robots-ultra [2016/05/14 20:02]
madoverlord
madoverlord:robots-ultra [2016/10/04 14:42]
Line 1: Line 1:
-=== 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 <A HREF="​pwm.t.html">​PWM reader</​A>​ for 
-more background on the debug port trick and what the heck 
-"​fracBase"​ is. 
- 
- 
- 
- 
- 
-<​code>​ 
-' PROGRAM: ​   BS2SX Sonar Parallel Ping Test 
-' Written by: Robert J Woodhead 
-' Date:       31 MAR 02 
-' 
-' Define BS2-SX Project Files 
-' 
-' &#​123;​$STAMP BS2SX&#​125;​ 
-' 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: 
-</​code>​ 
- 
- 
- 
-