# controlr.int
task Controller in
"controlr.syn"
export MotionDone


# conrolr.syn
const X:word = 0
const Y:word = 1
const Z:word = 2
var x, y, z, lastX, lastY, lastZ:byte
var calculatorX, calculatorY, calculatorZ:byte
var moved:byte[3]


handler MotionDone(direction:byte)
move[direction] = true

entry
start Calculator(X)
calculatorX := child
start Calculator(Y)
calculatorY := child
start Calculator(Z)
calculatorZ := child
# to force the robot arm at home position (0, 0, 0)

x := 0
y := 0
z := 0
lastX := 1
lastY := 1
lastZ := 1
moved[X] := false
moved[Y] := false
moved[Z] := false
loop
# ask for motion(s) if necessary
if x = lastX
moved[X] := true
else
lastX := x
moved[X] := false
interrupt calculatorX.NextPosition(x)
if y = lastY
moved[Y] := true
else
lastY := y

moved[Y] := false
interrupt calculatorY.NextPosition(y)
if z = lastZ
moved[Z] := true
else
lastZ := z
moved[Z] := false
interrupt calculatorZ.NextPosition(z)

# wait for acknowladgement
loop
enable wait MotionDone
exit when moved[X] and moved[Y] and moved[Z]

put "Arm is at:", x, ",", y, ",", z, "\n"

# get the next position from the operator
get x, y, z


# calc.int
task Calculator(direction:byte) in "calc.syn"
export NextPosition
from MotorController as child import NextMotion


# calc.syn
var step, direction, oldPosition, newPosition:byte
var motion:word

handler NextPosition(nextPos:byte)
nextPosition := nextPos

entry
oldPosition := 0
start MotorController(direction)

loop
enable wait NextPosition

# compute next motor step and direction from last and new
# positions
motion := step shl 8 and direction

# motion is a word, where
# step is the Most Significant Byte
# direction is the Least Significant Byte
oldPosition := newPosition
interrupt child.NextMotion(motion)


# motor.inc
task MotorController(direction:byte) in "motor.syn"
from Controller import MotionDone
export NextMotion


# motor.syn
handler NextMotion(motion:word)
# Move motor according to step and direction (motion)

entry
loop
enable wait NextMotion
interrupt Controller.MotionDone(direction)