Total Robots - Example - TR2 Rover With Wireless Control Module (WCM) and DS-WHC.

TR2 Rover With Wireless Control Module (WCM)
and DS-WHC.

The following example application uses the TR2 Rover connected to a Local WCM. The Rover can be manually overridden using a Wireless Hand Control (DS-WHC).

This example demonstrates an improved servo sweep from the TR1 Rover with servo sweep example. The new improved program allows the rover to continuously move forward while the servo sweep motion occurs. When an obstruction is encountered, the rover stops, moves backwards, then looks left and right. It decides which is the clearest way, then moves in that direction.

The addition of the WCM and WHC means a remote link can be made from a hand held remote control to the TR2 enabling a manual control mode to override the autonomous mode.
The WHC Should have its remote node set-up to 2. Its local node address is always 253.

Send these commands from the WHC to control the TR2:

Overide = ‘4’
Right = ‘8’
Left = ‘7’
Forward = ‘5’
Stop = ‘9’
Backward = ‘6’

Send these in any order to manipulate the TR2. When you want to return the rover back to autonomous mode, send decimal ‘14’ and the TR2 rover will return to this state.

Connections
WCM
Connect the WCM I2C Input to the I2C Bus on the DS-SXM16S module (Just connect SCL, SDA and GND).
WCM Power can also be taken from the DS-SXM16S module.
Address Jumpers set to ‘00’ (see WCM data sheet).
Pull up Jumpers not set (pull ups not used).
TR2 Rover
Connected as shown in TR2 Rover Data Sheet.

For further details please see the corresponding data sheets.

'****************************************************************
'****************************************************************
'**Description: TR2 Rover with wireless manual overide using the DS-WHC.
'**This program utilises a Servo sweep routine which improves the
'**obstacle avoidance capabilities of the TR1 Rover. If the rover
'**gets stuck. A manual overide command can be sent from the DS-WHC
'**remote control allowing complete user control from the remote of
'**the rovers movements.
'**
'** Additional Components:- 1 DS-WCM, 1 DS-WHC
'** Compiler Version: 4.0.0
'** Created: 22/11/02 Revision: 2.00
'** Written by: Total Robots (Jamie Finnan)
'** Adapted from TR2 Rover with wireless manual overide.
'**
'** TR-021
'**
'****************************************************************
'****************************************************************
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
Dim WCM As New oi2c
Dim LED As New oDio1
Dim WriteVal As New oByte 'Value to write to remote node
Dim LocNode As New oByte 'Local node address
Dim RemNode As New oByte 'Remote node address to write too
Dim ReadVal As New oByte 'Value read from remote node
Dim StoreVal As New oByte
Dim ReadAdr As New oByte 'Node address of remote node
Dim Error As New obit 'value came from
Dim OError As New oBit
Dim ReadOK As New oBit
Dim Done As New oBit
'****************************************************************
'****************************************************************
Sub Main()
Call Initialise 'Setup
LocNode = 2 'Define local node address (0-255)
RemNode = 253 'Define remote node address (0-255)
Const WriteTOut = 2 'Define write timeout in seconds (0-15)
Const WCMAdr = &h60 'WCM A0 & A1 jumpers ON (Range &h60-&h63)
Call SetUpWCM 'Setup WCM ready for communication
do
call ReadWCM 'Wait for data from remote node
If StoreVal = 4 then
Call Manual
End if
If OError = cvTrue then 'If read overflow error then
ReadVal = ReadVal XOR &hff 'Invert to show error
end if
Call Sweep
Loop
End Sub
'****************************************************************
Sub SetUpWCM()
'Set the DS-WCM I2C address shifted right by 1 bit
WCM.Node = WCMAdr 'Setup I2C address for WCM
'Setup I2C addressing to WCM
WCM.Width = cv8bit 'Control Info is 1-byte
WCM.Mode = cv10bit 'I2C mode is 10-Bit Addressing
WCM.NoInc = cvFalse 'Increment on every read/write
WCM.Location = 0 'Specifiy local node addresslocation
WCM.Value = LocNode 'and write local node address
End Sub
'****************************************************************
'Read data from the Remote WCM.
Sub ReadWCM()
WCM.Location = 2 'Start at read status flag (R2)
If WCM.Value = 255 then 'Has data been received (R2) ?
If WCM.Value = 0 then 'If so has overflow error
'occured (R3) ?
ReadAdr = WCM.Value 'If not then store WCM address data
'came from.
ReadVal = WCM.Value 'and store data
StoreVal = ReadVal
OError = cvFalse 'Indicate no overflow error
ReadOK = cvTrue
'Indicate data has been read and stored
else
OError = cvTrue 'Indicate overflow error has occured
ReadOK = cvTrue 'Indicate data has been read
ReadAdr = WCM.Value 'Store WCM address data came from
ReadVal = WCM.Value 'and store data
end if
else
OError = cvFalse 'If no data read then no error
ReadOK = cvFalse 'and no data
end if
End Sub
'****************************************************************
'Manual mode when the correct data has been sent.
Sub Manual()
Call Servo_Stop
Do
Call ReadWCM
If StoreVal = 8 then
Call Servo_MoveLeft 'This turns the rover right.
StoreVal = 0 'Clear Read Value
elseif StoreVal = 7 then
Call Servo_MoveRight 'This turns the rover left.
StoreVal = 0 'Clear Read Value
elseif StoreVal = 5 then
Call Servo_MoveForward 'This moves the rover forward.
StoreVal = 0 'Clear Read Value
elseif StoreVal = 6 then
Call Servo_MoveBackward'This moves rover backwards.
StoreVal = 0 'Clear Read Value.
elseif StoreVal = 9 then
Call Servo_Stop 'This stops any servo movement.
StoreVal = 0 'Clear Read Value.
elseif StoreVal = 14 then
StoreVal = 0 'Clear Read Value.
Exit Sub 'Exit the subroutine and set
End if 'rover back into autonomous mode.
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 > 50 Then 'Is Servo at end of Sweep?
Call Sweep_Back
End If
Call ReadWCM
If StoreVal = 4 then
Call Manual
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<50) then 'If sonar detects anything within 50cm
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
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()
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:
Call Servo_MoveBackward 'Move rover backwards
OOPic.delay = 150
Call Servo_Stop
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
OOPic.delay = 100
Call Servo_MoveForward 'Move rover forward
else
If Left_Test.Value = 0 Then 'Move to the left if clear
Call Servo_MoveRight
OOPic.delay = 100
Call Servo_MoveForward 'Move rover forward
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 = 55 '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
Left_Servo.Value=cLeftForward
Right_Servo.Operate = cvTrue
Left_Servo.Operate = cvTrue
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
End Sub
'****************************************************************
Sub Servo_MoveRight() 'Routine to move rover right
Call Servo_Stop
Left_Servo.Value = cLeftBackward
Right_Servo.Value = cRightForward
Left_Servo.Operate = cvTrue
Right_Servo.Operate = cvTrue
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