ROBOFORTH
DECIMAL
: REPCAL
STARTED?
CLRDSP
CR
MOTORS BIT 1- MEP !
MOTORS 0 DO
  LIMITS I 2* + @ 0 < IF MDP C@ I BIT OR MDP C! THEN
LOOP
MEP @ MCP ! ONLIM OFFLIM
  4 SPACES MOTORS 0 DO
    I 2* LIMITS + @
    I I BIT POSN @
    - #.
  LOOP
LIMITS ASSUME
ENCSET
;
: TEST1
DE-ENERGIZE
CR ." SET ROBOT AT HOME POSITION THEN PRESS ENTER... "
ASK
START
ENCOFF
CR ." CALIBRATING... "
LISTFLAG C0SET
0= IF CALIBRATE THEN
." HOME... " HOME
CR ." CALIBRATION TEST... "
4 0 DO
  REPCAL
  HOME
LOOP
CR ." FAST CALIBRATION TEST, PRESS ENTER TO CONTINUE OR ESCAPE TO QUIT... "
ASK DROP
30000 SPEED ! 3000 ACCEL !
4 0 DO
  REPCAL
  HOME
LOOP
LISTFLAG C1SET
NORMAL
;
: TEST2
DE-ENERGIZE
CR ." SET ROBOT AT HOME POSITION THEN PRESS ENTER... "
ASK
START
0= IF LISTFLAG C0SET CALIBRATE LISTFLAG C1SET THEN
ENCOFF
HOME
START
ENCOFF
4 0 DO
  CR
  ALL I 1000 * 1000 + MOVE
  ENCR1 0 GLOBALS @ - #.
  ENCR2 1 GLOBALS @ - #.
  ENCR3 2 GLOBALS @ - #.
  ENCR4 3 GLOBALS @ - #.
  ENCR5 4 GLOBALS @ - #.
  HOME
LOOP
30000 SPEED ! 3000 ACCEL !
4 0 DO
  CR
  ALL I 1000 * 1000 + MOVE
  ENCR1 0 GLOBALS @ - #.
  ENCR2 1 GLOBALS @ - #.
  ENCR3 2 GLOBALS @ - #.
  ENCR4 3 GLOBALS @ - #.
  ENCR5 4 GLOBALS @ - #.
  HOME
LOOP
CR NORMAL
;