( bumpy.iso - isomax code for a robot with
( differential drive and a single bump sensor

: INIT_MOTORS 50000 TA0 PWM-PERIOD ;
: LMOTOR_FWD 6554 TA0 PWM-OUT ;
: RMOTOR_FWD 3277 TA1 PWM-OUT ;
: LMOTOR_BAK 3277 TA0 PWM-OUT ;
: RMOTOR_BAK 6554 TA1 PWM-OUT ;
: LMOTOR_STOP TA0 OFF ;
: RMOTOR_STOP TA1 OFF ;

: BUMPER? PA1 OFF? ; ( returns true if bumper switch activated )

LOOPINDEX BAK_CNT
100 BAK_CNT END ( set for one second of backing )

LOOPINDEX SPIN_CNT
100 SPIN_CNT END ( set for one second of spinning )

MACHINE BUMPY
ON-MACHINE BUMPY
  APPEND-STATE STOPPED
  APPEND-STATE CRUISING
  APPEND-STATE BACKING
  APPEND-STATE SPINNING

IN-STATE STOPPED
CONDITION TRUE
CAUSES
  LMOTOR_FWD
  RMOTOR_FWD
THEN-STATE CRUISING TO-HAPPEN

IN-STATE CRUISING
CONDITION BUMPER?
CAUSES
  LMOTOR_BAK
  RMOTOR_BAK
THEN-STATE BACKING TO-HAPPEN

IN-STATE BACKING 
CONDITION BAK_CNT COUNT
CAUSES
  LMOTOR_BAK
  RMOTOR_FWD
THEN-STATE SPINNING TO-HAPPEN

IN-STATE SPINNING
CONDITION SPIN_CNT COUNT
CAUSES
  LMOTOR_FWD
  RMOTOR_FWD
THEN-STATE CRUISING TO-HAPPEN

: STARTBUMPY
  STOPPED SET-STATE
  INSTALL BUMPY ;