( WALLFOLLOWING ROBOT - ISOMAX - SCOTTY ORR )

SCRUB

COLD

: LWALL ADC0 ANALOGIN 8 / ; EEWORD
: RWALL ADC1 ANALOGIN 8 / ; EEWORD

: LLINE? PA2 OFF? ; EEWORD
: RLINE? PA3 OFF? ; EEWORD

: OBSTACLE? PA7 OFF? ; EEWORD

( MOTOR DEFINITIONS - left motor on TA0, right on TA1 )
: INIT_MOTORS 50000 TA0 PWM-PERIOD ; EEWORD

: LFWD 6554 TA0 PWM-OUT ; EEWORD
: RFWD 3277 TA1 PWM-OUT ; EEWORD

: LBAK 3277 TA0 PWM-OUT ; EEWORD
: RBAK 6554 TA1 PWM-OUT ; EEWORD

: LCENTER 4915 TA0 PWM-OUT ; EEWORD
: RCENTER 4915 TA1 PWM-OUT ; EEWORD

: LADJUST 5090 TA0 PWM-OUT ; EEWORD
: RADJUST 4740 TA1 PWM-OUT ; EEWORD

: LSTOP TA0 OFF ; EEWORD
: RSTOP TA1 OFF ; EEWORD

MACHINE WALLBOT EEWORD
ON-MACHINE WALLBOT
   APPEND-STATE STARTUP EEWORD
   APPEND-STATE LW_ADJ_L EEWORD
   APPEND-STATE LW_ADJ_R EEWORD
   APPEND-STATE CRUISING_LW EEWORD

IN-STATE STARTUP
CONDITION TRUE
CAUSES
  LFWD RFWD 
THEN-STATE CRUISING_LW TO-HAPPEN IN-EE

IN-STATE CRUISING_LW
CONDITION LWALL 2000 < ( 5 inches - too far )
CAUSES 
  LADJUST
  RFWD
THEN-STATE LW_ADJ_L TO-HAPPEN IN-EE

IN-STATE CRUISING_LW 
CONDITION LWALL 2500 > ( 3.5 inches - too CLOSE )
CAUSES 
  RADJUST
  LFWD
THEN-STATE LW_ADJ_R TO-HAPPEN IN-EE

IN-STATE LW_ADJ_L 
CONDITION LWALL 2200 > ( back inside 5 inches - just right )
CAUSES 
  LFWD RFWD 
THEN-STATE CRUISING_LW TO-HAPPEN IN-EE

IN-STATE LW_ADJ_R 
CONDITION LWALL 2300 < ( back outside 3.5 inches - just right )
CAUSES
  LFWD RFWD 
THEN-STATE CRUISING_LW TO-HAPPEN IN-EE

: MAIN ( initialize variables, set initial state, install machine )
STARTUP SET-STATE
INSTALL WALLBOT ; EEWORD

( AFTER LOADING, TYPE: )
( HEX 3C00 AUTOSTART MAIN )
( SAVE-RAM )