| |
'**************************************************************************
'**************************************************************************
'Description: X4 Rugged Terrain Rover (MKI) with the IRCM and LCD.
'This code enables the X4 to drive autonomously while looking for obstacles.
'On encountering any blockages, the rover will reverse, then spin right
to check for open
'spaces to drive into.
'In this application, a TV remote control can be used to communicate
with the onboard
'IRCM so the X4 can be manually overidden and controlled before being
sent back into
'autonoumous mode.
'
'Compiler Version: 5.01 (B2.0+) OOPic-R
'Created: 25/07/03 Revision 1.00
'Written By:Total Robots
'TR-X4MKI_IRCM
'
'**************************************************************************
'**************************************************************************
Dim right as new oServoSP2 'Right Servo's
Dim left as new oServoSP2 'Left Servo's
Dim buzz as word 'Count for Speaker
Dim count as new obyte 'Count for reverse beep
Dim Sonar as new oSonarDV 'SRF04
Dim SR as new oWord 'Word file for SRF04
Dim IRCM As New oi2c 'Make IRCM a new I2C device
Dim oLeftServo As New oServo 'Define left hand servo object
Dim oRightServo As New oServo 'Define right hand servo object
Dim RCformat As New oByte 'Remote control format register
Dim RCdata1 As New oByte 'Remote control data register 1
Dim RCdata2 As New oByte 'Remote control data register 2
Dim RCdata3 As New oByte 'Remote control data register 3
Dim RCdata4 As New oByte 'Remote control data register 4
Dim RCOK As New oBit 'Receive ok indicator
Dim LCD as new oLCDSE 'LCD object
Dim Red as new odio1 'Red LED
Dim Yel as new odio1 'Yellow LED
Dim Gre as new odio1 'Green LED
Dim beep2 As New oFreq 'Frequency tone for speaker
Const IRCMAdr = &h60 'IRCM A & B jumpers OFF (Range &h60-&h63)
'**************************************************************************
Sub main()
'*******************************************************
'Main routine
'*******************************************************
Sonar.IOLineE = 10 'Echo Line on Pin 4
Sonar.IOLinep = 11 'Pulse line on Pin 3
Sonar.Operate = 1 'Sonar on
right.Ioline = 8 'Right Servos IOLine 8
left.Ioline = 9 'Left Servos IOLine 9
right.operate = cvTrue 'Right on
left.operate = cvTrue 'Left on
Red.IOLine = 7 'Red LED IOLine
Yel.IOLine = 6 'Yellow LED IOLine
Gre.IOLine = 5 'Green LED IOLine
Red.Direction = cvOutput 'Set LED as output
Yel.Direction = cvOutput 'Set LED as output
Gre.Direction = cvOutput 'Set LED as output
LCD.IOLine = 16 'LCD on IOLine 16
Call Wake_Up 'Initial speaker sound and LCD message
Call Forward 'Move rover forward
Call Read_Sonar 'Take sonar readings
End Sub
'**************************************************************************
Sub Wake_Up()
'*******************************************************
'Initialise subroutine
'*******************************************************
oopic.delay = 50 '0.5 second delay
LCD.Value = 254 'LCD Command
LCD.Value = 128 'Set to first character position
LCD.String = "X4 Rover & IRCM" 'Send text String
LCD.Value = 254 'LCD Command
LCD.Value = 192 'Set to second line first character
LCD.String = "Warm Up........." 'Send Text String
buzz = 30000 'set buzz value
while (buzz < 65000) 'While construct
buzz = (buzz+1000) 'increment buzz by 5 each loop
beep2.beep (buzz,10) 'Speaker freq is buzz
wend 'While end construct
LCD.Clear 'Clear the LCD screen
end sub 'End of routine
'**************************************************************************
Sub SetUpIRCM()
'*******************************************************
'Set up the IRCM ready for communication
'*******************************************************
'Set the DS-WCM I2C address shifted right by 1 bit
IRCM.Node = IRCMAdr 'Setup I2C address for IRCM
'Setup I2C addressing to IRCM
IRCM.Width = cv8bit 'Control Info is 1-byte
IRCM.Mode = cv10bit 'I2C mode is 10-Bit Addressing
IRCM.NoInc = cvFalse 'Increment on every read/write
End Sub
'**************************************************************************
Sub Reverse_Beep()
'*******************************************************
'A small routine to output a sound when reversing
'*******************************************************
count = 0
while (count < 3) 'While construct
count = (count + 1) 'increment count by 1 each loop
oopic.delay = 20 '0.5 second delay
Beep2.Beep(37000,5) 'Set frequency and time
Beep2.Beep(47000,5) 'Set frequency and time
Beep2.Beep(57000,5) 'Set frequency and time
wend 'While end construct
End sub
'**************************************************************************
Sub Forward()
'*******************************************************
'Routine to move X4 forwards
'*******************************************************
Gre.Value = 1 'Green LED on
Red.Value = 0 'Red LED off
right = 127 'Right servos value
left = -127 'Left servos value
End Sub
'**************************************************************************
Sub Stop()
'*******************************************************
'Routine to stop the X4 and output a sound
'*******************************************************
Beep2.Beep(64000,5) 'Set frequency and time
Beep2.Beep(51000,5) 'Set frequency and time
Beep2.Beep(64000,5) 'Set frequency and time
Beep2.Beep(51000,5) 'Set frequency and time
Red.Value = 1 'Red LED is on
Gre.Value = 0 'Green LED off
right = 0 'Right servos value
left = 0 'Left servos value
End Sub
'**************************************************************************
Sub Spin_right()
'*******************************************************
'Spin the X4 right on the spot and output a sound
'*******************************************************
Beep2.Beep(5000,5) 'Set frequency and time
Beep2.Beep(51000,5) 'Set frequency and time
Beep2.Beep(5000,5) 'Set frequency and time
Beep2.Beep(51000,5) 'Set frequency and time
Gre.Value = 1 'Green LED is on
Red.Value = 1 'Red LED is on
right = -97 'Right servo value
left = -97 'Left servo value
End Sub
'**************************************************************************
Sub Spin_left()
'*******************************************************
'Spin the X4 left on the spot and output sound
'*******************************************************
Beep2.Beep(47000,5) 'Set frequency and time
Beep2.Beep(57000,5) 'Set frequency and time
Beep2.Beep(47000,5) 'Set frequency and time
Beep2.Beep(57000,5) 'Set frequency and time
Gre.Value = 1 'Green LED is on
Red.Value = 1 'Red LED is on
right = 97 'Right servo value
left = 97 'Left servo value
End Sub
'**************************************************************************
Sub Backward()
'*******************************************************
'Set the X4 into reverse
'*******************************************************
Call Reverse_Beep 'Reverse sound
right = -127 'Right servos value
left = 127 'Left servos value
End Sub
'**************************************************************************
Sub Read_Sonar()
'*******************************************************
'Read the Sonar reading from the SRF04
'and the IRCM values (if any).
'*******************************************************
Call SetUpIRCM 'Setup WCM ready for communication
Do
call ReadRC 'Read any data from remote node
If (RCData2 = 176) AND (RCData3 = 0) then
'Value of 176 sends to manual
Call Manual 'If IRCM has received data go into manual
End if
Call Forward 'Move rover forwards
SR = 0 'Sonar reading file
Sonar.Operate = 1 'Sonar on
Sonar.Operate = 0 'Sonar off
OOPic.Delay = 25 '0.25 Second delay
SR = (Sonar.Value) 'move read sonar value to SR
SR = (SR / 36) 'Divide SR by 36 to get cm (64 for inches)
If (SR < 2) then 'If SR is less than 2 then
Call Evade 'call evade sequence
end if
loop
End Sub
'**************************************************************************
Sub Evade()
'*******************************************************
'If an object has been detected, this routine
'will navigate around the obstacle.
'*******************************************************
Sonar.Operate = 0 'Sonar off
Call Stop 'Stop the servos
oopic.delay = 30 '0.3 second delay
Call Backward 'Move X4 backwards
OOPic.delay = 20 '0.2 second delay
Call Stop 'Stop servos
Do
Call Spin_right 'Spin the X4 right to look for a gap
SR = 0
Sonar.Operate = 1 'Sonar on
Sonar.Operate = 0 'Sonar off
OOPic.Delay = 25 '0.25 second delay
SR = (Sonar.Value)
SR = (SR/36)
If (SR > 5) then 'If a reading of greater than 5 is taken
Call Stop 'stop the servos
Call Forward 'move X4 forwards again.
Exit Sub
End If
Loop
End Sub
'**************************************************************************
Sub ReadRC()
'*******************************************************
'This routine reads the IRCM data registers to
'determine whether an authorised command has
'been received
'*******************************************************
IRCM.Location = 28 'Start at read status flag (R28)
If IRCM.Value = 255 then 'Has data been received (R28) ?
RCformat = IRCM.Value 'If so then store format
RCdata1 = IRCM.Value 'Store data
RCdata2 = IRCM.Value 'Store data
RCdata3 = IRCM.Value 'Store data
RCdata4 = IRCM.Value 'Store data
RCOK = cvTrue 'Indicate data has been read and stored
else
RCOK = cvFalse 'Indicate data has not been read
end if
LCD.Value = 254 'LCD Command
LCD.Value = 128 'LCD positioned at character 1
LCD.String = Str$(RCData1) + Str$(RCData2) + Str$(RCData3) + Str$(RCData4)
LCD.Value = 254 'LCD Command
LCD.Value = 192 'Second line, first character position
LCD.String = Str$(RCFormat) 'Write the Remote control manufacturer code
oopic.delay = 10 '0.1 second delay
End Sub
'**************************************************************************
Sub Manual()
'*******************************************************
'This routine determines how the code received
'from the remote control will effect the movement
'of the X4. (These codes are devised from a Sky
'digibox remote control. There are a large range
'remote controls that can be used. See the
'documentation for details.
'*******************************************************
Yel.Value = 1 'Yellow LED is on
Call Stop 'Stop the X4
Call Manual_Beep 'Sound the manual alert
Do
Call ReadRC 'Read the IRCM
'This If..Then statement decides what code received
'does what.
If (RCOK = cvTrue) AND (RCformat = 1) then
If (RCdata2 = 0) AND (RCdata3 = 2) then Call Forward
If (RCdata2 = 16) AND (RCdata3 = 2) then Call Backward
If (RCdata2 = 0) AND (RCdata3 = 1) then Call Spin_Right
If (RCdata2 = 16) AND (RCdata3 = 1) then Call Spin_Left
If (RCdata2 = 176) AND (RCdata3 = 0) then Call Stop
If (RCData2 = 176) AND (RCData3 = 3) then
Yel = 0
Exit Sub
End If
End If
Loop
End Sub
'**************************************************************************
Sub Manual_Beep()
'*******************************************************
'This routine is an alert when sent into
'manual mode.
'*******************************************************
Beep2.Beep(60000,10)
Beep2.Beep(61000,10)
Beep2.Beep(63000,10)
End Sub
'**************************************************************************
'**************************************************************************
|
