Example Source Code:
Parallax BS2


Description

This example uses a Basic Stamp 2 with a WC-132, two WheelWatchers, and two servos, mounted on a BoeBot chassis.

It establishes communication with the WheelCommander at 9600 baud, with the slow-comms option enabled. It displays various information about the WheelCommander, then uses Set Angle and Set Distance commands to drive the robot in a square..
 

Download

wc132_bs2_demo.bs2

 

Source Code

'p8 - red led
'p9 - TO WC-132 Comm RXD, PIN 1
'p10 - rcv - to WC-132 Comm TXD, pin 3

'{$STAMP  BS2}        'STAMP directive (specifies a BS2E)
' {$PBASIC 2.5}
' {$PORT COM5}

velocity VAR Word
rotation VAR Word
distance VAR Word(2)
angle VAR Word
voltage VAR Word
channel VAR Byte
digital VAR Byte
difference VAR Word
sum VAR Word
backup VAR Bit

status VAR Word
angmode VAR status.BIT0
velmode VAR status.BIT1
dstmode VAR status.BIT2
rotmode VAR status.BIT3
done VAR status.BIT4
running VAR status.BIT5
i2cmode VAR status.BIT6
rs232mode VAR status.BIT7
moten VAR status.BIT8
cmdoverflow VAR status.BIT9
portoverflow VAR status.BIT10
delayedcoast VAR status.BIT11
ldir VAR status.BIT12
lclk VAR status.BIT13
rdir VAR status.BIT14
rclk VAR status.BIT15

recBytes VAR Byte(4)

' define which pins connect to which signals
RedLed PIN 8
Xmit PIN 9
Rcv PIN 10

'#DEFINE NORMAL_COMM = 1

DEBUG "starting up",CR
OUTPUT RedLed

GOTO main

getack:
  SERIN Rcv, 84, [STR recBytes\2]
  IF recBytes(0) <> "a" THEN
    DEBUG "cmd failed to get ack:", recBytes(0), CR
  ENDIF
  RETURN

Sync:
DO
  RedLed = 1
sendit:
  SEROUT Xmit, 84, ["."] ' 6 = 38400, 84 = 9600, 396 = 2400
  RedLed = 0
  SERIN Rcv, 84, 100, sendit, [STR recBytes\2]
  RedLed = 1
  IF recBytes(0) = "." THEN godone
  DEBUG "bad char: ", HEX recBytes(0), "H '", recBytes(0), "'",CR
LOOP
godone:
  RETURN

getname:
  RedLed = 1
  SEROUT Xmit, 84, ["N", LF] ' Send the name request
  SERIN Rcv, 84, [STR recBytes\4]
  'DEBUG "Device Name = ", recBytes(0), recBytes(1), recBytes(2), recBytes(3)
  SERIN Rcv, 84, [STR recBytes\2]
  recBytes(2) = 0
  'DEBUG recBytes(0), recBytes(1), CR
  RedLed = 0
  recBytes(3) = 0
  RETURN

getstatus:
  'GOSUB sync
  SEROUT Xmit, 84, ["S", LF]
  SERIN Rcv, 84, [WAIT("S"), HEX4 status, STR recBytes\1]
  'DEBUG "Status = ", HEX4 status, CR
  RETURN

waitdone:
  angle = 0
waitdone2:
  GOSUB getstatus
  IF done THEN DEBUG "motion complete ", CR
  IF done = 1 THEN RETURN
  PAUSE 20
  IF angle = status THEN waitdone2
' IF velmode THEN DEBUG "vel "
' IF dstmode THEN DEBUG "dst "
' IF delayedcoast THEN DEBUG "cst "
' IF running THEN DEBUG "run "
' DEBUG "s=", HEX4 status, CR
  angle = status
  GOTO waitdone2
  RETURN

setwaitmode:
  SEROUT Xmit, 84, ["F028B", LF] ' mode = counter rotating axes, inverted platform polarity, notify, slow comms
  GOSUB getack
  RETURN

setnowaitmode:
  SEROUT Xmit, 84, ["F0289", LF] ' mode = counter rotating axes, inverted platform polarity, slow comms
  GOSUB getack
  RETURN

coast:
  SEROUT Xmit, 84, ["C", LF]
  GOSUB getack
  RETURN

reset:
  SEROUT Xmit, 84, ["R", LF]
  GOSUB getack
  RETURN

setvelocity:
  'DEBUG "entering set velocity = ", DEC velocity, CR
  SEROUT Xmit, 84, ["V", HEX4 velocity, LF]
  'DEBUG "sent command; waiting for status", CR
  GOSUB getack
  RETURN

getvelocity:
  SEROUT Xmit, 84, ["V", LF]
  SERIN Rcv, 84, [WAIT("V"), HEX4 velocity, STR recBytes\1]
  'DEBUG "velocity = ", DEC velocity, CR
  RETURN

setacceleration:
  'DEBUG "entering set acceleration = ", DEC acceleration, CR
  SEROUT Xmit, 84, ["A", HEX4 velocity, LF]
  'DEBUG "sent command; waiting for status", CR
  GOSUB getack
  RETURN

getacceleration:
  SEROUT Xmit, 84, ["A", LF]
  SERIN Rcv, 84, [WAIT("A"), HEX4 velocity, STR recBytes\1]
  'DEBUG "acceleration = ", DEC acceleration, CR
  RETURN

setrotation:
  SEROUT Xmit, 84, ["Y", HEX4 rotation, LF]
  GOSUB getack
  RETURN

getrotation:
  SEROUT Xmit, 84, ["Y", LF]
  SERIN Rcv, 84, [WAIT("Y"), HEX4 rotation, STR recBytes\1]
  'DEBUG "rotation = ", DEC rotation, CR
  RETURN

setdistance:
  SEROUT Xmit, 84, ["X", HEX4 distance(1), HEX4 distance(0), LF]
  GOSUB getack
  DEBUG "set distance = ", DEC distance(1), DEC distance(0), CR
  RETURN

getdistance:
  SEROUT Xmit, 84, ["D", LF]
  SERIN Rcv, 84, [WAIT("D"), HEX4 distance(1), HEX4 distance(0), STR recBytes\1]
  DEBUG "get distance = ", DEC distance(1), DEC distance(0), CR
  RETURN

setangle:
  SEROUT Xmit, 84, ["W", HEX4 angle, LF]
  GOSUB getack
  DEBUG "set angle = ", DEC angle, CR
  RETURN

getangle:
  SEROUT Xmit, 84, ["W", LF]
  SERIN Rcv, 84, [WAIT("W"), HEX4 angle, STR recBytes\1]
  DEBUG "get angle = ", DEC angle, CR
  RETURN

go:
  SEROUT Xmit, 84, ["G", LF]
  GOSUB getack
  RETURN

getservovoltage:
  SEROUT Xmit, 84, ["H07", LF]
  SERIN Rcv, 84, [WAIT("H07"), HEX4 voltage, STR recBytes\1]
  voltage = (voltage.HIGHBYTE * 5) + (voltage.LOWBYTE / 51)
  DEBUG "servo voltage = ", DEC voltage, CR
  RETURN

getlogicvoltage:
  SEROUT Xmit, 84, ["H06", LF]
  SERIN Rcv, 84, [WAIT("H06"), HEX4 voltage, STR recBytes\1]
  voltage = (voltage.HIGHBYTE * 5) + (voltage.LOWBYTE / 51)
  DEBUG "logic voltage = ", DEC voltage, CR
  RETURN

getanalog:
  SEROUT Xmit, 84, ["H", HEX2 channel, LF]
  SERIN Rcv, 84, [WAIT("H"), HEX2 channel, HEX4 voltage, STR recBytes\1]
'  DEBUG "channel ", DEC channel, " voltage = ", DEC voltage, CR
  RETURN

setdigital:
  SEROUT Xmit, 84, ["J", HEX1 channel, HEX1 digital, LF]
  GOSUB getack
  'DEBUG "channel ", DEC channel, " digital = ", DEC digital, CR
  RETURN

getdigital:
  SEROUT Xmit, 84, ["J", HEX2 channel, LF]
  SERIN Rcv, 84, [WAIT("J"), HEX2 channel, HEX2 digital, STR recBytes\1]
  'DEBUG "channel ", DEC channel, " digital = ", DEC digital, CR
  RETURN

initfw:
  SEROUT Xmit, 6, ["F0302", LF] ' at 38400, set baud = 9600
  GOSUB getack
  SEROUT Xmit, 84, ["F0289", LF] ' mode = counter rotating axes, inverted platform polarity, slow comms
  GOSUB getack
  RETURN


Main:
  'GOSUB initfw
  GOSUB sync
  DEBUG "WheelCommander Synchronized", CR
  GOSUB getname
  DEBUG "Clearing Notify Mode", CR
  GOSUB setnowaitmode
  DEBUG "Coast", CR
  GOSUB coast
  DEBUG "Reset", CR
  GOSUB reset
  DEBUG "Get Status", CR
  GOSUB getstatus
  DEBUG "Get Servo Voltage", CR
  GOSUB getservovoltage
  DEBUG "Get Logic Voltage", CR
  GOSUB getlogicvoltage

  DEBUG "Digital pins: ", CR
  FOR channel = 0 TO 3
    GOSUB getdigital
    DEBUG "channel ", DEC channel, " digital = ", DEC digital, CR
  NEXT

  velocity = 140
  GOSUB setacceleration
  velocity = 70
  GOSUB setvelocity

Loop4:
  GOSUB getdistance
  RedLed = 1
  angle = 0
  GOSUB setangle
  distance(0) = 120
  distance(1) = 0
  GOSUB setdistance
  GOSUB go
  GOSUB waitdone
'  GOSUB coast
  GOSUB getdistance
  RedLed = 0
  angle = 90
  GOSUB setangle
  distance(0) = 0
  distance(1) = 0
  GOSUB setdistance
  GOSUB go
  GOSUB waitdone
'  GOSUB coast
  GOTO LOOP4

  END

 

 © 2004-2013 Noetic Design, Inc. All Rights Reserved. Nubotics, Unicoder, WheelCommander, and WheelWatcher are trademarks of Noetic Design, Inc.