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
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-2009 Noetic Design, Inc. All Rights Reserved. Nubotics and WheelWatcher are trademarks of Noetic Design, Inc.