
' ====================================================
'  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
' ====================================================

'  ------------------------------- Master Module Pins
'  1 and 2 = Stepper Motor A
'  3 and 4 = Stepper Motor B
'  5 and 6 = Stepper Motor C
'  7 and 8 = Stepper Motor Z

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

Variable Numeric RotA
Variable Numeric RotB
Variable Numeric RotC

Variable Numeric PosX
Variable Numeric PosY
Variable Numeric PosZ

Variable Numeric DistX
Variable Numeric DistY
Variable Numeric DistZ

Load "Theremino_CNC.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"
	' ================================================
	'    WARNING --- THIS IS WRONG AND INCOMPLETE
	' ================================================
	DistX = RotA / 0.585 - RotB / 0.293 - RotC / 0.293
	DistY = RotA / 0.585 + RotB / 0.293 - RotC / 0.293
	' ================================================
	DistZ = Slot(108)
	'
	' ------------------ "Dist. to dest." to Theremino CNC
	Slot 2 = DistX
	Slot 4 = DistY
	Slot 6 = DistZ
	'
	' ------------------ Minimize the CPU charge
	Wait Seconds 0.001
Goto Loop


'BOOKMARKS 

