Example Source Code:
Parallax BS2 / Boe-Bot
Photo Gallery of Example Robot
Description
This example uses a Basic Stamp
2 with two WheelWatchers, on a Parallax Boe-Bot. Somewhat surprisingly, we are
able to pulse the servos while also counting both edges of the Channel A quadrature
signal and monitoring the hardware-decoded direction line. These actions are used
to implement a drive-in-a-square movement.
In order to use the WW-01 encoders on a Boe-Bot platform, certain changes were
required:
- replace wheels with Acroname wheels which are designed for our codewheel stickers; the Parallax wheels' hubs are too large for the sticker to fit
- mount the servos outside of the chassis rather than inside, so that our standard
spacers can be used to mount the WW-01 circuit boards at the correct height
The WW-01 encoders were wired up so that, for each wheel, the BS2 acesses the Channel A and Direction lines, not the Channel B nor the Clock lines. The reason is that performing a full decode of Channel A and Channel B for each wheel would take too much time for the BS2, but, the Clock line produces pulses that are too short for the BS2 to detect. Instead, we can take advantage of the decoded Direction line along with the more-slowly-changing Channel A signal (slow compared to the Clock line), and still get 64 pulses per revolution of the wheel, which is not too bad.
The connections are:
- BS2 Pin 0: Left Channel A (yellow)
- BS2 Pin 1: Left Direction (orange)
- BS2 Pin 2: Right Channel A (yellow)
- BS2 Pin 3: Right Direction (orange)
- Left and Right Vdd (red) to the Vdd proto board holes
- Left and Right Vss (black) to the Vss proto board holes
Download
Source Code
'{$STAMP BS2}
'{$PBASIC 2.5}
'== This program is an example of using a BS2 and
'== pbasic 2.5 with Nubotics WheelWatcher encoders on a BoEBot.
'== This program is based on an example by John Gregory (c) 2004,
'== for BS2SX and a serial servo controller.
'== This example improves on his by driving the servos
'== directly from the BS2, while also counting
'== encoder clicks to measure distance. This is used to
'== drive the robot in a square.
' -----[ I/O Definitions ]------------------------------------------------------
LChA PIN 0 ' left encoder channel A; pulses 64 times per wheel rotation
LDir PIN 1 ' left encoder direction flag
RChA PIN 2 ' right encoder channel A
RDir PIN 3 ' right encoder direction flag
LMotor CON 12 ' left servo motor
RMotor CON 13 ' right servo motor
' -----[ Constants ]------------------------------------------------------------
' (borrowed from the Parallax SumoBot main competition program example)
LFwdFast CON 500 ' left motor forward; fast
LFwdSlow CON 700 ' left motor forward; slow
LStop CON 750 ' left motor stop
LRevSlow CON 800 ' left motor reverse; slow
LRevFast CON 1000 ' left motor reverse; fast
RFwdFast CON 500 ' right motor forward; fast
RFwdSlow CON 700 ' right motor forward; slow
RStop CON 750 ' right motor stop
RRevSlow CON 800 ' right motor reverse; slow
RRevFast CON 1000 ' right motor reverse; fast
' -----[ Variables ]------------------------------------------------------------
L_dist VAR Word ' left encoder counter
R_dist VAR Word ' right encoder counter
Cur_LChA VAR Bit ' current state of left channel A
Cur_RChA VAR Bit ' state of right A
Prev_LChA VAR Bit ' previous state of left channel A
Prev_RChA VAR Bit ' previous state of right A
Counter VAR Byte ' stop pulse counter
' -----[ Main Routine ]---------------------------------------------------------
Main:
' set up inputs from encoders
INPUT 0
INPUT 1
INPUT 2
INPUT 3
' initialize state variables
Prev_LChA = LChA
Prev_RChA = RChA
' zero encoder counts
L_dist = 0
R_dist = 0
DEBUG "Nubotics WW-01 BS2 BoeBot Drive in a Square Test", CR
'gosub Encoder_Test
' drive in a square forever
DO
GOSUB Forward_OneRot
GOSUB Stop_Motion
GOSUB Turn_Left
GOSUB Stop_Motion
LOOP
END
'------------------------------
'- Controlled Motion Routines -
'- -
'- These routines use the -
'- WW-01 encoders to move -
'- the wheels fixed distances -
'- regardless of battery level-
'- or servo absolute speed. -
'------------------------------
Forward_OneRot:
DEBUG "Forward 1 wheel rotation", CR
DO
GOSUB Fast_Fwd
GOSUB Read_Encoders
' stop when we see 64 ticks of ChA = 1 rotation; but don't be confused
' by negative numbers on L_dist, which can happen for brief periods
' while changing motor speed and/or direction
IF (L_dist >= 64) AND (L_dist < 32767) THEN EXIT
LOOP
RETURN
Stop_Motion:
DEBUG "Stopping at L_dist=", SDEC L_dist, " R_dist=", SDEC R_dist, CR
GOSUB Stop_Servos
L_dist = 0
R_dist = 0
RETURN
Turn_Right:
DEBUG "Turn right 90 degrees", CR
DO
GOSUB Spin_Right
GOSUB Read_Encoders
' stop when we see 52 ticks of ChA = 1 rotation of the left wheel,
' while the right wheel is stopped; this is twice as accurate as
' spinning left forward and right back at the same speed, as the distance
' each wheel travels in that case is 1/2 as far, giving us 1/2 the
' number of encoder ticks than we do here
IF (L_dist >= 52) AND (L_dist < 32767) THEN EXIT
LOOP
RETURN
Turn_Left:
DEBUG "Turn left 90 degrees", CR
DO
GOSUB Spin_Left
GOSUB Read_Encoders
IF (R_dist >= 52) AND (R_dist < 32767) THEN EXIT
LOOP
RETURN
'--------------------------------
'- Uncontrolled Motion Routines -
'- -
'- These helper routines pulse -
'- the servo motors the right -
'- amount of time to move the -
'- base in a particular way. -
'--------------------------------
Fast_Fwd:
PULSOUT LMotor, LFwdFast
PULSOUT RMotor, RFwdFast
RETURN
Spin_Right:
PULSOUT LMotor, LFwdSlow
PULSOUT RMotor, RStop
RETURN
Spin_Left:
PULSOUT LMotor, LStop
PULSOUT RMotor, RFwdSlow
RETURN
Stop_Servos:
FOR Counter = 1 TO 10 ' stop motors
PULSOUT LMotor, LStop
PULSOUT RMotor, RStop
PAUSE 20
NEXT
RETURN
'--------------------------------
'- Read Encoders -
'- -
'- This routine samples the -
'- channel A quadrature signals,-
'- then compares them to their -
'- previous states. If they -
'- have changed, then use the -
'- direction line to either -
'- increase or decrease the -
'- distance variables. -
'--------------------------------
Read_Encoders:
'-- check left encoder -------
Cur_LChA = LChA
Cur_RChA = RChA
IF Cur_LChA <> Prev_LChA THEN
IF LDir = 0 THEN ' left wheel's direction is forward when LDir = 0
L_dist = L_dist + 1
ELSE
L_dist = L_dist - 1
ENDIF
Prev_LChA = Cur_LChA
ENDIF
'-- check right encoder --------
IF Cur_RChA <> Prev_RChA THEN
IF RDir = 1 THEN ' right wheel's direction is forward when RDir = 1
R_dist = R_dist + 1
ELSE
R_dist = R_dist - 1
ENDIF
Prev_RChA = Cur_RChA
ENDIF
RETURN
' call this when you first hook up your encoders;
' watch the counts as you turn the wheels, and make sure
' you get 64 counts per rotation forward and backward
' for each encoder
Encoder_Test:
DO
GOSUB Read_encoders
DEBUG "L_dist=", SDEC L_dist, " R_dist=", SDEC R_dist, CR
LOOP
RETURN ' never does