' Program: BS2SONAR.TXT ( short-range sonar using BS2)
' This program, in cooperation with the 40kHz send/receive circuitry
' creates an inexpensive short-range (10") sonar 
' system suitable for collision avoidance in small robots. The program
' instructs the circuit to emit a short burst of 40kHz sound (ping)
' and waits to hear a return echo. The BS2 RCtime instruction times
' the ping-to-echo interval to the nearest 2us. Raw sonar data
' is quite noisy, so the sonar subroutine actually takes several 
' samples (5 or more; set by the nSmp constant below). It scales
' the samples to byte size (0-255 units of 4us each), stores 
' them in an array, and sorts them from high to low. The program 
' then averages the five middle samples. The result is a reading 
' that's quite stable (assuming a fixed distance from the sonar
' to the object). Best accuracy is in the range of 2 to 7 inches
' (about 58 to 225 units). 
'========CONSTANTS
nSmp	con	10		' Number of samples (NOT LESS THAN 5).
maxSmp	con	nSmp-1		' Array index # of last sample. 
maxSrt	con	nSmp-2		' Maximum index # to sort. 
s1	con	nSmp/2-2  	' 1st sorted sample to include in average. 
s2	con	nSmp/2-1  	' 2nd "      "      "  "       "  "
s3	con	nSmp/2    	' 3rd "      "      "  "       "  "
s4	con	nSmp/2+1  	' 4th "      "      "  "       "  "
s5	con	nSmp/2+2  	' 5th "      "      "  "       "  "
ping	con	X		' !!!!! FREE STAMP PIN '0' !!!! Output to activate pinger. 
pingLen	con	200	' Duration of ping in 2-us units. 
compRC	con	X	' !!!!! FREE STAMP PIN '1' !!!!Output to set comparator RC circuit. 
rcvr	con	X		' !!!!! FREE STAMP PIN '2' !!!! Input from 40kHz receiver/comparator. 
'========VARIABLES
echTime	var	word	  	' Time to echo return. 
smp	var	byte(nSmp) 	' Storage for multiple readings. 
index	var	byte	   	' Counter for sampling. 
swapTmp	var	byte	   	' Temporary storage for swapping.  
swap	var	bit	   	' Flag to indicate whether sort is done. 
'========PROGRAM
' The "again" loop takes sonar ranges continuously and displays
' them on the PC's debug screen. 
high ping		' Turn pinger off initially. 
again: 			' Loop. 
  gosub sonar		' Take the sonar reading 
  debug "Echo time (0-255 units): ", dec echTime,cr	' Display it. 
goto again		' Repeat endlessly. 
'========SONAR SUBROUTINE
' It takes only five instructions to get a quick sonar snapshot of the 
' distance to the closest sonar-reflective object. However, you can get
' better, more consistent results by taking several sonar readings, 
' discarding the highest and lowest ones and averaging the middle. 
' This routine takes the number of samples specified by the constant 
' nSmp, sorts them, and averages the middle. Each reading takes
' only a few milliseconds (owing to the sonar's short range). 
sonar:
for index = 0 to maxSmp		' Take nSmp samples.
  high compRC			' Raise C2 to +5 volts. 
  pause 1			' Allow time for C2 to reach +5V. 
  input compRC			' Disconnect pin from C2. 
  pulsout ping,pingLen		' Send a short 40kHz pulse. 
  rctime rcvr,0,echTime		' Wait for echo; save time to echTime. 
  smp(index) = echTime/2 max 255  ' Save to array smp() as byte (0-255). 
next				' Get another sample. 
' At this point, there are nSmp sonar samples stored in the bytes of 
' the smp() array. One way to discard the lowest and highest samples
' is to sort the array so that the lowest index values contain the 
' largest numbers. The code starting with "sort" does this using 
' a technique called "bubble sort." The idea is simple--compare 
' adjacent bytes in the array, for instance smp(0) and smp(1). 
' If the value stored in smp(0) is greater than or equal to that in 
' smp(1), do nothing. Otherwise, swap the values so that smp(0) 
' gets the contents of smp(1), and vice versa. Keep doing this 
' with each pair of values in the array. The larger values in the 
' array will migrate toward the lower index values--they rise
' like soda bubbles. Repeated passes through the array will 
' completely sort it. The routine is done when it makes a loop
' through the array without swapping any pairs. 
sort: 
  swap = 0			' Clear flag that indicates swap. 
for index = 0 to maxSrt		' For each cell of the array...
  if smp(index) >= smp((index+1)) then noSwap  ' Move larger values up. 
    swapTmp = smp(index)	' ..by swapping them. 
    smp(index) = smp(index+1)
    smp(index+1) = swapTmp
    swap = 1			' Set bit if swap occurred.  
noSwap:	
next				' Check out next cell of the array. 
if swap = 1 then sort		' Keep sorting until no more swaps. 
' The line below just averages particular cells of the array. If you
' use my values of the constants s1 through s5, it averages readings
' from the middle of the range. By assigning other values to s1-
' s5, you can alter this. 
echTime = smp(s1)+smp(s2)+smp(s3)+smp(s4)+smp(s5)/5 max 255
return				' Done: return to program.