Using The Devantech SRF04 Sonar Range-Finder

There will be times when infrared object detection by itself is just not adequate for certain applications or environments. In situations like this, ultrasonic range finding fits the bill for navigation, distance measurements & obstacle avoidance.

Poor Infrared Environment Examples:

	Outdoors in bright sunlight with high ambient infrared light levels.
	Locations with numerous "dark obstacles" that don't reflect IR LED energy back onto the IR sensor.

Figure #1: SRF04 Connections

SRF04 Connections

The hardware.inc file has Trigger set to PortA.2, and Echo set to PortA.3. To change the defaults, and assign the SRF04 trigger and echo pins to other port pins, simply open and edit the hardware.inc file entries shown below.

Trigger VAR PortA.2 ' PortA.2 = SRF04 trigger pin Echo VAR PortA.3 ' PortA.3 = SRF04 echo pin

The Devantech ultrasonic range finder provides a precise, non-contact option for distance measurements in the range of ~1.2 inches to 3.3 yards, and is excellent for object detection and navigation. The SRF04 works by emitting a 40KHz ultrasonic burst consisting of 8 cycles, and listening for the echo.

This 40KHz ultrasonic burst travels through the air at the speed of sound (~1.1 foot per millisecond or 1,087 feet per second), strikes the target, and then returns to the SRF04. By measuring the time between the outgoing pulse and the echo return, the approximate distance to the target can be determined. The SRF04 then outputs a pulse width proportional to the distance to the target. The width of this pulse represents the round trip flight time of the sonic burst. If no echo is detected then it will automatically time out after about 30mS.

Note: The actual speed of sound will change [although very little] depending on several factors such as humidity, temperature, and even different altitudes.

How The PIC Communicates With The SRF04:

	Output a trigger pulse of 10uS minimum to the SRF04 trigger input pin.
	Measure the output pulse width on the SRF04 echo pin.
	Wait at least 10mS between measurements. *

* Note: Since we're only taking single sonar measurements, we don't need PAUSE statements. Single sonar readings are more than adequate for Micro-Bot navigation & object detection.

The Trigger Pulse:

The PicBasic Pro compilers PULSOUT command has a resolution of 2uS when the PIC is operating at 20MHz, and 10uS when the PIC is operating at 4MHz. Depending on the crystal speed you're using, you'll need to adjust the values of the period to use with the PULSOUT command in order to generate the 10uS minimum pulse to trigger the SRF04.

Example at 20MHz: PULSOUT Trigger, 5 ' Outputs a 10uS pulse on the Trigger pin. [5 x 2uS = 10uS]

Example at 4MHz: PULSOUT Trigger, 1 ' Outputs a 10uS pulse on the Trigger pin [1 x 10uS = 10uS]

Important Note: The PULSOUT command will toggle the state of the output pin twice to generate the pulse. To be sure you're outputting a logic 1 pulse, you first set the output pin to logic 0 before issuing the PULSOUT command. The initial state of the pin will determine the polarity of the pulse.

Example:

In the HardWare.Inc file included at the beginning of our program, the Trigger pin is assigned to PortA.2, and then initially set to 0. Setting the Trigger pin [PortA.2] to 0 ensures the PULSOUT command will toggle the pin from 0 to 1, then back to 0 as shown below in Figure #2.

Figure #2: The Trigger Pulse

The Echo Pulse:

The PicBasic Pro PULSIN command is used to measure the width of the pulse output on the SRF04 echo pin. Resolution for PULSIN is the same as with the PULSOUT command. 10uS at 4MHz, and 2uS at 20MHz.

The SRF04 outputs a pulse on the Echo pin after each ultrasonic measurement. Using the PicBasic Pro PULSIN command we can easily read & record this pulse.

If we're using a 20MHz oscillator, the factor to use for converting the echo pulse width to distance in inches is 73.746. We'll round this up to 74 since we're working with integer math.

Example: A pulse width of 453 is returned in word variable Distance after triggering the SRF04 and using PULSIN. We take 453 / 74 to find the distance in inches. A returned value of 453 / 74 = ~6 inches.

453 x 2uS = 906uS. The 40KHz burst had to travel 6 inches to the target, then return 6 inches from the target for a total round trip travel time of roughly 906uS. With the PIC running at 4MHz, at the same distance from target to SRF04, PULSIN would return a value somewhere around 90.

If we're using 4MHz, then 90 / 15 = 6 inches. Viola. Using 4MHz simply divide the resulting pulse width returned by PULSIN by 15 instead of 74 as in the 20MHz example.

For these examples we'll assume a crystal speed of 20MHz. The following two code examples will move Micro-Bot forward until an obstacle is detected within 9 inches.

SONABOT.BAS: When an object is detected, program flow will jump to the right or left turn sub-routines. Which sub-routine is jumped to depends on the total number of entries & exits to & from each routine. We'll use three as the default number of entry/exit counts before re-cycling the turns counter. Experiment a little with this and see what you can come up with.

'************************************************
'*  Name    : SONABOT.BAS                       *
'*  Author  : Bruce Reynolds                    *
'*  Notes   : Micro-Bot Sonar Navigation        *
'************************************************

' Declare a few local variables
Left_Turns  VAR BYTE      ' Counter for left turns
Right_Turns VAR BYTE      ' Counter for right turns
Left_Turns = 0            ' Reset left turns counter
Right_Turns = 0           ' Reset right turns counter

INCLUDE "HARDWARE.INC"    ' Hardware setup & variables

' We'll make right turns or left turns when a target is
' detected based on the accumulated numer of each turn
    
Begin:                    ' Get sonar reading
  GOSUB Ping
  
  ' Micro-Bot is around 7 1/8" in length, so we'll use 9 inches as the
  ' target distance to allow plenty of turning room. If the target is
  ' closer than or equal to 9 inches, back up and turn right. If not, then
  ' continue moving forward.
  
  IF Distance <= 9 THEN Obstacle
  PULSOUT Left_Servo,ForwardLeft   ' Forward left servo
  PULSOUT Right_Servo,ForwardRight ' Forward right servo 
  GOTO Begin
 
Obstacle:                 ' Backwards, then turn left or right [count dependent]
  FOR X=0 TO 9            ' Increase value to increase backup distance 
   PULSOUT Left_Servo,ReverseLeft   ' Reverse left servo
   PULSOUT Right_Servo,ReverseRight ' Reverse right servo
   PAUSE 20
  NEXT
  
Turn_Counts:
  IF (Left_Turns >= 3) AND (Right_Turns >= 3) THEN Reset_Count
  IF Right_Turns > 3 THEN ' IF right turns is less than 3, then turn right
   GOTO LeftTurn          ' GOTO left turn
  ELSE
   GOTO RightTurn         ' GOTO right turn
  ENDIF
  
Reset_Count:  
  Left_Turns = 0         ' Reset left turns counter
  Right_Turns=0          ' Reset right turns counter 
  
RightTurn:               ' Turn right, then check for target
  FOR X=0 TO 15          ' Approximately 90 degrees right
   PULSOUT Left_Servo,ForwardLeft   ' Forward left servo
   PULSOUT Right_Servo,ReverseRight ' Reverse right servo
   PAUSE 20
  NEXT
  GOSUB Ping    ' Take sonar reading
  
  ' Continue to turn right until target is more than 9 inches away
  IF Distance <= 9 THEN RightTurn
  Right_Turns=Right_Turns+1   ' Increment right turns count
  GOTO Begin                  ' Return

LeftTurn:                     ' Turn left, then check for target
  FOR X = 0 TO 15
   PULSOUT Left_Servo,ReverseLeft   ' Reverse left servo
   PULSOUT Right_Servo,ForwardRight ' Forward right servo
   PAUSE 20
  NEXT
  GOSUB Ping      ' Take sonar reading
  
  ' Continue to turn left until target is more than 9 inches away
  IF Distance <= 9 THEN LeftTurn
  Left_Turns=Left_Turns+1 ' Increment left turns count
  GOTO Begin      ' Return
    
  END

SONABOT2.BAS: When an object is detected within 9 inches, Micro-Bot will jump to a room mapping routine. In the mapping routine Micro-Bot will make a series of right turns, and take a distance measurement in each new direction after making each turn. Once all five readings are taken, Micro-Bot will turn back towards the path with the longest distance to an obstacle, and continue forward. This is a fun routine to experiment with.

'************************************************
'*  Name    : SONABOT2.BAS                      *
'*  Author  : Bruce Reynolds                    *
'*  Notes   : Sonar Navigation With Mapping     *
'************************************************

' Declare a few local variables
Index  VAR BYTE           ' Counter for turns & array index pointer
Position VAR WORD[5]      ' Array for 5 sonar readings
Direction VAR BYTE        ' Best direction to advance towards

INCLUDE "HARDWARE.INC"    ' Hardware setup & variables
  
Begin:                    
  GOSUB Ping              ' Jump to sonar routine 
  IF Distance <= 9 THEN Mapping    ' If object is <= 9 inches away, start mapping
  PULSOUT Left_Servo,ForwardLeft   ' Forward left servo
  PULSOUT Right_Servo,ForwardRight ' Forward right servo 
  GOTO Begin
 
Mapping:                   ' Back up a little first
  FOR X = 0 TO 24          ' Change to increase or decrease backwards movement
   PULSOUT Left_Servo,ReverseLeft
   PULSOUT Right_Servo,ReverseRight
   PAUSE 20
  NEXT X                   ' Finished backing up, now start mapping a path
 
  FOR Index = 0 TO 4       ' Loop 5 times + index 5 array elements
   FOR X=0 TO 4            ' Increase X here to expand sonar sweep turn radius
    PULSOUT Left_Servo,ForwardLeft   ' Forward left servo
    PULSOUT Right_Servo,ReverseRight ' Reverse right servo
    PAUSE 20
   NEXT
   PAUSE 500                ' Allow to settle after turning
   GOSUB Ping               ' Take sonar reading in new direction
   Position[Index]=Distance ' Log converted measurements into array
  NEXT Index
 
 ' Here we find the direction with the clearest path from
 ' the sonar sweep.
 
  Direction = 4            ' Point to 5th array element position
  FOR Index = 0 TO 4       ' Index position array elements 0 to 4
   IF Position[Index] > Position[Direction] THEN
    Direction = Index      ' Direction = array element # with longest distance reading
   ENDIF                   
  NEXT Index
  IF Direction = 4 THEN Begin ' No turn required? Move forward again
  Direction = 4 - Direction   ' Find # of turns to clearest path
  
 ' Here we make left turns to point back in the direction of the best path.

  FOR Index = 1 TO Direction
   FOR X=0 TO 4                      ' Turn back to best path
    PULSOUT Left_Servo,ReverseLeft   ' Reverse left servo
    PULSOUT Right_Servo,ForwardRight ' Forward right servo
    PAUSE 20
   NEXT
  NEXT Index
  GOTO Begin
  
  END

Back To Index