User:Narc/Redpower 6-directional platform FORTH controller

This is a possible program that will control the 6-directional platform described on the Redpower 2 wiki. So far, tested with 10 blocks moved in every direction, no errors encountered.

Note that the program will not do anything other than control the platform. Note also that it uses the inertial navigation option built into that design to signal errors to the operator and allow the operator to resolve them.

VARIABLE AxleTarget 0 AxleTarget !

VARIABLE StepDelay 16 StepDelay ! \ Operator note: Feel free to play with StepDelay -- it's an extra delay to allow power to flow into \the axle motors before they're called on to move the main body. As the power requirement grows with \the size of the main body, some may want to set this higher (and others may attempt to make it lower) \than its default value of 16.


 * PULSE ( bits ticks -- ) \ Turns on requested BITS for TICKS ticks, then turns them off.

OVER IOXSET TICKS IOXRST


 * BREAK ( -- ) \ Prompts user to press ENTER to continue, ABORTs on any other key

." Press ENTER to continue, any other key to abort..." KEY 13 <> IF ABORT THEN CR


 * AxleIsForward? ( -- b ) IOX@ 15 AND 1 = ; \ Forward is also the same as Down
 * AxleIsBack? ( -- b ) IOX@ 15 AND 3 = ;
 * AxleIsLeft? ( -- b ) IOX@ 15 AND 2 = ;
 * AxleIsRight? ( -- b ) IOX@ 15 AND 4 = ;
 * AxleIsUp? ( -- b ) IOX@ 15 AND 8 = ;
 * AxleIsDown? ( -- b ) IOX@ 15 AND 1 = ; \ Forward is also the same as Down


 * RaiseAxle ( -- ) \ WARNING: No error message: silently fails if axle is not forward/down

AxleIsForward? IF		16 16 PULSE 16 TICKS \ Moves axle up	THEN


 * NextAxleDirection ( -- n ) \ Returns next axle movement in horizontal chain (FRBL)

IOX@ 15 AND DUP 8 = IF DROP 64 EXIT THEN \ From up next is down DUP 4 = IF DROP 4096 EXIT THEN \ From right next is back DUP 3 = IF DROP 1024 EXIT THEN \ From back next is left DUP 2 = IF DROP 256 EXIT THEN \ From left next is forward DUP 1 = IF DROP 16384 EXIT THEN \ From forward next is right ." FATAL: NextAxleDirection called with invalid APD reading: ". ABORT


 * CycleAxle ( -- ) \ Cycles axle in the horizontal chain (f->r->b->l) until target or four movements

AxleTarget @ DUP 0= IF ." FATAL: CycleAxle called with null target position." ABORT THEN IOX@ 15 AND = IF EXIT THEN 4 0 DO		NextAxleDirection 16 PULSE 16 TICKS AxleTarget @ IOX@ 15 AND = IF UNLOOP EXIT THEN LOOP


 * AxleForward ( -- )

BEGIN 1 AxleTarget ! CycleAxle

AxleIsForward? DUP INVERT IF ." ERROR: Axle has not moved to down/forward position. Check for obstructions!" CR			BREAK THEN UNTIL


 * AxleBack ( -- )

BEGIN 3 AxleTarget ! CycleAxle

AxleIsBack? DUP INVERT IF ." ERROR: Axle has not moved to back position. Check for obstructions!" CR			BREAK THEN UNTIL


 * AxleLeft ( -- )

BEGIN 2 AxleTarget ! CycleAxle

AxleIsLeft? DUP INVERT IF ." ERROR: Axle has not moved to left position. Check for obstructions!" CR			BREAK THEN UNTIL


 * AxleRight ( -- )

BEGIN 4 AxleTarget ! CycleAxle

AxleIsRight? DUP INVERT IF ." ERROR: Axle has not moved to right position. Check for obstructions!" CR			BREAK THEN UNTIL


 * AxleUp ( -- )

BEGIN AxleForward RaiseAxle

AxleIsUp? DUP INVERT IF ." ERROR: Axle has not moved to up position. Check for obstructions!" CR BREAK THEN UNTIL \ AxleIsUp?


 * AxleDown ( -- ) AxleForward ;


 * StepsForward ( n -- ) \ Take n steps forward, with inertial guidance for error checking

DEPTH 0= IF EXIT THEN 0 DO		AxleForward StepDelay @ TICKS 512 16 PULSE 16 TICKS 1 \ Push a 1 onto the stack for use in +LOOP below AxleIsForward? IF ." ERROR: Axle doesn't seem to have moved. Check for obstructions!" CR BREAK DROP 0 \ Drop the 1 and push a 0 instead for this error condition. THEN +LOOP


 * StepsBack ( n -- )

DEPTH 0= IF EXIT THEN 0 DO		AxleBack StepDelay @ TICKS 512 16 PULSE 16 TICKS 1 \ Push a 1 onto the stack for use in +LOOP below AxleIsBack? IF ." ERROR: Axle doesn't seem to have moved. Check for obstructions!" CR BREAK DROP 0 \ Drop the 1 and push a 0 instead for this error condition. THEN +LOOP


 * StepsLeft ( n -- )

DEPTH 0= IF EXIT THEN 0 DO		AxleLeft StepDelay @ TICKS 512 16 PULSE 16 TICKS 1 \ Push a 1 onto the stack for use in +LOOP below AxleIsLeft? IF ." ERROR: Axle doesn't seem to have moved. Check for obstructions!" CR BREAK DROP 0 \ Drop the 1 and push a 0 instead for this error condition. THEN +LOOP


 * StepsRight ( n -- )

DEPTH 0= IF EXIT THEN 0 DO		AxleRight StepDelay @ TICKS 512 16 PULSE 16 TICKS 1 \ Push a 1 onto the stack for use in +LOOP below AxleIsRight? IF ." ERROR: Axle doesn't seem to have moved. Check for obstructions!" CR BREAK DROP 0 \ Drop the 1 and push a 0 instead for this error condition. THEN +LOOP


 * StepsUp ( n -- )

DEPTH 0= IF EXIT THEN 0 DO		AxleUp StepDelay @ TICKS 512 16 PULSE 16 TICKS 1 \ Push a 1 onto the stack for use in +LOOP below AxleIsUp? IF ." ERROR: Axle doesn't seem to have moved. Check for obstructions!" CR BREAK DROP 0 \ Drop the 1 and push a 0 instead for this error condition. THEN +LOOP


 * StepsDown ( n -- )

DEPTH 0= IF EXIT THEN 0 DO		AxleDown StepDelay @ TICKS 512 16 PULSE 16 TICKS 1 \ Push a 1 onto the stack for use in +LOOP below AxleIsDown? IF ." ERROR: Axle doesn't seem to have moved. Check for obstructions!" CR BREAK DROP 0 \ Drop the 1 and push a 0 instead for this error condition. THEN +LOOP

Usage examples: 10 StepsForward 15 StepsLeft 9 StepsUp

The program will notify you of most common error conditions, and give you a chance to correct them and continue without losing your place.