Total Robots - Example - Servo Sweep - SRF04 Ultra Sonic Range Finder

Servo Sweep using an SRF04 Ultra Sonic Range Finder

This example application uses an SRF04 Ultra Sonic Range Finder coupled with a servomotor to improve the obstacle avoidance capabilities of the TR1 Rover (although it could be easily added to any other mobile robot).

The SRF04 is mounted onto the servo using a Lynxmotion single sensor housing and both the SRF04 and the servo are connected to the OOPic via the DS-SXM16S Servo Expansion Module. The servomotor sweeps the sensor in a 180 degree arc so obstacles in front of and to the side of the robot are detected. When the sensor detects an object, the object's position is calculated using the pulse provided to the servo. The objects position is then used to determine the appropriate movement of the robot to avoid a collision.

The hardware required to implement this application is available separately (see our accessories page) as a complete package (see our programmable page).

'***************************************************************************
'***************************************************************************
'** Description: TR1 Rover With Servo Sweep. This program utilises a
'** Servo sweep routine which improves the obstacle avoidance capabilities
'** of the TR1 Rover.
'** Additional Components:- HS300 Servo, Single SRF04 Housing & SRF04
'** Ultra Sonic Range Finder.
'** Created: 05/08/02 Revision: 1.00
'** Written by: Total Robots (Jamie Finnan)
'** TR-002
'**
'***************************************************************************
'***************************************************************************
Dim SR as new oWord 'the sonar reading
Dim Right_Servo As New oServo 'Right Servo Wheel
Dim Left_Servo As New oServo 'Left Servo Wheel
Dim Sweep_Servo As New oServo 'Ultra Sound Sweep Servo
Dim Left_Test As New oBit 'Test Bit
Dim Right_Test As New oBit 'Test Bit
dim echoTimer as new oTimer 'Echo Timer for Ultra Sound
dim tmrControl as new oGate (1) 'Timer Control for Echo
dim TRIGGER as new oDio1 'Trigger Pulse Input
dim ECHO as new oDio1 'Echo Pulse Outputconst cLeftForward = 10 'Constant
const cLeftBackward = 50 'Constant
const cRightForward = 50 'Constant
const cRightBackward = 10 'Constant
'***************************************************************************
'***************************************************************************
Sub Main()
Call Initialise 'Setup
Do
OOPic.Delay = 100 'Delay to Settle System
Sweep_Servo.Value = 4 'First position of sweep servo
Call Sweep
Loop
End Sub
'***************************************************************************
Sub Sweep()
Call Servo_MoveForward 'Move Rover Forward
Sweep_Servo.Value = 4 'Start Position of Sweep Servo
Do
Sweep_Servo.Value = (Sweep_Servo.Value + 1) 'Increment sweep
call readSonar
if (SR<40) then 'If Sonar detects anything within 40cm
Call Evade 'Then evade the object.
end if
If Sweep_Servo.Value = 63 Then'Is Servo at end of Sweep?
Call Sweep_Back
End If
Loop
End Sub
'***************************************************************************
Sub Sweep_Back()
Do
Sweep_Servo.Value = (Sweep_Servo.Value - 1) 'Decrement sweep
call readSonar
if (SR<40) then 'If sonar detects anything within 40cm
Call Evade 'Then evade object
end if
If Sweep_Servo.Value = 4 Then 'Servo returned to start position when 4
Call Servo_MoveForward 'Move rover forward

End If
If Sweep_Servo.Value = 4 Then
Exit Sub

End If
Loop
End Sub
'***************************************************************************
sub readSonar ()
echoTimer.value=0 'Set up Echo timer

TRIGGER.value=1 'Send Trigger pulse to SRF04
TRIGGER.value=0
oopic.delay=1
while (ECHO.value=1)
wend
SR=echoTimer.value/36 'this converts the sonar
'reading to a distance
'in cm
oopic.delay=1
end sub
'***************************************************************************
Sub Initialise()
OOPic.Delay = 200
Call Servo_Setup 'Servo setup
Call Sonarsetup 'Sonar setup
End Sub
'***************************************************************************
Sub Servo_Setup()
Right_Servo.IOLine = 10 'Right Servo on SV6
Right_Servo.Center = 32 'Center position for Servo
Right_Servo.Operate = CVTrue 'Set Right Servo
Left_Servo.IOLine = 5 'Left Servo onSV5
Left_Servo.Center = 32 'Center position for servo
Left_Servo.Operate = CVTrue 'Set Left Servo
Sweep_Servo.IOLine = 12 'Sweep Servo on SV8
Sweep_Servo.Center = 22 'Center Sweep Servo
Sweep_Servo.Operate = CVTrue 'Set Sweep Servo
Call Servo_Stop 'Stop Right and Left Servo's
End Sub
'Note that center positions can varry on different servo motors.
'***************************************************************************
sub Sonarsetup ()
echoTimer.ExtClock=cvOff
echoTimer.PreScale=3
TRIGGER.IOLine=4 'connect Trigger Pulse Input
'on SRF04 to SV4
TRIGGER.Direction=cvOutput
TRIGGER.value=0
ECHO.IOLine=3 'connect Echo Pulse Output
'to SV3
ECHO.Direction=cvInput

tmrControl.Input1.Link(ECHO.value)
tmrControl.Output.Link(echoTimer.Operate)
tmrControl.Operate=cvTrue
End Sub
'***************************************************************************
Sub Servo_Stop()
Left_Servo.Operate = cvFalse 'Stop Left Servo
Right_Servo.Operate = cvFalse 'Stop Right Servo
End Sub
'***************************************************************************
Sub Evade()
If Sweep_Servo < 12 Then 'Check for obstacles on the left
Call Servo_SmallMoveRight 'If obstacle, move right
else
If Sweep_Servo > 48 Then 'Check for obstacles on the right
Call Servo_SmallMoveLeft 'If obstacle, move left
else
Call Decide 'Otherwise obstacle in front
End If
End If
End Sub
'***************************************************************************
Sub Decide()
Again:
Call Servo_MoveBackward 'Move rover backwards
Call Search_Left 'Scan Left for obstacles
Call Search_Right 'Scan Right for obstacles
If Right_Test.Value = 0 Then 'Move to the right if clear
Call Servo_MoveLeft
else
If Left_Test.Value = 0 Then 'Move to the left if clear
Call Servo_MoveRight
Else
Goto Again 'If both sides blocked, redo routine

End If
End If
End Sub
'***************************************************************************
Sub Search_Left()
Call Set_Left 'Set Left start position
OOPic.Delay = 50
Left_Test.Value = 0 'Clear Left_Test bit
Call Sweep_Left
End Sub
'***************************************************************************
Sub Sweep_Left()
Do
Sweep_Servo.Value = (Sweep_Servo.Value + 1) 'Increment Sweep
call readSonar 'Read sonar for any barriers
if (SR<40) then 'If barrier within 40cm then
Left_Test.Value = 1 'Set LeftTest bit if yes.
end if
If Sweep_Servo.Value = 29 Then 'If sweep is at 29 then finish
Exit Sub

End If
Loop
End Sub
'***************************************************************************
Sub Set_Left()
Sweep_Servo.Value = 4 'Start point for sweep
End Sub
'***************************************************************************
Sub Search_Right()
Call Set_Right
OOPic.Delay = 50
Right_Test.Value = 0 'Clear Right Test Bit
Call Sweep_Right
End Sub
'***************************************************************************
Sub Sweep_Right()
Do
Sweep_Servo.Value = (Sweep_Servo.Value - 1) 'Decrement file
call readSonar
if (SR<40) then 'Any barriers within 40cm?
Right_Test.Value = 1 'Set Right_Test bit if yes.
end if
If Sweep_Servo.Value = 34 Then '34 is end of sweep
Exit Sub
End If
Loop
End Sub
'***************************************************************************
Sub Set_Right()
Sweep_Servo.Value = 63 'Start point for right sweep
End Sub
'***************************************************************************
Sub Servo_MoveForward() 'Routine to move rover forward
Call Servo_Stop
Left_Servo.Value = cLeftForward
Right_Servo.Value = cRightForward
Left_Servo.Operate = cvTrue
Right_Servo.Operate = cvTrue
OOPic.delay = 100 'Move forward for 1 sec
Call Servo_Stop 'Stop the rover
End Sub
'***************************************************************************
Sub Servo_MoveLeft() 'Routine to move rover Left
Call Servo_Stop
Right_Servo.Value = cRightBackward
Right_Servo.Operate = cvTrue
OOPic.delay = 100
Call Servo_MoveForward 'Move rover forward
Call Servo_Stop 'Stop the rover
End Sub
'***************************************************************************
Sub Servo_MoveBackward() 'Routine to move rover backward
Call Servo_Stop
Left_Servo.Value = cLeftBackward
Right_Servo.Value = cRightBackward
Left_Servo.Operate = cvTrue
Right_Servo.Operate = cvTrue
OOPic.delay = 150 'Rover moves for 1.5 secs
Call Servo_Stop
End Sub
'***************************************************************************
Sub Servo_MoveRight() 'Routine to move rover right
Call Servo_Stop
Left_Servo.Value = cLeftBackward
Left_Servo.Operate = cvTrue
OOPic.delay = 100 'Move rover for 1 sec
Call Servo_MoveForward 'Move rover forward
Call Servo_Stop 'Stop rover
End Sub
'***************************************************************************
Sub Servo_SmallMoveLeft() 'Routine to move rover a little left
Call Servo_Stop
Right_Servo.Value = cRightForward
Right_Servo.Operate = cvTrue
OOPic.delay = 50 'Move for 0.5 secs
Call Servo_MoveForward 'Move rover forward
Call Servo_Stop 'Stop rover
End Sub
'***************************************************************************
Sub Servo_SmallMoveRight() 'Routine to move rover a little right
Call Servo_Stop
Left_Servo.Value = cLeftForward
Left_Servo.Operate = cvTrue
OOPic.delay = 50 'Move for 0.5 secs
Call Servo_MoveForward 'Move rover forward
Call Servo_Stop 'Stop Rover
End Sub
'***************************************************************************
'***************************************************************************

Examples | Shop