SCRUB

COLD
 
HEX
: HALFSPEEDCPU F413 FA1 ! 82 F00 ! ; EEWORD ( DIV BY 2

HALFSPEEDCPU


 4E2 CONSTANT LOSTOP EEWORD
 9C4 CONSTANT HISTOP EEWORD
HISTOP LOSTOP + 2/ CONSTANT MIDWAY EEWORD 
HISTOP LOSTOP -    CONSTANT RANGE  EEWORD 

: PWMLOAD E00 @ DROP C3 E00 ! ; EEWORD

: FWDL HISTOP ( LOSTOP ) E06 ! PWMLOAD ; EEWORD
: FWDR LOSTOP E07 ! PWMLOAD ; EEWORD

: BACKL LOSTOP ( HISTOP ) E06 ! PWMLOAD ; EEWORD
: BACKR HISTOP E07 ! PWMLOAD ; EEWORD

: STOPL MIDWAY E06 ! PWMLOAD ; EEWORD
: STOPR MIDWAY E07 ! PWMLOAD ; EEWORD

: SLOWL [ RANGE 20 / MIDWAY + ] LITERAL E06 ! PWMLOAD ; EEWORD
: SLOWR [ RANGE 20 / MIDWAY - ] LITERAL E07 ! PWMLOAD ; EEWORD ( HALF

: MEDL [ RANGE 8 / MIDWAY + ] LITERAL E06 ! PWMLOAD ; EEWORD
: MEDR [ RANGE 8 / MIDWAY - ] LITERAL E07 ! PWMLOAD ; EEWORD ( HALF

: SPINL FWDR BACKL ; EEWORD
: SPINR FWDL BACKR ; EEWORD

: CURVEL FWDR SLOWL ; EEWORD
: CURVER FWDL SLOWR ; EEWORD

: DRIFTL FWDR MEDL ; EEWORD
: DRIFTR FWDL MEDR ; EEWORD

: STOP STOPL STOPR ; EEWORD
: BACK BACKL BACKR ; EEWORD
: FWD FWDL FWDR ; EEWORD
: SLOW SLOWL SLOWR ; EEWORD

: PWM-INIT
  HALFSPEEDCPU

  8000   E03 !
  61A8   E05 ! 
  0      E0D !
  0      E0E !
  000E   E0F ! ( CENTER ALIGN FOR DIV BY 2
  MIDWAY E06 ! 
  MIDWAY E07 ! PWMLOAD

; EEWORD

: AD-INIT
  0      FC3 ! ( ENABLE PB0 - 7

  2003 E80 ! ( SET UP A/D
  0000 E81 !
  3210 E83 !
  7654 E84 !
; EEWORD


HEX
VARIABLE A/D0 EEWORD
VARIABLE A/D1 EEWORD
2VARIABLE RUN-AVE0 EEWORD
2VARIABLE RUN-AVE1 EEWORD
8 CONSTANT RUN# EEWORD

: A/D
  E89 ( A/D0 ) @ 8 / A/D0 !
  E8A ( A/D1 ) @ 8 / A/D1 !
  RUN-AVE0 2@ D>F FDUP RUN# S>F F/ F- 
  F>D A/D0 @ S->D D+ RUN-AVE0 2! ( running average
  RUN-AVE1 2@ D>F FDUP RUN# S>F F/ F- 
  F>D A/D1 @ S->D D+ RUN-AVE1 2! ( running average
; EEWORD

( 1200 CONSTANT LVL EEWORD
 300 CONSTANT LVL EEWORD

( : DETR? LVL E89 @ 2/ 2/ 2/ U< ; EEWORD
: DETR? LVL RUN-AVE0 2@ D>F RUN# S>F F/ F>D DROP < ; EEWORD

( : DETL? LVL E8A @ 2/ 2/ 2/ U< < ; EEWORD
: DETL? LVL RUN-AVE1 2@ D>F RUN# S>F F/ F>D DROP < ; EEWORD

: RANGE0 RUN-AVE0 2@ D>F RUN# S>F F/ F>D D. ."  ave" ; EEWORD
: RANGE1 RUN-AVE0 2@ D>F RUN# S>F F/ F>D D. ."  ave" ; EEWORD
: RB CR RANGE0 RANGE1 ; EEWORD

VARIABLE F-ON EEWORD

DECIMAL
LOOPINDEX 1R
  1R 60 END EEWORD
LOOPINDEX 3A
  3A 90 END EEWORD
LOOPINDEX 1L
  1L 60 END EEWORD
LOOPINDEX 2F 
  2F 50 END EEWORD
LOOPINDEX 5SEC 
  5SEC 250 END EEWORD
LOOPINDEX 1T
  1T 60 END EEWORD
LOOPINDEX SHORT
  SHORT 60 START 0 END -1 STEP EEWORD
LOOPINDEX DEBOUNCE
  DEBOUNCE 30 START 0 END -1 STEP EEWORD
HEX

: FOLLOWIR
  DETL? GRNLED SET
  DETR? REDLED SET
; EEWORD

MACHINE MINISUMO-ATTACK EEWORD
 ON-MACHINE MINISUMO-ATTACK
   APPEND-STATE MSSAFE   EEWORD

   APPEND-STATE W5SEC    EEWORD ( WAIT FOR 5SEC

   APPEND-STATE SEARCHL  EEWORD 
   APPEND-STATE SEARCHR  EEWORD 
   APPEND-STATE CHASEL   EEWORD 
   APPEND-STATE CHASER   EEWORD 
   APPEND-STATE TWISTL   EEWORD 
   APPEND-STATE TWISTR   EEWORD 
   APPEND-STATE PUSHFWD  EEWORD 


IN-STATE
  W5SEC 
CONDITION
  5SEC COUNT
CAUSES
  YELLED OFF
  PWM-INIT
  TRUE F-ON !
  FWD
  CR ." FWD"
  SHORT RESET
THEN-STATE
  PUSHFWD
TO-HAPPEN IN-EE

IN-STATE
  SEARCHR 
CONDITION
  FOLLOWIR
  DETL? DETR? NOT AND
CAUSES
  DRIFTL
  CR ." DL"
  SHORT RESET
THEN-STATE
  SEARCHL
TO-HAPPEN IN-EE


IN-STATE
  SEARCHR 
CONDITION
  FOLLOWIR
  DETL? NOT DETR? NOT AND
  SHORT VALUE IF COUNT AND THEN 
CAUSES
  CURVER
  CR ." CR"
  SHORT RESET
THEN-STATE
  CHASER
TO-HAPPEN IN-EE


IN-STATE
  SEARCHR 
CONDITION
  FOLLOWIR
  DETL? DETR? AND
CAUSES
  FWD
  CR ." FWD"
THEN-STATE
  PUSHFWD
TO-HAPPEN IN-EE




IN-STATE
  SEARCHL 
CONDITION
  FOLLOWIR
  DETL? NOT DETR? AND
CAUSES
  DRIFTR
  CR ." DR"
  SHORT RESET
THEN-STATE
  SEARCHR
TO-HAPPEN IN-EE


IN-STATE
  SEARCHL 
CONDITION
  FOLLOWIR
  DETL? NOT DETR? NOT AND
  SHORT VALUE IF COUNT AND THEN 
CAUSES
  CURVEL
  CR ." CL"
  SHORT RESET
THEN-STATE
  CHASEL
TO-HAPPEN IN-EE


IN-STATE
  SEARCHL 
CONDITION
  FOLLOWIR
  DETL? DETR? AND
CAUSES
  FWD
  CR ." FWD"
THEN-STATE
  PUSHFWD
TO-HAPPEN IN-EE


IN-STATE
  CHASER 
CONDITION
  FOLLOWIR
  DETL? DETR? NOT AND
CAUSES
  DRIFTL
  CR ." DL"
  SHORT RESET
THEN-STATE
  SEARCHL
TO-HAPPEN IN-EE


IN-STATE
  CHASER 
CONDITION
  FOLLOWIR
  DETL? NOT DETR? NOT AND
  SHORT VALUE IF COUNT AND THEN 
CAUSES
  SPINR
  CR ." SR"
  SHORT RESET
THEN-STATE
  TWISTR
TO-HAPPEN IN-EE


IN-STATE
  CHASER 
CONDITION
  FOLLOWIR
  DETL? DETR? AND
CAUSES
  FWD
  CR ." FWD"
THEN-STATE
  PUSHFWD
TO-HAPPEN IN-EE


IN-STATE
  CHASEL 
CONDITION
  FOLLOWIR
  DETL? NOT DETR? AND
CAUSES
  DRIFTR
  CR ." DR"
  SHORT RESET
THEN-STATE
  SEARCHR
TO-HAPPEN IN-EE


IN-STATE
  CHASEL 
CONDITION
  FOLLOWIR
  DETL? NOT DETR? NOT AND
  SHORT VALUE IF COUNT AND THEN 
CAUSES
  SPINL
  CR ." SL"
  SHORT RESET
THEN-STATE
  TWISTL
TO-HAPPEN IN-EE


IN-STATE
  CHASEL 
CONDITION
  FOLLOWIR
  DETL? DETR? AND
CAUSES
  FWD
  CR ." FWD"
THEN-STATE
  PUSHFWD
TO-HAPPEN IN-EE






IN-STATE
  TWISTR 
CONDITION
  FOLLOWIR
  DETL? NOT DETR? AND
CAUSES
  DRIFTL
  CR ." DL"
  SHORT RESET
THEN-STATE
  SEARCHL
TO-HAPPEN IN-EE


IN-STATE
  TWISTR 
CONDITION
  FOLLOWIR
  DETL? NOT DETR? NOT AND
  SHORT VALUE IF COUNT AND THEN 
CAUSES
  DRIFTL
  CR ." DL"
  SHORT RESET
THEN-STATE
  SEARCHL
TO-HAPPEN IN-EE


IN-STATE
  TWISTR 
CONDITION
  FOLLOWIR
  DETL? DETR? AND
CAUSES
  FWD
  CR ." FWD"
THEN-STATE
  PUSHFWD
TO-HAPPEN IN-EE


IN-STATE
  TWISTL 
CONDITION
  FOLLOWIR
  DETL? NOT DETR? AND
CAUSES
  DRIFTR
  CR ." DR"
  SHORT RESET
THEN-STATE
  SEARCHR
TO-HAPPEN IN-EE


IN-STATE
  TWISTL 
CONDITION
  FOLLOWIR
  DETL? NOT DETR? NOT AND
  SHORT VALUE IF COUNT AND THEN 
CAUSES
  DRIFTR
  CR ." DR"
  SHORT RESET
THEN-STATE
  SEARCHR
TO-HAPPEN IN-EE


IN-STATE
  TWISTL 
CONDITION
  FOLLOWIR
  DETL? DETR? AND
CAUSES
  FWD
  CR ." FWD"
THEN-STATE
  PUSHFWD
TO-HAPPEN IN-EE

IN-STATE
  PUSHFWD
CONDITION
  FOLLOWIR
  DETL? DETR? NOT AND
CAUSES
  DRIFTL
  CR ." DL"
THEN-STATE
  SEARCHL
  SHORT RESET
TO-HAPPEN IN-EE


IN-STATE
  PUSHFWD
CONDITION
  FOLLOWIR
  DETL? NOT DETR? AND
CAUSES
  DRIFTR
  CR ." DR"
  SHORT RESET
THEN-STATE
  SEARCHR
TO-HAPPEN IN-EE


: F-ON?
(  P.O.R. IS-STATE? NOT 
(  W5SEC IS-STATE? NOT AND
  F-ON @
; EEWORD


MACHINE MINISUMO-RETREAT EEWORD
 ON-MACHINE MINISUMO-RETREAT
   APPEND-STATE BACKSAFE  EEWORD 
   APPEND-STATE BACKOKAY  EEWORD 
   APPEND-STATE BACKAWAY  EEWORD 
   APPEND-STATE BACKOFFR  EEWORD 
   APPEND-STATE BACKOFFL  EEWORD 
   APPEND-STATE RUNAWAY  EEWORD 


IN-STATE
  BACKOKAY 
CONDITION
  PB0 OFF?
  PB1 ON? AND
  F-ON? AND
CAUSES
  1L RESET
  CR ." BL"
THEN-STATE
  BACKOFFL
TO-HAPPEN IN-EE


IN-STATE
  BACKOKAY 
CONDITION
  PB0 ON?
  PB1 OFF? AND
  F-ON? AND
CAUSES
  1R RESET
  CR ." BR"
THEN-STATE
  BACKOFFR
TO-HAPPEN IN-EE


IN-STATE
  BACKOKAY 
CONDITION
  PB0 OFF?
  PB1 OFF? AND
  F-ON? AND
CAUSES
  3A RESET
  CR ." BA"
THEN-STATE
  BACKAWAY
TO-HAPPEN IN-EE


IN-STATE
  BACKOKAY 
CONDITION
  PB2 OFF?
  F-ON? AND
  PA0 ?ON DUP NOT PA0 SET PA1 SET
CAUSES
  SHORT RESET
  PA0 ?ON ( CAREFUL, JUST INVERTED
  IF
    CURVER 
    SEARCHR SET-STATE
    CR ." RSR"
  ELSE
    CURVEL
    SEARCHL SET-STATE
    CR ." RSL"
  THEN
THEN-STATE
  BACKOKAY 
TO-HAPPEN IN-EE




IN-STATE
  BACKOFFL 
CONDITION
  BACKL
  STOPR
  1L COUNT
CAUSES
  DRIFTL
  CR ." DL OK"
THEN-STATE
  BACKOKAY
TO-HAPPEN IN-EE


IN-STATE
  BACKOFFL 
CONDITION
  PB0 OFF?
  PB1 OFF? AND
CAUSES
  3A RESET
  CR ." BA"
THEN-STATE
  BACKAWAY
TO-HAPPEN IN-EE


IN-STATE
  BACKOFFL 
CONDITION
  PB2 OFF?
CAUSES
  2F RESET
  CR ." RA"
THEN-STATE
  RUNAWAY
TO-HAPPEN IN-EE


IN-STATE
  BACKOFFR 
CONDITION
  BACKR
  STOPL
  1R COUNT
CAUSES
  DRIFTR
  CR ." DR OK"
  SHORT RESET
THEN-STATE
  BACKOKAY
TO-HAPPEN IN-EE


IN-STATE
  BACKOFFR 
CONDITION
  PB0 OFF?
  PB1 OFF? AND
CAUSES
  3A RESET
  CR ." BA"
THEN-STATE
  BACKAWAY
TO-HAPPEN IN-EE


IN-STATE
  BACKOFFR 
CONDITION
  PB2 OFF?
CAUSES
  2F RESET
  CR ." RA"
THEN-STATE
  RUNAWAY
TO-HAPPEN IN-EE


IN-STATE
  BACKAWAY 
CONDITION
  BACK
  3A COUNT
CAUSES
  CURVEL
  SEARCHL SET-STATE
  CR ." SL"
  SHORT RESET
THEN-STATE
  BACKOKAY
TO-HAPPEN IN-EE


IN-STATE
  RUNAWAY 
CONDITION
  FWD
  2F COUNT
CAUSES
  DRIFTR
  CR ." DR OK"
  SHORT RESET
THEN-STATE
  BACKOKAY
TO-HAPPEN IN-EE

IN-STATE
  RUNAWAY 
CONDITION
  PB0 OFF? OR
  PB1 OFF? OR
CAUSES
  CR ." NO"
  SHORT RESET
THEN-STATE
  BACKOKAY
TO-HAPPEN IN-EE



MACHINE LF EEWORD
 ON-MACHINE LF
   APPEND-STATE LFSAFE    EEWORD ( LINE FOLLOWING
   APPEND-STATE LFINIT    EEWORD
   APPEND-STATE LF.R      EEWORD
   APPEND-STATE LF.       EEWORD
   APPEND-STATE LF.L      EEWORD


IN-STATE
  LFINIT
CONDITION
  DETR? DETL? AND
CAUSES
  CR ." LF."
  PWM-INIT
  FWDL FWDR
THEN-STATE
  LF.
TO-HAPPEN IN-EE


IN-STATE
  LF.
CONDITION
  PB0 OFF? ( LINE ON RIGHT
  PB1 ON?
  AND
CAUSES
  STOPR
THEN-STATE
  LF.L
TO-HAPPEN IN-EE


IN-STATE
  LF.
CONDITION
  PB0 ON?
  PB1 OFF? ( LINE ON LEFT
  AND
CAUSES
  STOPL
THEN-STATE
  LF.R
TO-HAPPEN IN-EE


IN-STATE
  LF.R
CONDITION
  PB1 ON?
CAUSES
  FWDL
THEN-STATE
  LF.
TO-HAPPEN IN-EE


IN-STATE
  LF.R
CONDITION
  PB0 OFF?
  PB1 OFF? AND
CAUSES
  STOP
  CR ." STOP LF"
THEN-STATE
  LFSAFE 
TO-HAPPEN IN-EE


IN-STATE
  LF.L
CONDITION
  PB0 ON?
CAUSES
  FWDR
THEN-STATE
  LF.
TO-HAPPEN IN-EE


IN-STATE
  LF.L
CONDITION
  PB0 OFF?
  PB1 OFF? AND
CAUSES
  STOP
  CR ." STOP LF"
THEN-STATE
  LFSAFE 
TO-HAPPEN IN-EE



MACHINE MASTER EEWORD
 ON-MACHINE MASTER 
   APPEND-STATE P.O.R.   EEWORD ( WAIT FOR BUTTON

   APPEND-STATE DO-MS  EEWORD 
   APPEND-STATE DO-LF  EEWORD 
   APPEND-STATE DO-QT   EEWORD 
   APPEND-STATE DO-TT   EEWORD 
   APPEND-STATE DO-WF   EEWORD 


IN-STATE
  P.O.R. 
CONDITION
  0 E03 ! ( STOP PWM
  FALSE F-ON !
  PB4 OFF?
  DEBOUNCE VALUE IF DEBOUNCE COUNT AND THEN
CAUSES
  REDLED OFF
  YELLED OFF
  GRNLED OFF
  5SEC RESET
  DEBOUNCE RESET
  CR ." W5"
  W5SEC SET-STATE
  BACKOKAY SET-STATE
THEN-STATE
  DO-MS
TO-HAPPEN IN-EE


IN-STATE
  DO-MS
CONDITION
  PB4 OFF?
  DEBOUNCE VALUE IF DEBOUNCE COUNT AND THEN
CAUSES
  REDLED ON
  YELLED ON
  GRNLED ON
  DEBOUNCE RESET
  CR ." MS SAFE"
  MSSAFE SET-STATE
  BACKSAFE SET-STATE
THEN-STATE
  P.O.R. 
TO-HAPPEN IN-EE


IN-STATE
  P.O.R. 
CONDITION
  PB5 OFF?
  DEBOUNCE VALUE IF DEBOUNCE COUNT AND THEN
CAUSES
  REDLED OFF
  YELLED OFF
  GRNLED OFF
  0 F-ON !
  DEBOUNCE RESET
  CR ." LFINIT"
  LFINIT SET-STATE
THEN-STATE
  DO-LF
TO-HAPPEN IN-EE


IN-STATE
  DO-LF
CONDITION
  PB5 OFF?
  DEBOUNCE VALUE IF DEBOUNCE COUNT AND THEN
CAUSES
  REDLED ON
  YELLED ON
  GRNLED ON
  DEBOUNCE RESET
  CR ." LF SAFE"
  LFSAFE SET-STATE
THEN-STATE
  P.O.R. 
TO-HAPPEN IN-EE




MACHINE-CHAIN ALL
  A/D
  MASTER
  MINISUMO-ATTACK
  MINISUMO-RETREAT
  LF
END-MACHINE-CHAIN EEWORD

: STARTUP
  PWM-INIT
  AD-INIT
  P.O.R. SET-STATE
  MSSAFE SET-STATE
  BACKSAFE SET-STATE
  LFSAFE SET-STATE
  DEBOUNCE RESET
  EVERY C350 CYCLES SCHEDULE-RUNS ALL
; EEWORD

 3C00 AUTOSTART STARTUP
 SAVE-RAM