Basic Stamp II code: (May 6, 2001 5:00 pm)

' Paul Contino

' Personal Robot Technologies, Inc.

' May 6, 2001

' Movement simulation for plotting on PC

' {$STAMP BS2} ' Code programmed for BS2

'-----Declarations-----

encoder_count var byte ' Current count on the wheel encoder

encoder_dir var byte ' Wheel direction as read by encoder

instruction var byte ' Variable stores serial-in instruction

direction var byte ' Direction to move

pulse_count var byte ' Number of pulses to send to servo

right_width var word ' Variable stores right pulse width

left_width var word ' Variable stores left pulse width

loop_count var word ' Servo movement loop counter

'-----Initialization-----

encoder_dir = "F" ' Default encoder direction

encoder_count = 0 ' Default encoder count

pulse_count = 0 ' Initialize servo pulse count

left_width = 751 ' No initial movement

right_width = 753

'-----Main Routine-----

main: ' Main routine

' Designated input Pin 16, 9600 baud

' Wait until "A", receive direction instruction, receive pulse count

serin 16,16468,[WAIT("A"),instruction,pulse_count]

pause 1000 ' Wait 1000ms

' Determine direction to move BOEBot in

lookdown instruction, = ["B","L","R","F","Q"], direction

branch direction, [backward,left_turn,right_turn,forward,quit]

backward: right_width = 770: left_width = 720: goto polling

left_turn: right_width = 720: left_width = 720: goto polling

right_turn: right_width = 770: left_width = 770: goto polling

forward: right_width = 720: left_width = 770: goto polling

quit: stop ' End program

'-----Subroutines-----

polling: ' Polling subroutine allows for sending of

' pulses to servos as well as check on

' wheel encoder

for loop_count = 1 to pulse_count ' Determines distance to travel

gosub pulses ' Send pulse to servo

gosub encoders ' Check encoder data

next ' Jump to top of FOR loop

serout 16,16468,["END"] ' Let user know when movement is done

encoder_count = 0 ' Reset count -- change of direction

goto main ' Return to main program

pulses:

pulsout 12, right_width ' Right servo

pulsout 13, left_width ' Left servo

pause 20 ' Wait 20 ms

return ' Return to polling routine

encoders: ' Read in wheel encoder data from LS7166

encoder_count = encoder_count + 1 ' Simulation encoder pulse

' Send encoder count to PC for analysis - key on "!"

serout 16,16468,[DEC encoder_count,"!"]

'pause 1000 ' Wait 500ms

return