\ Setup for download...........................................................|
PROGRAM      \ erase previous program
MARKER PROGRAM    \ mark start of program to enable deleting and updating
\ Utilities: blip, .stack, yield, wait, set_sensor, get_sensor (see BOS.f)

\ Program......................................................................|
: calc_angle ( raw -- angle )  128 SWAP DUP 1023 SWAP - */ ;  \ 128*x/(1023-x)
: display ( n -- )  12290 SWAP 12289 LCD_NUMBER LCD_REFRESH ;
: get_angle ( -- angle )  0 get_sensor calc_angle ;
: both_motors ( power mode -- )  2DUP 0 MOTOR_SET 2 MOTOR_SET ;
: drive ( power -- )  DUP 0< IF 0 SWAP - 2 ELSE 1 THEN both_motors ;
: control ( angle -- power )  120 - -7 * 30 / ;
: init ( -- )  0 0 0 set_sensor ;       \ setup sensor 0 as raw
: scan ( -- )  get_angle DUP display control drive ;	\ scan task
\ Register "init" and "scan" for ready for execution...........................|
' init init_vector !
' scan scan_vector !
main

