| |
'****************************************************************
'****************************************************************
'**Description: TR2 Rover with SP03 Text to Speech Synthesizer.
'**This program utilises a Servo sweep routine which improves the
'**obstacle avoidance capabilities of the TR1 Rover.
'** The SP03 allows the rover to speak during its sweep search,
'**commentating on its status.
'**
'** Compiler Version: 4.0.0
'** Created: 09/11/02 Revision: 1.00
'** Written by: Total Robots (Jamie Finnan)
'** Adapted from TR1 Rover with Servo Sweep
'** TR-016
'**
'****************************************************************
'****************************************************************
Dim Speak as new oI2C 'SP03 communication
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 Output
const cLeftForward = 10 'Constant
const cLeftBackward = 50 'Constant
const cRightForward = 50 'Constant
const cRightBackward = 10 'Constant
'****************************************************************
Sub Main()
Call Initialise 'Setup
OOPic.delay=600 'Long delay for opening phrase
speak = 21 'Phrase number 21
OOPic.delay=300 '3 second delay
do
Call Sweep 'Main sweep routine
Loop
End Sub
'****************************************************************
Sub Sweep()
Call Servo_MoveForward 'Move Rover Forward
Sweep_Servo.Value = 10 'Start Position of Sweep Servo
Do
Sweep_Servo.Value = (Sweep_Servo.Value + 5) '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 > 65 Then 'Is Servo at end of Sweep?
Call Sweep_Back
End If
Call Servo_MoveForward 'Move rover forward
Loop
End Sub
'****************************************************************
Sub Sweep_Back()
Do
Sweep_Servo.Value = (Sweep_Servo.Value - 5) '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 < 10 Then 'Servo returned to start position
when <10
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 'End Trigger pulse
oopic.delay=1 'Small delay
while (ECHO.value=1)
wend
SR=echoTimer.value/36 'this converts the sonar
'reading to a distance
'in cm
oopic.delay=1 'Small delay
end sub
'****************************************************************
Sub Initialise() 'Setup routine
OOPic.Delay = 200
Call Servo_Setup 'Servo setup
Call Sonarsetup 'Sonar setup
Call SP03_Setup 'SP03 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 vary 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 SP03_Setup()
const speakadr = 98 'SP03 address shifted right one bit
speak.node = speakadr 'Set Node address
speak.width = cv8bit '1 byte wide
speak.mode = cv10bit
speak.NoInc = cvtrue 'No incrementation
End Sub
'***************************************************************
Sub Servo_Stop()
Left_Servo.Operate = cvFalse 'Stop Left Servo
Right_Servo.Operate = cvFalse 'Stop Right Servo
End Sub
'****************************************************************
Sub Evade()
Call Servo_Stop
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
Call Servo_MoveForward
End Sub
'****************************************************************
Sub Decide()
Again:
speak=&h06 'Phrase 6
OOPic.delay=50 'Short phrase delay
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
elseif Left_Test.Value = 0 Then 'Move to the left if clear
Call Servo_MoveRight
Else
speak=11 'Phrase 11
oopic.delay=500
Goto Again 'If both sides blocked, redo routine
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<60) then 'If barrier within 60cm 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()
speak=20
oopic.delay=100
Do
Sweep_Servo.Value = (Sweep_Servo.Value - 1) 'Decrement file
call readSonar
if (SR<60) then 'Any barriers within 60cm?
Right_Test.Value = 1 'Set Right_Test bit if yes.
end if
If Sweep_Servo.Value = 40 Then '40 is end of sweep
Exit Sub
End If
Loop
End Sub
'****************************************************************
Sub Set_Right()
Sweep_Servo.Value = 70 '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
End Sub
'****************************************************************
Sub Servo_MoveLeft() 'Routine to move rover Left
speak=&h09 'Phrase 9
OOPic.delay=200 'Phrase 2 second delay
Call Servo_Stop
Right_Servo.Value = cRightBackward
Left_Servo.Value = cLeftForward
Right_Servo.Operate = cvTrue
Left_Servo.Operate = cvTrue
OOPic.delay = 70
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 = 120 'Rover moves for 1.5 secs
Call Servo_Stop
End Sub
'****************************************************************
Sub Servo_MoveRight() 'Routine to move rover right
Speak=&h08 'Phrase 8
OOPic.delay=200 '2 second phrase delay
Call Servo_Stop
Left_Servo.Value = cLeftBackward
Right_Servo.Value = cRightForward
Left_Servo.Operate = cvTrue
Right_Servo.Operate = cvTrue
OOPic.delay = 70 '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
'****************************************************************
'****************************************************************
|
