| |
'**************************************************************************
'**************************************************************************
'Description: This example application demonstrates how a BX-24 can
be plugged
'an RCBV2 and used to drive an autonomous X4 Rover. The X4 moves forwards
'looking for obstacles. If an obstruction is encountered, it moves backwards,
then
'turns to find an open space which it then moves into.
'
'Compiler Version 2.1 Beta 1
'Created: 17/02/03 Revision 1.00
'Written by: Total Robots
'TR-32
'
'**************************************************************************
'**************************************************************************
'Dim SonarStack(1 to 100) as byte 'Assigns 100 bytes of memory for the
sonar
'task to run parallel with main program.
'Dim Dist As Integer Store for the SRF04 Reading
'Dim Count As byte Delay Count
'Const LeftServo As Byte = 14 Servo on SV9
'Const RightServo As Byte = 13 Servo on SV10
'**************************************************************************
Sub Main()
call delay (5.0) '5 Second delay for setup
callTask"sonar",SonarStack 'Run the sonar task parallel to
main sub.
do
Call PulseOut(LeftServo,0.0015,1) 'Start Positions for servo1, 0.0015s
pulse.
Call Delay(0.01) '0.01 second delay
Call PulseOut(RightServo,0.0022,1) 'Start position for servo2, 0.0022s
pulse.
Call Delay(0.01) '0.01 second delay
if Dist < 30 then 'if sonar reading is less than 30
Call delay(0.5) '0.5 second delay
Call backservo 'X4 reverse routine
Call spinsearch 'Search for a clear space
end if
loop
end sub
'**************************************************************************
Sub sonar()
Const EchoPin As Byte = 16 'Echo pin set up on pin 16
Const TrigPin As Byte = 15 'Trigger pin set up on pin 15
Const RefreshPeriod As Single = 0.02 'Generate a refresh rate of 50Hz
Call PutPin(EchoPin, bxInputTristate) 'Setup the Echo and trigger pins
Call PutPin(TrigPin, bxOutputLow)
do
Call PulseOut(TrigPin, 10, 1) '10uS Trigger pulse
Dist = PulseIn(EchoPin, 1) \ 54 'use 54 for Cm or 137 for Inches
Debug.Print "Range is "; CStr(Dist) 'Print distance to the
Debug window
Call Delay(0.1) '100mS to next ping (minimum is 10mS)
loop
end sub
'**************************************************************************
Sub spinsearch()
do while(Dist < 80) 'While distance is less than 60
Call PulseOut(LeftServo,0.001,1) 'Pulse 0.001s to left servo
Call Delay(0.01) '0.01 second delay
Call PulseOut(RightServo,0.001,1) 'Pulse 0.001s to right servo
Call Delay(0.01) '0.01 second delay
Debug.Print "Spin Search Here " 'Output Spin search details
to the debug screen
loop
end sub
'**************************************************************************
Sub backservo()
count = 0
do while(count < 20) 'Do loop while count is less than 20
count=count + 1 'Increment count by 1 each loop
Call PulseOut(LeftServo,0.0022,1) 'Pulse 0.0022s to left servo
Call Delay(0.01) '0.01 second delay
Call PulseOut(RightServo,0.0015,1) 'Pulse 0.0015s to right servo
Call Delay(0.01) '0.01 second delay
Debug.Print "Reverse X4 " 'Output Rverese info to debug screen
loop
end sub
'**************************************************************************
'**************************************************************************
|
