Example Source Code:
Parallax BS2SX
Description
This example uses a Basic Stamp 2SX with two WheelWatchers, two proximity detectors (can be either bumpers or IR), and a FerretTronics FT639 Serial Servo Controller Chip.
The FT639 can easily be replaced with some other external servo controller, such as Parallax's own 'Parallax Servo Controller.' Either way, an external servo controller takes a large burden off of the Basic Stamp architecture, as it is not designed to output servo control pulses in the background.
Another thing to try is the Parallax PWMPAL. It provides both hardware pulse generation for DC motors (e.g., stripped servos) or servos AND four 16 bit hardware counters, accessible over a serial line from the Stamp. This should allow you to use the CLK lines and get good odometry, even on a Basic Stamp.
Currently, this example only counts the encoders, but does not yet do anything with the counts. Stay tuned for more improvements.
Download
Source Code
'{$STAMP BS2sx}
'{$PBASIC 2.5}
'==This program is an example of using a BS2SX and
'==pbasic 2.5 with WheelWatcher encoders.
'==copyright by John Gregory, 7/16/2004
'--this source or portions thereof may not be used
'--for commercial purposes.
'--this source or portions thereof may be copied for
'--personal use
'--provided for use as a WheelWatcher example program
'--for Noetic Design, Inc.
Encoder_Variables:
L_cnt VAR Word 'left encoder counter
R_cnt VAR Word 'right encoder counter
X_cnt VAR Word 'left encoder state
y_cnt VAR Word 'left encoder state
L_cnt_dir VAR Word 'left counter direction
R_cnt_dir VAR Word 'right counter direction
L_velocity_cnt VAR Word
R_velocity_cnt VAR Word
prev_Ldir VAR Byte
prev_Rdir VAR Byte
lchk_cntl VAR Byte
rchk_cntl VAR Byte
Chg_dir VAR Byte
sensors VAR Nib
encoder VAR Nib
Main:
INPUT 4 ' left WW-01 ChA
INPUT 5 ' right WW-01 ChA
INPUT 6 ' left WW-01 DIR
INPUT 7 ' right WW-01 DIR
Chg_dir = -1
L_cnt_dir = 1
R_cnt_dir = 1
'‘====================================
cntl PIN 0
baud CON 17405
header CON 106
pulse CON 90
GOSUB setup 'these steps initialize the FT639
GOSUB setpulse '
GOSUB setheader '
GOSUB setactive '
DO
GOSUB Chk_sensors
GOSUB Motor_cntl
GOSUB Chk_encoders
LOOP
'--- these sensors may be any object detection type
'--- (IR or bumper)
Chk_sensors:
sensors.BIT0 = IN1
sensors.BIT1 = IN2
RETURN
Motor_cntl:
ON sensors GOSUB backward, lefturn, righturn, forward
RETURN
'--These routines check the encoders (left and right)
'--and count up or down depending on if the direction
'--changed.
'---this program is set to check for a falling
'---edge on the encoder ChA line. (in4/5)
'---the velocity_cnt variable may be used to measure
'---the width of the clock pulse but the way it is
'---used here will not be very acurate...
Chk_encoders:
'-- 4 & 5 are connected to the ChA pins of
'-- each encoder.
x_cnt = IN4
y_cnt = IN5
'-- check left encoder -------
IF x_cnt = 0 THEN
l_cnt = l_cnt + L_cnt_dir
ENDIF
'-- check right encoder --------
IF y_cnt = 0 THEN
r_cnt = r_cnt + R_cnt_dir
ENDIF
GOSUB chk_Ldir
GOSUB chk_Rdir
RETURN
'--The Ldir/Rdir routines result in a cnt_dir
'--being a +1 or -1. These variables are then
'--used to add or subtract 1 from the L_cnt
'--and R_cnt variables for total pulses counted.
'-- check the Dir line on the left encoder --
'-- look for a change (comparing to prev_Ldir) --
chk_Ldir:
IF ((IN6 = 1) AND (prev_Ldir = 0) OR (IN6 = 0 AND prev_Ldir = 1)) THEN
prev_Ldir = IN6
L_cnt_dir = L_cnt_dir * Chg_dir
ENDIF
RETURN
'-- check the Dir line on the right encoder --
'-- look for a change (comparing to prev_Rdir) --
chk_Rdir:
IF ((IN7 = 1) AND (prev_Rdir = 1) OR (IN7 = 0 AND prev_Rdir = 1)) THEN
prev_Rdir = IN7
R_cnt_dir = R_cnt_dir * Chg_dir
ENDIF
RETURN
'---begin section = servo setup/contol
' Subroutines used to set up the FT639 Serial Servo Controller Chip
' (http://www.ferrettronics.com/docs/FT639/ft639.pdf)
'-- substitute any code needed here
'-- for whatever servo controller you
'-- are using
setactive:
SEROUT cntl,baud,[117]
SEROUT cntl,baud,[117] 'sets active mode
RETURN
setpulse:
SEROUT cntl,baud,[pulse] 'sets pulse length
RETURN
setup:
SEROUT cntl,baud,[122] 'sets setup mode
RETURN
setheader:
SEROUT cntl,baud,[header] 'sets header length
RETURN
'--motion subroutines
lefturn:
SEROUT cntl,baud,[1,131] 'starts L (no. 1) servo fwd slowly
SEROUT cntl,baud,[31,159] 'starts R (no. 2) servo fwd quickly
PAUSE 2000
RETURN
righturn:
SEROUT cntl,baud,[0,128] 'starts L (no. 1) servo fwd quickly
SEROUT cntl,baud,[16,140] 'starts R (no. 2) servo fwd slowly
PAUSE 2000
RETURN
backward:
RETURN
forward:
SEROUT cntl,baud,[0,128] 'L (no. 1) servo fwd full
SEROUT cntl,baud,[31,159] 'R (no. 2) servo fwd full
RETURN
'---====end section = servo setup/control
© 2004-2011 Noetic Design, Inc. All Rights Reserved. Nubotics and WheelWatcher are trademarks of Noetic Design, Inc.