'M6 Start, Rack type tool changer with T0 empty tool return

'--- Variables:------

NewTool = GetSelectedTool()	  'Tool required
OldTool = GetCurrentTool()	  'Tool in spindle
MaxToolNum = 8								'Maximum number of tools
FFeed = 250                   'Fast feed on and off tool
ZSafe = -5										'Safe Z
DrawBar = Output11						'Output Port of mach3
Xpos = 0											'X position of tool in rack
YPos = 0											'Y position of tool in rack
ZPos = 0											'Z position of tool in rack
YPosSlide = 570.00						'Safe start for sliding the tool in and out of rack
SpindleEmpty = 0							'Is the spindle empty

'Check if the requested tool is within range
If OldTool = 0 Then
	'No tool in spindle
	SpindleEmpty = 0
Else
	'Spindle has a tool loaded
	SpindleEmpty = 1
End if

'Do we need a new tool
IF NewTool = OldTool Then
	'I don't need to change tool
	Message "Tool already loaded."
	Exit Sub
End If

If NewTool = 0 Then
  If SpindleEmpty = 0 Then
	  'No tool to put away exit
	Else
    'Put tool away
		Call PutToolInRack
	End If
	SetCurrentTool(NewTool)	'Set the NewTool as current tool
	Sleep(250)						  'Wait for DRO to update
	Exit Sub
Else
  If NewTool > MaxToolNum or NewTool < 0 Then
	  'Stop program and exit
    Message "Tool out of range [Tool #" & NewTool & "]"
    Code "M30"
    Exit Sub
	Else
	  If SpindleEmpty = 0 Then
		  'Get new tool
			GetToolFromRack
		Else
	    'Swap tools
			Call PutToolInRack
			Call GetToolFromRack
		End If
		SetCurrentTool(NewTool)	'Set the NewTool as current tool
		Sleep(250)						  'Wait for DRO to update
  End If
End If

End

Sub PutToolInRack
	'Put tool in rack
  Code "G00 G53 Z" & ZSafe                'Move Z axis safe
	Call WaitMove(100)
	Call ToolPos (OldTool,XPos, Ypos, ZPos) 'Get rack position
	Code "G53 X" & Xpos & " Y" & YPosSlide		'Move to sliding position
	Call WaitMove(100)
	Code "G01 G53 Z" & Zpos & " F" & FFeed	'Move spindle down
	Call WaitMove(100)
	Code "G53 Y" & Ypos   									'Slide tool into rack
	Call WaitMove(100)
	ActivateSignal(DrawBar)									'Activate the drawbar and release the tool
	Sleep(1000)												      'Wait 1 second for open the drawbar
	Code "G00 G53 Z" & ZSafe								'Move to safe tool height
	DeActivateSignal(DrawBar)               'Clamp drawbar
	Sleep(250)
	SpindleEmpty = 0                        'No tool in spindle
End Sub

Sub GetToolFromRack
	'Get tool from rack
	Code "G00 G53 Z" & ZSafe                'Move Z axis safe
	Call WaitMove(100)
	Call ToolPos (NewTool,XPos, YPos, ZPos)	'Get tool position
	Code "G53 X" & Xpos & " Y" & YPos	    	'Move to new tool position
	Call WaitMove(100)
	ActivateSignal(DrawBar)							  	'Activate the drawbar
	Sleep(1000)
	Code "G01 G53 Z" & ZPos & " F" & FFeed	'Down to engage the tool
	Call WaitMove(100)
	DeActivateSignal(DrawBar)								'Clamp the tool
	Sleep(6000)											        'Wait for tool to clamp
	Code "G53 Y" & YPosSlide								'Slide out of rack
	Code "G00 G53 Z" & ZSafe								'Move to Z safe
	SpindleEmpty = 1                        'Spindle has tool loaded
End Sub

Sub WaitMove(ByVal Dtime As Integer)
  While IsMoving()
    Sleep(Dtime)
  Wend
End Sub

Sub ToolPos(ByVal ToolNumber As Integer, ByRef XPos, ByRef YPos, ByRef ZPos)
'Subroutine to find tool location coordinates
	Select Case ToolNumber
		Case is = 1								'If the tool number is 1 then read these XYZ positions
					Xpos = 112.25				'Here specify  the Machine coords of the tool 1 position.
					Ypos = 304.79				'same for all cases
					Zpos = -80.00
		Case is = 2
					Xpos = 212.25
					Ypos = 304.79
					Zpos = -80.00			
		Case is = 3
					Xpos = 312.25
					Ypos = 304.79
					Zpos = -80.00
		Case is = 4
					Xpos = 412.25
					Ypos = 304.79
					Zpos = -80.00
		Case is = 5
					Xpos = 512.25
					Ypos = 304.79
					Zpos = -80.00
		Case is = 6
					Xpos = 612.25
					Ypos = 304.79
					Zpos = -80.00
		Case is = 7
					Xpos = 711.50
					Ypos = 304.79
					Zpos = -80.00
		Case is = 8
					Xpos = 111.50
					Ypos = 304.79
					Zpos = -50.00
	End Select
End Sub