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:

  1. replace wheels with Acroname wheels which are designed for our codewheel stickers; the Parallax wheels' hubs are too large for the sticker to fit
  2. 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:


 

Download

ww01_bs2_boebot.bs2

 

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