'{$STAMP BS2}

'program is designed to be used with the TFS-2
'dual bi-directional motor control board
'available from J. Forkin, 3210 Shadyway Drive
'Pittsburgh, PA 15227
'visit http://www.geocities.com/jimforkin2003/
'for additional details on this and other products
'ROBOT WALKING PROGRAM CONTROLS TWO MOTORS
'MAY 9, 2003
'J. FORKIN
'**********************************************
'walking motion is generated by moving legs in
'an oscillating motion similar to how a turtle
'walks on land
'**********************************************
x  VAR  Byte
y  VAR  Byte
bumped  VAR  Byte
start:
'**********************************************
' robot walks forward until it sees an object
' and then walks back three steps then forward
' goback could be replaced with a turn to avoid
' obstacle and then continue on way.
'**********************************************
'for x=1 to 6     'this routine would cause the
  GOSUB here      'robot to go six steps forward
  'next           'and then six steps back and
'for y=1 to 6     'repeat till batteries die
  'gosub goback
  'next
GOTO start
'**********************************************
'SUBROUTINES
'**********************************************
goback:  'one motor cw other ccw for reverse

FOR y=1 TO 3 'set up counter to take three steps


'clockwise
HIGH 15  'this series of commands could be shortened
HIGH 11  'into one line to save program space
HIGH 14  'but this illustrates technique to switch
HIGH 10  'the h-bridges forward and reverse
LOW 13
LOW 9
LOW 12
LOW 8

PAUSE 750 'this gives motors time to move to end of travel

'counter clockwise
LOW 15
LOW 11
LOW 14
LOW 10
HIGH 13
HIGH 9
HIGH 12
HIGH 8

PAUSE 750  'this gives motors time to move to end of travel

NEXT
GOTO here  'start walking forward again



here:  'forward walking


LOW 15
HIGH 11
LOW 14
HIGH 10
HIGH 13
LOW 9
HIGH 12
LOW 8

PAUSE 750  'this gives motors time to move to end of travel

HIGH 15
LOW 11
HIGH 14
LOW 10
LOW 13
HIGH 9
LOW 12
HIGH 8

PAUSE 750  'this gives motors time to move to end of travel
GOSUB bump
RETURN
'****************************************
'following routine looks for sensor which
'could be optical, audio or even a switch
'which indicates collision or proximity
'to object [looks for low state]
'****************************************
bump:  'looks for collision sensor
'debug "   looking   "
  BUTTON 0,0,255,250,bumped,0,nopress
  GOTO goback  'back up three steps
    nopress:
    RETURN