\ DIFFMOTOR3.TXT - words to manipulate the motors and indicators

DIFFMOTOR
MARKER DIFFMOTOR

HEX
RCX_INIT

\ MOTOR_SHOW_xyz sets the LCD segments for a specific
\ direction and motor

: MOTOR_SHOW_FORWARD ( idx*3 -- )
    DUP 300E + LCD_SHOW
    DUP 3010 + LCD_SHOW
        300F + LCD_HIDE ;

: MOTOR_SHOW_REVERSE ( idx*3 -- )
    DUP 300E + LCD_SHOW
    DUP 3010 + LCD_HIDE
        300F + LCD_SHOW ;

: MOTOR_SHOW_BRAKE ( idx*3 -- )
    DUP 300E + LCD_SHOW
    DUP 3010 + LCD_SHOW
        300F + LCD_SHOW ;

: MOTOR_SHOW_FLOAT ( idx*3 -- )
    DUP 300E + LCD_HIDE
    DUP 3010 + LCD_HIDE
        300F + LCD_HIDE ;

\ Improved MOTOR_SHOW

CREATE MOTOR_SHOW_VECTOR
' MOTOR_SHOW_FLOAT   ,  \ Dummy entry for dir=0
' MOTOR_SHOW_FORWARD ,
' MOTOR_SHOW_REVERSE ,
' MOTOR_SHOW_BRAKE   ,
' MOTOR_SHOW_FLOAT   ,

\ Note: if dir is ANYTHING other than 0,1,2,3,4 this
\       routine will crash the RCX

: MOTOR_SHOW ( dir idx --   )
  3 *        ( dir idx*3 -- )
  SWAP CELLS              \ Calculate the correct entry
  MOTOR_SHOW_VECTOR +     \ Point at the entry in the table
  @ EXECUTE ;             \ Fetch it, then execute it

\ FACTORED DIFFERENTIAL DRIVE CONTROL WORDS

: BOT_MOTOR_SET ( left right -- )
  7 SWAP 0 2DUP MOTOR_SHOW MOTOR_SET
  7 SWAP 2 2DUP MOTOR_SHOW MOTOR_SET ;

: BOT_FORWARD ( -- )
  1 1 BOT_MOTOR_SET ;

: BOT_REVERSE ( -- )
  2 2 BOT_MOTOR_SET ;

: BOT_LEFT    ( -- )
  1 2 BOT_MOTOR_SET ;

: BOT_RIGHT   ( -- )
  2 1 BOT_MOTOR_SET ;

: BOT_STOP    ( -- )
  3 3 BOT_MOTOR_SET ;
