SCRUB

HEX

( PID16 ADDS VELOCITY PROFILE 4/05/03
( PID17 ADDS ODOMETRY 4/08/03

0 RAM !
( PWM H BRIDGE DRIVE 40 KHz
   400 CONSTANT PER# EEWORD

VARIABLE CH EEWORD

: CHVAR
  <BUILDS HERE P, 4 ALLOT DOES> P@ CH @ 2* + 
; EEWORD

CHVAR Kp EEWORD
CHVAR Ki EEWORD
CHVAR Kd EEWORD
CHVAR Kl EEWORD

CHVAR Yp EEWORD
CHVAR Yi EEWORD
CHVAR Yd EEWORD
CHVAR Y EEWORD


CHVAR DLTP EEWORD
CHVAR ACCUM EEWORD

CHVAR PERR EEWORD

CHVAR ENDPOS EEWORD
CHVAR CMDPOS EEWORD
CHVAR CMDVEL EEWORD
CHVAR CMDACC EEWORD
CHVAR CURPOS EEWORD
CHVAR CURVEL EEWORD
CHVAR CURACC EEWORD
CHVAR DECCNT EEWORD

CHVAR XACCUM EEWORD
CHVAR YACCUM EEWORD

VARIABLE RPRT EEWORD
VARIABLE ETBL EEWORD
1FF ALLOT 
ETBL 200 ERASE

40. 2CONSTANT TOLERANCE EEWORD

FVARIABLE WHEELBASE EEWORD ( MEASURED IN ENCODER TICKS
FVARIABLE THETA EEWORD

: CH0 0 CH ! ; EEWORD
: CH1 1 CH ! ; EEWORD

: PWMAxLOAD CH @ IF E0A ELSE E06 THEN ! E00 C@ DROP 03 E00 ! ; EEWORD

: INIT
  PWMA2 ON ( PWMA2 TO 100% FOR HIGH OUTPUT FOR ENABLE IF NEEDED
  PWMA3 ON ( PWMA3 TO 100% FOR HIGH OUTPUT FOR ENABLE IF NEEDED

  PER#    E05 ! ( SET MODULO FOR PWM COUNT
  0       E0C ! ( GET RID OF DEADBAND
  0       E0D ! 
  0       E0E !
  1004    E0F ! ( EDGE ALIGN PWA0/1 COMP PWA4/5 COMP

  ( PWMA CH FOR DC SERVO
  CH0
  PER# 2/ PWMAxLOAD ( HALF
  CH1
  PER# 2/ PWMAxLOAD ( HALF
  8C0C    E03 ! ( TURNS ON OUTPUT LAST THING

[ DECIMAL ]
  CH0
( DAVE'S SHNOZ
  6E0   Kp F!
  0.5E0   Ki F!
  1.2E0  Kd F!

  100.   Kl 2!
  0.0     ENDPOS 2!
  0.0     CMDPOS 2!
  0.0     CURPOS 2!
  0.0     ACCUM 2!
  0.0     CURVEL 2!
  0.0     CMDVEL 2!
  6.      CMDACC 2!
  0E0     XACCUM F!
  0E0     YACCUM F!


  CH1
( DAVE'S SHNOZ
  6E0   Kp F!
  0.5E0   Ki F!
  1.2E0  Kd F!

  100.   Kl 2!
  0.0     ENDPOS 2!
  0.0     CMDPOS 2!
  0.0     CURPOS 2!
  0.0     ACCUM 2!
  0.0     CURVEL 2!
  0.0     CMDVEL 2!
  6.      CMDACC 2!
  0E0     XACCUM F!
  0E0     YACCUM F!

[ HEX ]
  0.0     E47 2!
  0.0     E57 2!

; EEWORD


: BOTHDONE?
  CH0
  ENDPOS 2@ CMDPOS 2@ D= 
  PERR 2@ DABS TOLERANCE D< AND
  CURVEL 2@ D0= AND
  CH1
  ENDPOS 2@ CMDPOS 2@ D= 
  PERR 2@ DABS TOLERANCE D< AND
  CURVEL 2@ D0= AND
  AND
; EEWORD

: MOVEDONE?
  ENDPOS 2@ CMDPOS 2@ D= 
; EEWORD

: MOVEDIR?
  ENDPOS 2@ CURPOS 2@ D< ( ???? 
; EEWORD

: REMAINING?
  ENDPOS 2@ CURPOS 2@ D- DABS ( ????
; EEWORD

: DECEL?
  CURVEL 2@ CMDACC 2@  D/ CURVEL 2@ CMDACC 2@ DMOD D0= NOT
     IF 1. D+ THEN DECCNT 2!
  CURVEL 2@ DECCNT 2@ D* 2. D/ ( GET HALF CUS HALF FASTER, HALF SLOWER
  CURVEL 2@ DECCNT 2@ D* 2. DMOD ( IF THERE'S REMAINDER TAKE ONE MORE
  D+ REMAINING? D< NOT ( SEE IF ITS LONGER THAN REMAINING
; EEWORD

: ATSPEED?
  CURVEL 2@ CMDVEL 2@ D= 
; EEWORD

: PROFILE
  MOVEDONE? NOT
(  CMDVEL 2@ 0= NOT OR
  IF
    ( COMPUTE NECESSARY ACC
    CMDACC 2@ 
    DECEL?
    IF
      DNEGATE 
    ELSE
      ATSPEED? IF 2DROP 0.0 THEN
    THEN
    CURACC 2!
    ( COMPUTE NECESSARY VELOCITY
    CURVEL 2@ CURACC 2@ D+
    0.0 DMAX CMDVEL 2@ DMIN

    CURVEL 2!  
    ( COMPUTE NECESSARY POSITION
    REMAINING? CURVEL 2@ DU<
    IF
      ENDPOS 2@ CMDPOS 2!
    ELSE 
      CURVEL 2@ MOVEDIR? IF DNEGATE THEN CMDPOS 2@ D+ CMDPOS 2!
    THEN
  ELSE
    0.0 CURVEL 2! 
    0.0 CURACC 2! 
    0.0 DECCNT 2! 
  THEN
; EEWORD

: GO ENDPOS 2! ; EEWORD

: RGO ENDPOS 2@ D+ ENDPOS 2! ; EEWORD ( RELATIVE GO


: POSPID
(  CURPOS 2@  ( FOR NON HARDWARE DIFFERENCE GENERATION
(  CURPOS 2@ D- 2SWAP DLTP 2! ( FOR NON HARDWARE DIFFERENCE GENERATION
  CMDPOS 2@ CURPOS 2@ D- PERR 2!     ( Compute POSITION ERROR
  PERR  2@ ACCUM 2@ D+           ( Update ACCUM for Ki term
  Kl 2@         DMIN             (   but limit it with Kl
  Kl 2@ DNEGATE DMAX ACCUM 2!    (   both sides of zero
  PERR  2@ D>F Kp F@ F* Yp F!    ( Compute Yp
  ACCUM 2@ D>F Ki F@ F* Yi F!    ( Compute Yi
  DLTP  2@ D>F Kd F@ F* Yd F!    ( Compute Yd
  Yp F@ Yi F@ F+ Yd F@ F+ Y F!   ( Compute Y
  Y F@ F>D                       ( Output Y as PWM
  [ PER# 2/ S->D ] DLITERAL DMIN
  [ PER# 2/ NEGATE 1+ S->D ] DLITERAL DMAX DROP
  [ PER# 2/ ] LITERAL + PWMAxLOAD 
; EEWORD



: DUALPOSPID
  CH1
  E57 @ E5A @ SWAP CURPOS 2! ( GET CURRENT POSITION
  E54 @ NEGATE S->D DLTP 2! ( Get DELTA POS
  PROFILE
  POSPID

  CH0
  E47 @ E4A @ SWAP CURPOS 2! ( GET CURRENT POSITION
  E44 @ NEGATE S->D DLTP 2! ( Get DELTA POS
  PROFILE
  POSPID

  RPRT @
  IF
    RPRT 1-!
    PERR 2@ ETBL RPRT @ 2* + 2!
  THEN 
; EEWORD


: REP Kp F? Ki F? Kd F? Kl 2@ D. ;  EEWORD


: EDUMP FF BEGIN CR DUP 2* ETBL + 2@ D. 8 - DUP 0< UNTIL CR REP ; EEWORD

: ESTEP
  S->D CMDPOS 2@ D+ CMDPOS 2!
  100 RPRT !
  BEGIN RPRT @ 0= UNTIL 
  EDUMP
; EEWORD

: X 
  DECIMAL
  BEGIN
    CR ( use one of these two lines
(    D EMIT ( or the other, CR for scroll, D EMIT for over write one line
    CURPOS 2@ 8 D.R SPACE ( ENCODER POSITION
    DLTP 2@ 8 D.R SPACE ( ENCODER POSITION
(    CMDPOS 2@ 8 D.R SPACE ( ENCODER POSITION
(   E44 @ 8 .R SPACE ( ENCODER DIFF
    PERR 2@ 8 D.R SPACE ( ERROR SIGNAL
    Yp F@ F. ( POS ERROR SIGNAL
    Yi F@ F. ( INT ERROR SIGNAL
    Yd F@ F. ( DIR ERROR SIGNAL
(   Y  F@ F. ( TOT ERROR SIGNAL
(   ACCUM 2@ 8 D.R ( ACCUM 
(    6 SPACES ( clear any stray left over numbers immed after off screen
    ?TERMINAL
  UNTIL
; EEWORD


: V 
  DECIMAL
  BEGIN
    CR ( use one of these two lines
(    D EMIT ( or the other, CR for scroll, D EMIT for over write one line
    ENDPOS 2@ 8 D.R SPACE 
    CMDPOS 2@ 8 D.R SPACE 
    CURPOS 2@ 8 D.R SPACE 
    ."   "
    CMDVEL 2@ 8 D.R SPACE 
    CURVEL 2@ 8 D.R SPACE 
    ."   "
    CMDACC 2@ 8 D.R SPACE 
    CURACC 2@ 8 D.R SPACE 
    ."   "
    DECCNT 2@ 8 D.R SPACE 
    ?TERMINAL
  UNTIL
; EEWORD

DECIMAL
: ANGCHK
  FDUP PI PI F+ F< NOT IF PI F- PI F- THEN 
  FDUP      0E0 F<     IF PI F+ PI F+ THEN 
; EEWORD

: ODDOMETRY
  CH0 ( Get DELTA POS, RELIES ON POSPID TO UPDATE
  THETA F@ FSIN DLTP 2@ DNEGATE D>F F* XACCUM F@ F+ XACCUM F!
  THETA F@ FCOS DLTP 2@ DNEGATE D>F F* YACCUM F@ F+ YACCUM F!
  CH1 ( Get DELTA POS, RELIES ON POSPID TO UPDATE
  THETA F@ FSIN DLTP 2@ DNEGATE D>F F* XACCUM F@ F+ XACCUM F!
  THETA F@ FCOS DLTP 2@ DNEGATE D>F F* YACCUM F@ F+ YACCUM F!
  ( CALC NEW THETA
  CH0 DLTP 2@ CH1 DLTP 2@ D- D>F WHEELBASE F@ F/ 
  THETA F@ F+ ANGCHK THETA F!    
; EEWORD
( HEX

: O
  CR THETA F@ F.
  CR CH0 DLTP 2@ D. XACCUM F@ F. YACCUM F@ F. 
  CR CH1 DLTP 2@ D. XACCUM F@ F. YACCUM F@ F. 
; EEWORD

DECIMAL
2VARIABLE 180DEG EEWORD
2VARIABLE  90DEG EEWORD
2VARIABLE 16FT EEWORD
2VARIABLE  8FT EEWORD

2VARIABLE FWDL EEWORD
2VARIABLE FWDR EEWORD

: GOFAR
  CH0 16FT 2@ 3. D* RGO ( LONG ENOUGH
  CH1 16FT 2@ 3. D* RGO ( LONG ENOUGH
; EEWORD

: LFWD CH0 FWDL 2@ CMDVEL 2! ; EEWORD
: RFWD CH1 FWDR 2@ CMDVEL 2! ; EEWORD
: LDFT CH0 FWDL 2@ 7. D* 9. D/ CMDVEL 2! 
       CH1 FWDR 2@ 8. D* 9. D/ CMDVEL 2! ; EEWORD
: RDFT CH1 FWDR 2@ 7. D* 9. D/ CMDVEL 2! 
       CH0 FWDR 2@ 8. D* 9. D/ CMDVEL 2! ; EEWORD
: LMED CH0 FWDL 2@ 4. D* 8. D/ CMDVEL 2!
       CH1 FWDR 2@ 6. D* 8. D/ CMDVEL 2! ; EEWORD
: RMED CH1 FWDR 2@ 4. D* 8. D/ CMDVEL 2!
       CH0 FWDR 2@ 6. D* 8. D/ CMDVEL 2! ; EEWORD
: LTRN CH0 FWDL 2@ 2. D* 8. D/ CMDVEL 2!
       CH1 FWDR 2@ 6. D* 8. D/ CMDVEL 2! ; EEWORD
: RTRN CH1 FWDR 2@ 2. D* 8. D/ CMDVEL 2!
       CH0 FWDR 2@ 6. D* 8. D/ CMDVEL 2! ; EEWORD
: LSLW CH0 8. CMDVEL 2! ; EEWORD
: RSLW CH1 8. CMDVEL 2! ; EEWORD
: LSTP CH0 0.0 CMDVEL 2! ; EEWORD
: RSTP CH1 0.0 CMDVEL 2! ; EEWORD
: LBCK CH0 FWDL 2@ DNEGATE CMDVEL 2! ; EEWORD
: RBCK CH1 FWDR 2@ DNEGATE CMDVEL 2! ; EEWORD

: -LOOPVAR 
  <BUILDS
   HERE P, 1- DUP , ,
  DOES>
    P@
    DUP @ 0=
    IF DUP 1 + @ SWAP ! TRUE ELSE 1-! FALSE THEN 
; EEWORD

100 -LOOPVAR CNT EEWORD

: CONTEST-INIT
( DAVE'S SHNOZ
  4400. 180DEG 2!
  2200.  90DEG 2!
  62000. 16FT 2! ( 415664.
  31000. 8FT 2!
  90. FWDL 2!
  90. FWDR 2!
(  10. FWDL 2!
(  10. FWDR 2!

  1400E0 WHEELBASE F!
  0E0 THETA F!
  CH0 0E0 XACCUM F!  0E0 YACCUM F!
  CH1 0E0 XACCUM F!  0E0 YACCUM F!
  
; EEWORD

MACHINE CONTEST EEWORD
 ON-MACHINE CONTEST
   APPEND-STATE W.F.B.    EEWORD ( WAIT FOR BUTTON

   APPEND-STATE QTINIT    EEWORD ( QUICK TRIP
   APPEND-STATE QTDOWN    EEWORD
   APPEND-STATE QTTURN    EEWORD
   APPEND-STATE QTBACK    EEWORD
   APPEND-STATE QTENDIT   EEWORD
   APPEND-STATE QTABORT   EEWORD


   APPEND-STATE TTINIT    EEWORD ( TTIME
   APPEND-STATE TTDOWN    EEWORD
   APPEND-STATE TTTURN1   EEWORD
   APPEND-STATE TTHALF    EEWORD
   APPEND-STATE TTTURN2   EEWORD
   APPEND-STATE TTSIDEOUT EEWORD
   APPEND-STATE TTTURN3   EEWORD
   APPEND-STATE TTSIDEIN  EEWORD
   APPEND-STATE TTTURN4   EEWORD
   APPEND-STATE TTBACK    EEWORD
   APPEND-STATE TTENDIT   EEWORD
   APPEND-STATE TTABORT   EEWORD


   APPEND-STATE LFINIT    EEWORD ( LINE FOLLOWING
   APPEND-STATE LF.RR     EEWORD
   APPEND-STATE LF.R      EEWORD
   APPEND-STATE LF.       EEWORD
   APPEND-STATE LF.L      EEWORD
   APPEND-STATE LF.LL     EEWORD
   APPEND-STATE LFABORT   EEWORD


IN-STATE
  W.F.B. 
CONDITION
  PB6 OFF?
CAUSES
  INIT
  REDLED OFF
  YELLED OFF
  GRNLED OFF
  PB0 OFF
THEN-STATE
  TTINIT
TO-HAPPEN IN-EE

IN-STATE
  W.F.B. 
CONDITION
  PB5 OFF?
CAUSES
  INIT
  REDLED OFF
  YELLED OFF
  GRNLED OFF
  PB0 OFF
THEN-STATE
  LFINIT
TO-HAPPEN IN-EE

IN-STATE
  W.F.B. 
CONDITION
  PB7 OFF?
CAUSES
  INIT
  REDLED OFF
  YELLED OFF
  GRNLED OFF
  PB0 OFF
  ." QTINIT"
THEN-STATE
  QTINIT
TO-HAPPEN IN-EE



IN-STATE
  QTINIT
CONDITION
  CNT
CAUSES
  LFWD CH0 16FT 2@ RGO
  RFWD CH1 16FT 2@ RGO
  REDLED OFF
  YELLED OFF
  GRNLED ON
  PB0 ON
  ." QTDOWN"
THEN-STATE
  QTDOWN
TO-HAPPEN IN-EE

IN-STATE
  QTDOWN
CONDITION
  BOTHDONE?
  PB7 OFF? OR
CAUSES
  LSLW CH0 90DEG 2@ RGO
  RSLW CH1 90DEG 2@ DNEGATE RGO
  REDLED ON
  YELLED OFF
  GRNLED OFF
  ." QTTURN"
THEN-STATE
  QTTURN
TO-HAPPEN IN-EE

IN-STATE
  QTTURN
CONDITION
  BOTHDONE?
  PB7 OFF? OR
CAUSES
  LFWD CH0 16FT 2@ RGO
  RFWD CH1 16FT 2@ RGO
  REDLED OFF
  YELLED ON
  GRNLED OFF
  ." QTBACK"
THEN-STATE
  QTBACK
TO-HAPPEN IN-EE

IN-STATE
  QTBACK
CONDITION
  BOTHDONE?
  PB7 OFF? OR
CAUSES
  LSLW CH0 90DEG 2@ RGO
  RSLW CH1 90DEG 2@ DNEGATE RGO
  REDLED ON
  YELLED OFF
  GRNLED OFF
  ." QTENDIT"
THEN-STATE
  QTENDIT
TO-HAPPEN IN-EE

IN-STATE
  QTENDIT
CONDITION
  BOTHDONE?
  PB7 OFF? OR
CAUSES
  REDLED ON
  YELLED ON
  GRNLED OFF
  ." QTABORT"
THEN-STATE
  QTABORT
TO-HAPPEN IN-EE

IN-STATE
  QTABORT
CONDITION
  PB7 ON?
CAUSES
  LSTP RSTP
  ." W.F.B."
THEN-STATE
  W.F.B.
TO-HAPPEN IN-EE





IN-STATE
  TTINIT
CONDITION
  CNT
CAUSES
  LFWD CH0 16FT 2@ RGO
  RFWD CH1 16FT 2@ RGO
  REDLED OFF
  YELLED OFF
  GRNLED ON
  PB0 ON
  ." TDOWN "
THEN-STATE
  TTDOWN
TO-HAPPEN IN-EE

IN-STATE
  TTDOWN
CONDITION
  BOTHDONE?
  PB6 OFF? OR
CAUSES
  LSLW CH0 90DEG 2@ RGO
  RSLW CH1 90DEG 2@ DNEGATE RGO
  REDLED OFF
  YELLED ON
  GRNLED OFF
  ." TTTURN1 "
THEN-STATE
  TTTURN1
TO-HAPPEN IN-EE

IN-STATE
  TTTURN1
CONDITION
  BOTHDONE?
  PB6 OFF? OR
CAUSES
  LFWD CH0 8FT 2@ RGO
  RFWD CH1 8FT 2@ RGO
  REDLED OFF
  YELLED ON
  GRNLED ON
  ." TTHALF "
THEN-STATE
  TTHALF
TO-HAPPEN IN-EE

IN-STATE
  TTHALF
CONDITION
  BOTHDONE?
  PB6 OFF? OR
CAUSES
  LSLW CH0 90DEG 2@ RGO
(  RSLW CH1 90DEG 2@ DNEGATE RGO
  REDLED ON
  YELLED OFF
  GRNLED ON
  ." TTTURN2 "
THEN-STATE
  TTTURN2
TO-HAPPEN IN-EE

IN-STATE
  TTTURN2
CONDITION
  BOTHDONE?
  PB6 OFF? OR
CAUSES
  LFWD CH0 8FT 2@ RGO
  RFWD CH1 8FT 2@ RGO
  REDLED ON
  YELLED ON
  GRNLED OFF
  ." TTSIDEOUT "
THEN-STATE
  TTSIDEOUT
TO-HAPPEN IN-EE

IN-STATE
  TTSIDEOUT
CONDITION
  BOTHDONE?
  PB6 OFF? OR
CAUSES
  LSLW CH0 90DEG 2@ RGO
  RSLW CH1 90DEG 2@ DNEGATE RGO
  REDLED ON
  YELLED ON
  GRNLED ON
  ." TTTURN3 "
THEN-STATE
  TTTURN3
TO-HAPPEN IN-EE

IN-STATE
  TTTURN3
CONDITION
  BOTHDONE?
  PB6 OFF? OR
CAUSES
  LFWD CH0 8FT 2@ RGO
  RFWD CH1 8FT 2@ RGO
  REDLED OFF
  YELLED OFF
  GRNLED OFF
  ." TTSIDEIN "
THEN-STATE
  TTSIDEIN
TO-HAPPEN IN-EE

IN-STATE
  TTSIDEIN
CONDITION
  BOTHDONE?
  PB6 OFF? OR
CAUSES
  LSLW CH0 90DEG 2@ RGO
(  RSLW CH1 90DEG 2@ DNEGATE RGO
  REDLED OFF
  YELLED OFF
  GRNLED ON
  ." TTTURN4 "
THEN-STATE
  TTTURN4
TO-HAPPEN IN-EE

IN-STATE
  TTTURN4
CONDITION
  BOTHDONE?
  PB6 OFF? OR
CAUSES
  LFWD CH0 8FT 2@ RGO
  RFWD CH1 8FT 2@ RGO
  REDLED OFF
  YELLED ON
  GRNLED OFF
  ." TTBACK "
THEN-STATE
  TTBACK
TO-HAPPEN IN-EE

IN-STATE
  TTBACK
CONDITION
  BOTHDONE?
  PB6 OFF? OR
CAUSES
  LSLW CH0 90DEG 2@ RGO
  RSLW CH1 90DEG 2@ DNEGATE RGO
  REDLED OFF
  YELLED ON
  GRNLED ON
  ." TTENDIT "
THEN-STATE
  TTENDIT
TO-HAPPEN IN-EE

IN-STATE
  TTENDIT
CONDITION
  BOTHDONE?
  PB6 OFF? OR
CAUSES
  LSTP RSTP
  REDLED ON
  YELLED ON
  GRNLED OFF
THEN-STATE
  TTABORT
TO-HAPPEN IN-EE

IN-STATE
  TTABORT
CONDITION
  PB6 ON?
CAUSES
THEN-STATE
  W.F.B.
TO-HAPPEN IN-EE





IN-STATE
  LFINIT
CONDITION
  CNT
CAUSES
  LFWD RFWD GOFAR
  PB0 ON? REDLED SET
  PB1 ON? YELLED SET
  PB2 ON? GRNLED SET
  ." LF. "
THEN-STATE
  LF.
TO-HAPPEN IN-EE


IN-STATE
  LF.
CONDITION
  PB0 ON? DUP REDLED SET
  PB1 ON? DUP YELLED SET NOT 
  PB2 ON?     GRNLED SET
  AND
CAUSES
  LMED
  ." LF.L "
THEN-STATE
  LF.L
TO-HAPPEN IN-EE


IN-STATE
  LF.
CONDITION
  PB0 ON? DUP REDLED SET
  PB1 ON? DUP YELLED SET 
  PB2 ON?     GRNLED SET
  AND
CAUSES
  LDFT
  ." LF.LD "
THEN-STATE
  LF.
TO-HAPPEN IN-EE


( IN-STATE
(   LF.
( CONDITION
(   PB0 ON? DUP REDLED SET NOT
(   PB1 ON? DUP YELLED SET 
(   PB2 ON? DUP GRNLED SET NOT
(   AND AND
( CAUSES
(   LFWD
(   RFWD
(   ." LF. "
( THEN-STATE
(   LF.
( TO-HAPPEN IN-EE


IN-STATE
  LF.
CONDITION
  PB0 ON?     REDLED SET
  PB1 ON? DUP YELLED SET
  PB2 ON? DUP GRNLED SET
  AND
CAUSES
  RDFT
  ." LF.RD "
THEN-STATE
  LF.
TO-HAPPEN IN-EE


IN-STATE
  LF.
CONDITION
  PB0 ON?     REDLED SET
  PB1 ON? DUP YELLED SET NOT
  PB2 ON? DUP GRNLED SET
  AND
CAUSES
  RMED
  ." LF.R "
THEN-STATE
  LF.R
TO-HAPPEN IN-EE

IN-STATE
  LF.L
CONDITION
  PB0 ON? DUP REDLED SET NOT
  PB1 ON? DUP YELLED SET NOT
  PB2 ON? DUP GRNLED SET NOT
  AND AND
CAUSES
  LTRN
  ." LF. "
THEN-STATE
  LF.L
TO-HAPPEN IN-EE


IN-STATE
  LF.L
CONDITION
  PB0 ON? DUP REDLED SET NOT 
  PB1 ON? DUP YELLED SET NOT
  PB2 ON? DUP GRNLED SET
  AND AND
CAUSES
  LMED
  ." LF. "
THEN-STATE
  LF.L
TO-HAPPEN IN-EE


IN-STATE
  LF.L
CONDITION
  PB0 ON? DUP REDLED SET NOT
  PB1 ON? DUP YELLED SET
  PB2 ON? DUP GRNLED SET NOT
  AND AND
CAUSES
  RMED
  ." LF. "
THEN-STATE
  LF.
TO-HAPPEN IN-EE


IN-STATE
  LF.R
CONDITION
  PB0 ON? DUP REDLED SET NOT
  PB1 ON? DUP YELLED SET
  PB2 ON? DUP GRNLED SET NOT
  AND AND
CAUSES
  LMED
  ." LF. "
THEN-STATE
  LF.
TO-HAPPEN IN-EE


IN-STATE
  LF.R
CONDITION
  PB0 ON? DUP REDLED SET
  PB1 ON? DUP YELLED SET NOT
  PB2 ON? DUP GRNLED SET NOT
  AND AND
CAUSES
  RMED
  ." LF.R "
THEN-STATE
  LF.R
TO-HAPPEN IN-EE


IN-STATE
  LF.R
CONDITION
  PB0 ON? DUP REDLED SET NOT
  PB1 ON? DUP YELLED SET NOT
  PB2 ON? DUP GRNLED SET NOT
  AND AND
CAUSES
  RTRN
  ." LF.R "
THEN-STATE
  LF.R
TO-HAPPEN IN-EE


( SENSE END 
IN-STATE
  LF.
CONDITION
  PB0 ON? PB2 ON? AND
  PB5 OFF? OR
CAUSES
  LSTP RSTP
  ." LF STOP"
THEN-STATE
  TTABORT
TO-HAPPEN IN-EE

IN-STATE
  LF.L
CONDITION
  PB0 ON? PB2 ON? AND
  PB5 OFF? OR
CAUSES
  LSTP RSTP
  ." LF STOP"
THEN-STATE
  TTABORT
TO-HAPPEN IN-EE


IN-STATE
  LF.R
CONDITION
  PB0 ON? PB2 ON? AND
  PB5 OFF? OR
CAUSES
  LSTP RSTP
  ." LF STOP"
THEN-STATE
  LFABORT
TO-HAPPEN IN-EE


IN-STATE
  LFABORT
CONDITION
  PB5 ON?
CAUSES
THEN-STATE
  W.F.B.
TO-HAPPEN IN-EE


MACHINE-CHAIN QTPID
  CH @
  DUALPOSPID
  ODDOMETRY
  CONTEST
  CH !
END-MACHINE-CHAIN EEWORD

HEX  
: STARTUP
  INIT
  CONTEST-INIT
  W.F.B. SET-STATE
  EVERY C350 2 / CYCLES SCHEDULE-RUNS QTPID DECIMAL 
  LSTP RSTP
; EEWORD

: TR
  LFWD CH0 90DEG 2@ RGO
  RFWD CH1 90DEG 2@ DNEGATE RGO
; EEWORD

: TL
  RFWD CH1 90DEG 2@ RGO
  LFWD CH0 90DEG 2@ DNEGATE RGO
; EEWORD

  7C00 AUTOSTART STARTUP
  SAVE-RAM