' ====================================================
'  Connection from Theremino_CNC to Steppers A/B/C
' ----------------------------------------------------
'  See the following comment:
'  www.theremino.com/blog/robotics-and-cnc/comment-page-6#comment-29547
' ====================================================

'  ------------------------------- SLOTS from/to CNC
'  1 = X from CNC
'  2 = Distance to X
'  3 = Y from CNC
'  4 = Distance to Y
'  5 = Z from CNC
'  6 = Distance to Z

'  ------------------------------- Master Module Slots
'  101 = Stepper Motor A
'  102 = Distance A
'  103 = Stepper Motor B
'  104 = Distance B
'  105 = Stepper Motor C
'  106 = Distance C
'  107 = Stepper Motor Z
'  108 = Distance D


Variable Numeric RotA
Variable Numeric RotB
Variable Numeric RotC

Variable Numeric DistA
Variable Numeric DistB
Variable Numeric DistC

Variable Numeric PosX
Variable Numeric PosY
Variable Numeric PosZ

Variable Numeric DistX
Variable Numeric DistY
Variable Numeric DistZ

Load "Theremino_HAL.exe"
Load "Theremino_CNC.exe"
Load "Theremino_SlotViewer.exe"
Option Speed  9
Gosub Loop
Stop

Label Loop
	' ------------------ X, Y, Z from Theremino_CNC
	PosX = Slot(1)
	PosY = Slot(3)
	PosZ = Slot(5)
	'
	' ------------------ A, B, C rotations from X and Y
	RotA =  PosX * 0.585
	RotB = -PosX * 0.293 + PosY * 0.293
	RotC = -PosX * 0.293 - PosY * 0.293
	'
	' ------------------ A, B, C, Z to Stepper Motors
	Slot 101 = RotA
	Slot 103 = RotB
	Slot 105 = RotC
	Slot 107 = PosZ
	'
	' ------------------ Prepare "Distances to destination"
	DistA = Slot(102)
	DistB = Slot(104)
	DistC = Slot(106)
	DistZ = Slot(108)
	'
	DistX = DistA / 0.585 - DistB / 0.293 - DistC / 0.293
	DistY = DistA / 0.585 + DistB / 0.293 - DistC / 0.293
	'
	' ------------------ "Dist. to dest." to Theremino CNC
	Slot 2 = DistX
	Slot 4 = DistY
	Slot 6 = DistZ
	'
	' ------------------ Minimize the CPU load
	Wait Seconds 0.001
Goto Loop