Basic Stamp II code: (April 29, 2001 5:00 pm)

' Movement Control - April 29, 2001

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

loop_count var byte ' Servo movement loop counter

pulse_count var word ' Used to set number of pulses delivered

right_width var word ' Variable stores right pulse width

left_width var word ' Variable stores left pulse width

instruction var byte ' Variable stores serial-in instruction

direction var byte ' Variable stores direction to move

l_wheel_count var byte ' Stores left wheel counts

r_wheel_count var byte ' Stores right wheel counts

wheel_direction var byte ' Determines direction of robot (0 or 1)

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

high 11 ' Set P11 to high so LED does not light (OUTPUT)

low 12 ' Set P12 to low (INPUT)

low 13 ' Set P13 to low (INPUT)

low 8 ' No transmission output allowed

low 0 ' No transmission input allowed

pulse_count = 0 ' Initialize servo pulse count

left_width = 751 ' No initial movement

right_width = 753

l_wheel_count = 0 ' Initialize wheel encoder counters

r_wheel_count = 0

wheel_direction = 1 ' Initialize robot moving forward

' LS7166 initialization

' Clear counter

' Set to X1 mode

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

main: ' Main routine

high 11 ' Dim LED (OUTPUT)

low 8 ' No voltage to transmitter BS2-side . . . prohibit output

high 0 ' Allow for input transmission

' Designated input Pin 0, 2400 baud

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

serin 0,16780,[WAIT("A"),instruction,DEC pulse_count]

' serin 0,396,[WAIT("A"),instruction,DEC pulse_count]

'serout 1,16780,[pulse_count] ' Echo back

low 0 ' Prohibit input transmission

high 8 ' Allow for output transmission

low 11 ' Light LED (INPUT)

' 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 1,16780,["END"] ' Let user know when movement is done

goto main ' Return to main program

pulses:

' for loop_count = 1 to pulse_count ' Determines distance to travel

pulsout 12, right_width ' Right servo

pulsout 13, left_width ' Left servo

pause 20 ' Wait 20 ms

' next

' serout 1,16780,["END"] ' Let user know when movement is done

' goto main ' Return to main program

return ' Return to polling routine

encoders: ' Read in wheel encoder data from LS7166

' l_wheel_count = l_wheel_count + ' Count on left wheel encoder

' r_wheel_count = r_wheel_count + ' Count on right wheel encoder

' wheel_direction = ' Direction of robot

' Send data to computer for analysis

' serout 1,16780,["START",l_wheel_count,r_wheel_count,wheel_direction]

return ' Return to polling routine