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
'p0 - red led
'p1 - xmit - to WC-116 Comm RXD, pin 1
'p2 - rcv - to WC-116 Comm TXD, pin 3
'{$STAMP BS2} 'STAMP directive (specifies a BS2E)
' {$PBASIC 2.5}
' {$PORT COM1}
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)
RedLed PIN 0
Xmit PIN 1
Rcv PIN 2
'#DEFINE NORMAL_COMM = 1
DEBUG "starting up",CR
OUTPUT RedLed
GOTO main
getack:
SERIN 2, 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 1, 84, ["."] ' 6 = 38400, 84 = 9600, 396 = 2400
RedLed = 0
SERIN 2, 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 1, 84, ["N", LF] ' Send the name request
SERIN 2, 84, [STR recBytes\4]
'DEBUG "Device Name = ", recBytes(0), recBytes(1), recBytes(2), recBytes(3)
SERIN 2, 84, [STR recBytes\2]
recBytes(2) = 0
'DEBUG recBytes(0), recBytes(1), CR
RedLed = 0
recBytes(3) = 0
RETURN
getstatus:
'GOSUB sync
SEROUT 1, 84, ["S", LF]
SERIN 2, 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 1, 84, ["F028B", LF] ' mode = counter rotating axes, inverted platform polarity, notify, slow comms
GOSUB getack
RETURN
setnowaitmode:
SEROUT 1, 84, ["F0289", LF] ' mode = counter rotating axes, inverted platform polarity, slow comms
GOSUB getack
RETURN
coast:
SEROUT 1, 84, ["C", LF]
GOSUB getack
RETURN
reset:
SEROUT 1, 84, ["R", LF]
GOSUB getack
RETURN
setvelocity:
'DEBUG "entering set velocity = ", DEC velocity, CR
SEROUT 1, 84, ["V", HEX4 velocity, LF]
'DEBUG "sent command; waiting for status", CR
GOSUB getack
RETURN
getvelocity:
SEROUT 1, 84, ["V", LF]
SERIN 2, 84, [WAIT("V"), HEX4 velocity, STR recBytes\1]
'DEBUG "velocity = ", DEC velocity, CR
RETURN
setacceleration:
'DEBUG "entering set acceleration = ", DEC acceleration, CR
SEROUT 1, 84, ["A", HEX4 velocity, LF]
'DEBUG "sent command; waiting for status", CR
GOSUB getack
RETURN
getacceleration:
SEROUT 1, 84, ["A", LF]
SERIN 2, 84, [WAIT("A"), HEX4 velocity, STR recBytes\1]
'DEBUG "acceleration = ", DEC acceleration, CR
RETURN
setrotation:
SEROUT 1, 84, ["Y", HEX4 rotation, LF]
GOSUB getack
RETURN
getrotation:
SEROUT 1, 84, ["Y", LF]
SERIN 2, 84, [WAIT("Y"), HEX4 rotation, STR recBytes\1]
'DEBUG "rotation = ", DEC rotation, CR
RETURN
setdistance:
SEROUT 1, 84, ["X", HEX4 distance(1), HEX4 distance(0), LF]
GOSUB getack
DEBUG "set distance = ", DEC distance(1), DEC distance(0), CR
RETURN
getdistance:
SEROUT 1, 84, ["D", LF]
SERIN 2, 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 1, 84, ["W", HEX4 angle, LF]
GOSUB getack
DEBUG "set angle = ", DEC angle, CR
RETURN
getangle:
SEROUT 1, 84, ["W", LF]
SERIN 2, 84, [WAIT("W"), HEX4 angle, STR recBytes\1]
DEBUG "get angle = ", DEC angle, CR
RETURN
go:
SEROUT 1, 84, ["G", LF]
GOSUB getack
RETURN
getservovoltage:
SEROUT 1, 84, ["H07", LF]
SERIN 2, 84, [WAIT("H07"), HEX4 voltage, STR recBytes\1]
voltage = (voltage.HIGHBYTE * 5) + (voltage.LOWBYTE / 51)
DEBUG "servo voltage = ", DEC voltage, CR
RETURN
getlogicvoltage:
SEROUT 1, 84, ["H06", LF]
SERIN 2, 84, [WAIT("H06"), HEX4 voltage, STR recBytes\1]
voltage = (voltage.HIGHBYTE * 5) + (voltage.LOWBYTE / 51)
DEBUG "logic voltage = ", DEC voltage, CR
RETURN
getanalog:
SEROUT 1, 84, ["H", HEX2 channel, LF]
SERIN 2, 84, [WAIT("H"), HEX2 channel, HEX4 voltage, STR recBytes\1]
' DEBUG "channel ", DEC channel, " voltage = ", DEC voltage, CR
RETURN
setdigital:
SEROUT 1, 84, ["J", HEX1 channel, HEX1 digital, LF]
GOSUB getack
'DEBUG "channel ", DEC channel, " digital = ", DEC digital, CR
RETURN
getdigital:
SEROUT 1, 84, ["J", HEX2 channel, LF]
SERIN 2, 84, [WAIT("J"), HEX2 channel, HEX2 digital, STR recBytes\1]
'DEBUG "channel ", DEC channel, " digital = ", DEC digital, CR
RETURN
initfw:
SEROUT 1, 6, ["F0302", LF] ' at 38400, set baud = 9600
GOSUB getack
SEROUT 1, 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 7
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-2008 Noetic Design, Inc. All Rights Reserved. Nubotics and WheelWatcher are trademarks of Noetic Design, Inc.