' {$STAMP BS2}
' {$PBASIC 2.5}
ch     VAR  Byte
pw1    VAR  Word
pw2    VAR  Word
pw3    VAR  Word
pw4    VAR  Word
pw5    VAR  Word
pw6    VAR  Word
pw7    VAR  Word
pw9    VAR  Word
pw10   VAR  Word
pw11   VAR  Word
pw13   VAR  Word
ra     VAR  Byte
Sdat   PIN  8
baud   CON  396
Reps   VAR  Word

ra = 7                  'motor's ramp(response velocity)
 pw5 = 900              'no.15 and no.10 lift up leg 150 ; no. 9 step forward 150 ; no.13 step back 150
 pw1 = 908              'no.14 step forward 150
 pw2 = 930              'no. 8 step back 150
 pw3 = 600              'no.11 and no.12 lift up leg 150 ; no.13 step forward 150 ; no. 9 step back 150
 pw6 = 780              'no. 8 initial position
 pw9 = 750              'midpoint
 pw10 = 630             'no. 8 step forward 150
 pw11 = 758             'no.14 initial position
 pw13 = 608             'no.14 step back 150
 pw4 = 705              'no.15 initial position
 pw7 = 795              'no.12 initial position

 SEROUT Sdat, Baud+$8000, ["!SC", 10, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]      'set initial position
 SEROUT Sdat, Baud+$8000, ["!SC", 8, ra, pw6.LOWBYTE, pw6.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 11, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 9, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 15, ra, pw4.LOWBYTE, pw4.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 13, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 14, ra, pw11.LOWBYTE, pw11.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 12, ra, pw7.LOWBYTE, pw7.HIGHBYTE, CR]
 PAUSE 500

HIGH 9            'to give high voltage(5V) to input pin 12~15 ; alternative give from Vdd
INPUT 15          'let pin 15 be input
INPUT 14
INPUT 13
INPUT 12


GOSUB initial
IF IN15 = 1 AND IN14 = 0 AND IN13 = 0 AND IN12 = 0 THEN forward         'when pin15 get high voltage and other pins get ground, then robot move forward
IF IN14 = 1 AND IN15 = 0 AND IN13 = 0 AND IN12 = 0 THEN back
IF IN13 = 1 AND IN15 = 0 AND IN14 = 0 AND IN12 = 0 THEN left
IF IN12 = 1 AND IN15 = 0 AND IN14 = 0 AND IN13 = 0 THEN right
IF IN15 = 0 AND IN14 = 0 AND IN13 = 0 AND IN12 = 0 THEN initial         'when there is no voltage, then lock itself
initial :
DO
IF IN15 = 1 THEN RETURN          'if exist high state on pin12~15 then goback to the line after GOSUB and re-determine the motion
IF IN14 = 1 THEN RETURN
IF IN13 = 1 THEN RETURN
IF IN12 = 1 THEN RETURN
 SEROUT Sdat, Baud+$8000, ["!SC", 10, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]      'set initial position
 SEROUT Sdat, Baud+$8000, ["!SC", 8, ra, pw6.LOWBYTE, pw6.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 11, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 9, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 15, ra, pw4.LOWBYTE, pw4.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 13, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 14, ra, pw11.LOWBYTE, pw11.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 12, ra, pw7.LOWBYTE, pw7.HIGHBYTE, CR]
 PAUSE 2000
LOOP

forward :
DO
IF IN15 = 0 THEN RETURN          'if pin15 lose its voltage, then goback and recheck
 SEROUT Sdat, Baud+$8000, ["!SC", 12, ra, pw3.LOWBYTE, pw3.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 15, ra, pw5.LOWBYTE, pw5.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 14, ra, pw1.LOWBYTE, pw1.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 13, ra, pw3.LOWBYTE, pw3.HIGHBYTE, CR]
 PAUSE 500
 SEROUT Sdat, Baud+$8000, ["!SC", 12, ra, pw7.LOWBYTE, pw7.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 15, ra, pw4.LOWBYTE, pw4.HIGHBYTE, CR]
 PAUSE 500
 SEROUT Sdat, Baud+$8000, ["!SC", 14, ra, pw11.LOWBYTE, pw11.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 13, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]
 PAUSE 500                                                                     'no.12~15 step forward 150 and then return
 SEROUT Sdat, Baud+$8000, ["!SC", 10, ra, pw5.LOWBYTE, pw5.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 11, ra, pw3.LOWBYTE, pw3.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 8, ra, pw10.LOWBYTE, pw10.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 9, ra, pw5.LOWBYTE, pw5.HIGHBYTE, CR]
 PAUSE 500
 SEROUT Sdat, Baud+$8000, ["!SC", 10, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 11, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]
 PAUSE 500
 SEROUT Sdat, Baud+$8000, ["!SC", 8, ra, pw6.LOWBYTE, pw6.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 9, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]
 PAUSE 500                                                                     'no.8~11 step forward 150 and then return
LOOP

back:
DO
IF IN14 = 0 THEN RETURN
 SEROUT Sdat, Baud+$8000, ["!SC", 10, ra, pw5.LOWBYTE, pw5.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 11, ra, pw3.LOWBYTE, pw3.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 8, ra, pw2.LOWBYTE, pw2.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 9, ra, pw3.LOWBYTE, pw3.HIGHBYTE, CR]
 PAUSE 500
 SEROUT Sdat, Baud+$8000, ["!SC", 10, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 11, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]
 PAUSE 500
 SEROUT Sdat, Baud+$8000, ["!SC", 8, ra, pw6.LOWBYTE, pw6.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 9, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]
 PAUSE 500                                                                     'no.8~11 step back 150 and then return
 SEROUT Sdat, Baud+$8000, ["!SC", 12, ra, pw3.LOWBYTE, pw3.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 15, ra, pw5.LOWBYTE, pw5.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 14, ra, pw13.LOWBYTE, pw13.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 13, ra, pw5.LOWBYTE, pw5.HIGHBYTE, CR]
 PAUSE 500
 SEROUT Sdat, Baud+$8000, ["!SC", 12, ra, pw7.LOWBYTE, pw7.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 15, ra, pw4.LOWBYTE, pw4.HIGHBYTE, CR]
 PAUSE 500
 SEROUT Sdat, Baud+$8000, ["!SC", 14, ra, pw11.LOWBYTE, pw11.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 13, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]
 PAUSE 500
LOOP                                                                           'no.12~15 step back 150 and then return

left:
DO
IF IN13 = 0 THEN RETURN
 SEROUT Sdat, Baud+$8000, ["!SC", 12, ra, pw3.LOWBYTE, pw3.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 15, ra, pw5.LOWBYTE, pw5.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 14, ra, pw13.LOWBYTE, pw13.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 13, ra, pw3.LOWBYTE, pw3.HIGHBYTE, CR]
 PAUSE 500                                                                     'both no.13 and no.14 rotate counterclockwise 150
 SEROUT Sdat, Baud+$8000, ["!SC", 12, ra, pw7.LOWBYTE, pw7.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 15, ra, pw4.LOWBYTE, pw4.HIGHBYTE, CR]
 PAUSE 500
 SEROUT Sdat, Baud+$8000, ["!SC", 14, ra, pw11.LOWBYTE, pw11.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 13, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]
 PAUSE 500
LOOP

right:
DO
IF IN12 = 0 THEN RETURN
 SEROUT Sdat, Baud+$8000, ["!SC", 10, ra, pw5.LOWBYTE, pw5.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 11, ra, pw3.LOWBYTE, pw3.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 8, ra, pw2.LOWBYTE, pw2.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 9, ra, pw5.LOWBYTE, pw5.HIGHBYTE, CR]
 PAUSE 500                                                                     'both no.8 and no.9 rotate clockwise 150
 SEROUT Sdat, Baud+$8000, ["!SC", 10, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 11, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]
 PAUSE 500
 SEROUT Sdat, Baud+$8000, ["!SC", 8, ra, pw6.LOWBYTE, pw6.HIGHBYTE, CR]
 SEROUT Sdat, Baud+$8000, ["!SC", 9, ra, pw9.LOWBYTE, pw9.HIGHBYTE, CR]
 PAUSE 500
LOOP