Example Source Code:
OOPIC II+ Simple Odometry
Description
This example demonstrates the
use of a virtual circuit that automatically stops the servo when the encoder
reaches a certain count.
It uses this prinicple to drive the robot in a 6" square pattern.
Download
ww01_oopic2p_square.osc (fixed line termination problem, 10/2/2005)
Source Code
'------------------------------------------------
'OOPIC II+ WheelWatcher Drive in a Square Example
'
' This example assumes the robot platform contains two RC servos modified for
' continuous rotation, two WW-01 boards, and one OOPIC II+ controller board.
' The IOLines used are appropriate for a MarkIII controller; adjust the IOLines
' to suit your robot.
'
' Noetic Design, Inc.
' Rev 1.00 - 7/12/2004
'
' NOTE: this example does not use the DIR and CLK lines -- it uses the OOPIC oQencode objects
' to do the same thing.
' IOLine 13 = Left WheelWatcher DIR
' IOLine 12 = Right WheelWatcher DIR
' IOLine 8 = Left WheelWatcher CLK
' IOLine 17 = Right WheelWatcher CLK (for PIC use RA4/Exp 1, but this is LSDA for the OOPIC, so use RC1 for OOPIC)
' IOLine 27 = Left CHA
' IOLine 26 = Left CHB
' IOLine 25 = Right CHA
' IOLine 24 = Right CHB
' IOLine 10 = Left Servo
' IOLine 9 = Right Servo
'
' NOTE: this example does not use the following line sensors or proximity sensors,
' beyond setting them up.
' IOLine 7 = Left Line Sensor
' IOLine 6 = Center Line Sensor (N/C)
' IOLine 5 = Right Line Sensor
' IOLine 4 = Left Eye (Left GP2D02)
' IOLine 3 = Right Eye (Right GP2D02)
'----------------------------------------
Dim lww As New oQencode
Dim rww As New oQencode
Dim ls As New oServo
Dim rs As New oServo
Dim rc As New oCompare
Dim lc As New oCompare
Dim lw As New oWire
Dim rw As New oWire
Dim rstop As New oWord
Dim lstop As New oWord
Dim re As New oA2D
Dim le As New oA2D
Dim rl As New oDio1
Dim ll As New oDio1
'-------------------------------------------
' Perform One Time Hardware Setup
'-------------------------------------------
Sub Setup()
oopic.pullup = 1 ' pull up (to logic high) the PIC B port lines using the PIC's weak internal pullups
' set up quadrature encoder objects
lww.IOLine1 = 27
lww.IOLine2 = 26
lww.Operate = cvTrue
rww.IOLine1 = 25
rww.IOLine2 = 24
rww.Operate = cvTrue
' set up servos
ls.IOLine = 10
ls.Center = 23 ' means 31 = 1.5ms, 0 = .638ms, 63 = 2.39ms
ls.Operate = cvTrue
ls = 31 ' neutral
rs.IOLine = 9
rs.Center = 23
rs.Operate = cvTrue
rs = 31 ' neutral
' set up the right virtual circuit
rstop = rww ' assume we are at the goal for now
rc.Input.Link(rww) ' compare the current right wheel watcher count
rc.ReferenceIn.Link(rstop) ' to the right stop variable
rc.Fuzziness = 8 ' allow up to an error of 8 ticks
rc.Operate = 1
rw.Input.Link(rc.Between) ' when rww is between rstop +/- fuzziness, then turn off the servo
rw.Output.Link(rs.Operate)
rw.InvertIn = 1 ' this inverts the sense of between -- servo is on when not between
rw.Operate = 1
' set up the left virtual circuit
lstop = lww ' assume we are at the goal for now
lc.Input.Link(lww) ' compare the current right wheel watcher count
lc.ReferenceIn.Link(lstop) ' to the right stop variable
lc.Fuzziness = 8 ' allow up to an error of 8 ticks
lc.Operate = 1
lw.Input.Link(lc.Between) ' when rww is between rstop +/- fuzziness, then turn off the servo
lw.Output.Link(ls.Operate)
lw.InvertIn = 1 ' this inverts the sense of between -- servo is on when not between
lw.Operate = 1
' set up ranging sensors (not used in this example):
le.IOLine = 0 ' same as 4
le.Operate = cvTrue
re.IOLine = 3
re.Operate = cvTrue
' set up line following sensors (not used):
ll.IOLine = 7
ll.Direction = cvInput
rl.IOLine = 5
rl.Direction = cvInput
End Sub
'-------------------------------------------
' SetRightDriveDistance
'
' This subroutine takes two parameters:
' dir: set to 1 for forward, 0 for backward
' dist: set in terms of encoder ticks; with
' the oQencode object, you get 64 ticks
' per wheel rotation.
'
' We use the following objects:
' rww: the quadrature encoder object, which counts
' encoder ticks
' rs: the right servo
' rstop: the goal value for the encoder count
' rc: a compare object, used to determine
' when the goal is reached
' rw: a wire, to connect the result of the
' comparison to the operation of the
' servo.
'
' How it works:
' We turn off the servo to ensure the motor is stopped.
' We connect the count of the encoder (rww)
' to the input of the comparison object.
' We calculate a stopping point based on the
' desired direction and distance, then
' link that to the comparison object too.
' We use the fuzziness feature of oCompare to
' tolerate a little error in the counter
' since the robot can't "stop on a dime."
' We then link, using a wire, the output of
' the comparison to the running of the servo.
' We invert the wire's value, so that the servo
' runs until the comparison is reached.
'-------------------------------------------
Sub SetRightDriveDistance(dir As Byte, dist As Word)
rs.Operate = 0
' NOTE: the right wheel counts backwards compared to the left, when moving the robot forwards,
' so we correct for that here. That way, dir = 1 means forward for both wheels.
if dir = 1 then
rstop = rww - dist
rs = 0
else
rstop = rww + dist
rs = 63
end if
End Sub
'-------------------------------------------
' SetLeftDriveDistance
'
'-------------------------------------------
Sub SetLeftDriveDistance(dirl as Byte, distl As Word)
ls.Operate = 0
if dirl = 0 then
lstop = lww - distl
ls = 0
else
lstop = lww + distl
ls = 63
end if
End Sub
'-------------------------------------------
' WaitDone
'
' Wait until both servos are turned of,
' which indicates that they have both reached
' their goals.
'-------------------------------------------
Sub WaitDone()
Do
if Not rs.Operate then
if Not ls.Operate then
exit do
end if
end if
Loop
End Sub
'-------------------------------------------
' Main
'
' This simple example uses the above subroutines to drive the robot in a
' 6" square pattern.
' The resulting pattern will be followed well, but errors will accumulate
' over time due to wheel slippage and lack of velocity and acceleration
' control, as well as the need to use the fuzziness feature of oCompare.
' A more complex example would include these as well.
'
' How do we calculate how many ticks to count on the encoders? Here's how.
' The example robot's wheel base is 3.5":
' 3.5 * PI ~= 11" wheel base circumference
' To spin in place 90 degrees, move each wheel by 1/4 of the wheel base
' circumference = 11/4 = 2.75", with both wheels turning in opposite directions.
' (Another option is to leave one wheel stopped, and move the other forward by
' 11/2 = 5.5".)
' Now we need to figure out how to convert this distance required into
' ticks of the encoder. The example robot's wheels are 2.75" in diameter:
' 2.75 * PI = 8.64" wheel circumference
' with oQencode, we get 64 ticks per turn, leaving:
' 64/8.64 = 7.40 ticks per inch
' so, to spin in place 1/4 turn, we need to count:
' 7.40 ticks per inch * 2.75" = 20 ticks.
' To move 6" forward, we need to count:
' 7.40 ticks per inch * 6" = 44 ticks.
'-------------------------------------------
Sub Main()
Setup
Do
' drive a distance
SetLeftDriveDistance(1, 44) ' drive both wheels forward 6"
SetRightDriveDistance(1, 44)
ls.Operate = 1 ' now, start both motors at the same time.
rs.Operate = 1
WaitDone
OOPIC.Delay = 85 ' give it time to stop.
' Spin in place 1/4 turn:
SetLeftDriveDistance(1, 20)
SetRightDriveDistance(0, 20) ' turn clockwise 1/4 turn
ls.Operate = 1
rs.Operate = 1
WaitDone
OOPIC.Delay = 85
' Alteratively, move just the left wheel:
' SetLeftDriveDistance(1, 41)
' ls.Operate = 1
' WaitDone
' OOPIC.Delay = 85
Loop
End Sub