'M6 Start, Rack type tool changer with T0 empty tool return
'This macro is made for a ATC with pneumatic tool rack and two sensors in the Spindle (opencollet and closed collet)
'This macro is possible with the help of Graham Waterworth and TPS  from machsupport forum and CncWoodProjects (i learned a lot with Graham and TPS profesional programers)
'This macro is FREE and can be modified but can't be sold or used commercially, only mention the creators and share your macro for help others.
'For the moment this macro is a test but when all working i go to put in a mach3 forum all variables (without sensors, without tool peneumatic and the all in one version).
'Be sure to change the tools positions, inputs and outputs pins corresponding of your machine (every cnc is different).
'First try to test if it's working in the air (put a Zpos above the tools for example in my case my tool rack is in -110 anf for test i used -80 without tool inside).

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

NewTool = GetSelectedTool()	  	'Tool required
OldTool = GetCurrentTool()	  	'Tool in spindle
MaxToolNum = 9				'Maximum number of tools
FFeed = 1000                  		'Fast feed on and off tool
ZSafe = -49				'Safe Z
DrawBar = Output11			'Output Port of mach3 (open the collet for tool change)
ToolRack = Output12			'Output Port of mach3 for open the output for activate pneumatic tool rack and in future can use this output for raise dust shoe too (at same time if you don't wan't to put an independent output)
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 = 1090.00			'Safe start for sliding the tool in and out of rack
SpindleEmpty = 0			'Is the spindle empty
'This two variables are for security and see if the tool open and know if a tool is inside and in this test macro aren't used
Const OpenColletSensor = 2		'Sensor for see if the collet are openend
Const ToolInside = 1 			'Tool are inside the spindle sensor (doesn't work for see if the collet are closed because the sensor only works in some positions of the spindle for see if are closed without tool, but for indicate the tool is inside work in all positions)



'this variables are for future upgrades and are commented
'ToolRackSensor = Input "x"     'When i have time i wan't to add a limit switch for stop the process if the toolrack doesnt move to position (aircompresor off for example).



'-------Security Check: is spindle running? ----
If GetOEMLED (11) Then
Message "Spidnle working STOP the spindle and retry!! ABORT!!!"
Exit Sub
End If



'-------Security check: Is the machine moving? if is moving wait ----
While IsMoving()			'Wait for finish moving
Sleep(100)				'Sleep for avoid cpu consumption
Wend					'continue with the main program




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




'Do we need a new tool
If NewTool = OldTool Then		
	'I don't need to change tool
	Message "Correct 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
	'Fisrt check if any tool are inside (maybe i made a manual tool change with an independent button i have in the cnc for make manual tool changes)
'	If Not IsActive(ToolInside) Then 	'If the collet doesn't have a tool inside
'		Sleep(1000)
'		Message("The spindle doesn't have tool inside! and mach3 think i have a tool loaded!!")   'this part i need to polish it because not's necesary to stop the tool change only avoid to put tool in rack because i dont have tool, but for the moment i want to see when i have this problem see what error i made
'		Sleep(1000)
'		End
'		Exit Sub
'		
'	Else				'continue main code
		Code "G00 G53 Z" & ZSafe                'Move to Z axis safe
		Call WaitMove(100)			'Wait for finish moving to Zsafe
		ActivateSignal(ToolRack)		'Activate the pneumatic tool holder
		Sleep(1000)
		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
		Call WaitMove (100)
		DeActivateSignal(DrawBar)               'Clamp drawbar (without wait for close the collet because it's not needed here).
		DeActivateSignal(ToolRack)              'Return tool rack to retract position
'
'	End If     'This part is finished and can continue to next security check
'
'		'Check if the tool are droped at toolrack
'	If IsActive(ToolInside) Then 		'If the collet have a tool inside STOP!!! the tool arent droped
'		Sleep(1000)
'		Message("The spindle still have tool inside!!")
'		Exit Sub
'	
'		Else    'if tool arent inside all go in right direction
'		Message("Tool number [Tool #" & OldTool & "] are in tool rack")   'Only for debug and more info
'		Sleep(1000)
'	End If

	'continue main code	
	Sleep(250)
	SpindleEmpty = 0                        'No tool in spindle
End Sub

'------------------------------------------------------------------------

Sub GetToolFromRack
'	'First check if any tool are inside
'	If IsActive(ToolInside) Then 	'If have a tool inside spindle
'		Sleep(1000)
'		Message("The spindle have tool inside! and mach3 think i don't have a tool loaded!!")   'something failed and the spindle aren't empty
'		Sleep(1000)
'		Exit Sub
'	Else
		'Get tool from rack			'start getting the tool from toolrack
		ActivateSignal(ToolRack)		'Activate the pneumatic tool holder
		Sleep(1000)
		Code "G00 G53 Z" & ZSafe                'Move Z axis safe
		Call WaitMove(100)
		Sleep(500)				'Little wait for wait pneumatic tool holder (toolrack)
		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 (open collet)
		Code "G01 G53 Z" & ZPos & " F" & FFeed	'Down to engage the tool
		Call WaitMove(100)
		DeActivateSignal(DrawBar)		'Clamp the tool
		Sleep(1000)				'Wait for tool to clamp
'	'This part is finished and continue with the next checks for continue the tool change
'
'		'---check of the sensor for see if the tool are loaded with the sensor of the spindle---	
'		If Not IsActive(ToolInside) Then 		'If the tool aren't loaded (empty tool rack, maybe i extracted the tool manually and i don't put it in the tool rack) 
'			Sleep(1000)
'			Message ("No tool in spindle -> Abort")
'			Sleep(200)
'			Code "M30"		'Stop The program
'		End If	
'	
'	'We got a tool in spindle so we continue
		Code "G53 Y" & YPosSlide		'Slide out of rack
		Code "G00 G53 Z" & ZSafe		'Move to Z safe position
		Call WaitMove(100)
		DeActivateSignal(ToolRack)		'Retract the pneumatic toolrack
		SpindleEmpty = 1                        'Spindle has tool loaded
'	End If '
	
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 = 74.00				'Here specify  the Machine coords of the tool 1 position.
					Ypos = 1116.00				'same for all cases
					Zpos = -131.20
		Case is = 2
					Xpos = 144
					Ypos = 1116.00
					Zpos = -131.20			
		Case is = 3
					Xpos = 214
					Ypos = 1116.00
					Zpos = -131.20
		Case is = 4
					Xpos = 284
					Ypos = 1116.00
					Zpos = -131.20
		Case is = 5
					Xpos = 354
					Ypos = 1116.00
					Zpos = -131.20
		Case is = 6
					Xpos = 423.3
					Ypos = 1116.00
					Zpos = -131.20
		Case is = 7
					Xpos = 492.4
					Ypos = 1116.00
					Zpos = -131.20
		Case is = 8
					Xpos = 561.5
					Ypos = 1116.00
					Zpos = -131.20
		Case is = 9
					Xpos = 630.6
					Ypos = 1116.00
					Zpos = -131.20
	End Select
End Sub