Sonar Code Translation

JWatts

Hi everyone,
I recently built a sonar sensor based on the specifications listed at the following site http://www.reconnsworld.com/bs2sonar.html
I’ve tested the sensor with an oscilloscope and the hardware seems to be working perfectly fine.
The site also has the code listed which should load a debug window in the basic stamp compiler, and display distance readings (in 'units') from the circuit.
However, the code seems too outdated. It looks like PBasic 2.0?
Since I’m currently running a BS2sx together with a board of education, I’ve been trying to update the code for my module using the PBasic 2.5 syntax. I’m having problems updating the subroutines and was wondering if someone could help me fix and optimise the code so that it can work together with the BS2x module.

I’ve also attached the original BS2 code along with the current code I’m working on.

I would be grateful for any help in this matter. Thanks in advance!

Post Edited (JWatts) : 4/15/2005 12:58:30 PM GMT

JWatts

Original Code:

' Program: BS2SONAR.BS2 ( 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  0    ' !!!!! FREE STAMP PIN '0' !!!! Output to activate pinger.
pingLen  CON  200  ' Duration of ping in 2-us units.
compRC  CON  1  ' !!!!! FREE STAMP PIN '1' !!!!Output to set comparator RC circuit.
rcvr  CON  2    ' !!!!! 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.

In progress code:

' =========================================================================
'   File...... Basic Stamp Sonar
'   Purpose... Short-range sonar using BS2SX
'   Author.... http://www.reconnsworld.com/bs2sonar.html
'   E-mail....
'   Started...
'   Updated...
'
'   {$STAMP BS2sx}
'   {$PBASIC 2.5}
'
' =========================================================================

' -----[noparse][[/noparse] Program Description ]---------------------------------------------
' 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).

' -----[noparse][[/noparse] Revision History ]------------------------------------------------
' Updating the code from PBasic 2.0 to 2.5
' Updating code to run for a BS2SX module.

' -----[noparse][[/noparse] I/O Definitions ]-------------------------------------------------

ping      PIN    0       ' Output to activate pinger.
compRC    PIN    1       ' Output to set comparator RC circuit.
rcvr      PIN    2       ' Input from 40kHz receiver/comparator.


' -----[noparse][[/noparse] Constants ]-------------------------------------------------------
#SELECT $STAMP
  #CASE BS2, BS2E, BS2PE
    T1200       CON     813
    T2400       CON     396
    T4800       CON     188
    T9600       CON     84
    T19K2       CON     32
    T38K4       CON     6
  #CASE BS2SX, BS2P
    T1200       CON     2063
    T2400       CON     1021
    T4800       CON     500
    T9600       CON     240
    T19K2       CON     110
    T38K4       CON     45
#ENDSELECT

Inverted        CON     $4000
Open            CON     $8000

Baud            CON     T9600


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 "      "      "  "       "  "

pingLen   CON   200      ' Duration of ping in 2-us units.

' -----[noparse][[/noparse] 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.

' -----[noparse][[/noparse] EEPROM Data ]-----------------------------------------------------


' -----[noparse][[/noparse] Initialization ]--------------------------------------------------
HIGH ping      ' Turn pinger off initially.

' -----[noparse][[/noparse] Program Code ]----------------------------------------------------

' The "again" loop takes sonar ranges continuously and displays
' them on the PC's debug screen.

again:         ' Loop
  DO
    GOSUB sonar  ' Take the sonar reading
    DEBUG "Echo time (0-255 units): ", DEC echTime,CR   ' Display it.
  LOOP         ' Repeat endlessly.
  END

' -----[noparse][[/noparse] Subroutines ]-----------------------------------------------------
' 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:      ' 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.

Andy Lindsay (Parallax)

In the Sonar subroutine, keep in mind, the BASIC Stamp 2sx RCTIME units are 2.5 times smaller than the BASIC Stamp 2. Thus, you will need to divide eachTime by five instead of two.

For optimization, I would first get rid of the array and the bubble sorting routine because it's unnecessarily consuming processor time and RAM. Instead of an array and all those swap variables, declare five variables, index, sample, sum, smallest, and biggest. Sum should be a word variable. Then, keep a running sum of the samples and an ongoing record of the largest and smallest and subtract them out at the end.

Each time through the Sonar FOR...NEXT loop, your code should decide whether sample is the smallest, biggest, or somewhere in-between. The first time though the loop, copy sample to sum, biggest, and smallest. The rest of the loop iterations should work like this: If the sample is in-between the smallest and biggest, just add it to the sum variable. If it's bigger than the current biggest, also set biggest equal to sample. If it's smaller than the current smallest, set smallest equal to sample. When the loop is done, subtract biggest and smallest from sum. Then, divide by the number of samples, which is the number of loop iterations minus two (nsamp-2) and you've got your average.

The best way to develop and test this averaging algorithm is with a small program and the DEBUGIN command. This will allow you to enter values that you want to average into the Debug Terminal's transmit windowpane, and of course, use the DEBUG command to display the results in the Debug Terminal's receive windowpane.