'------------------------------------------------------------------------------
'-----------------------------
'------------------------------------------------------------------------------
' CS-Lab s.c.
' Toolchanger VB macro (MILL - Flat ATC with toolholders on the work table)
' v1.3
'------------------------------------------------------------------------------
'-----------------------------
'------------------------------------------------------------------------------


'------------------------------------------------------------------------------
' Digital outputs CONFIG
'------------------------------------------------------------------------------
Const OUT_OPENCOLLET	=	10	' open collet CSMIO output
Const OUT_CLEANCONE	=	11	' cone cleaning CSMIO output
Const OUT_CURTAIN	=	12	' raise curtain CSMIO output

'------------------------------------------------------------------------------
' Digital inputs CONFIG
'------------------------------------------------------------------------------
Const IN_COLLETCLOSED	=	20		' collet locked sensor CSMIO input
Const IN_COLLETOPENED	=	21		' collet open sensor CSMIO input
Const IN_TOOLINSIDE	=	22		' tool locked sensor CSMIO input

'------------------------------------------------------------------------------
' variables declaration - don't modify
'------------------------------------------------------------------------------
Dim Z_TOOLGET, Z_LIFT, Z_SAFE, X_FORSLIDE, X_LOCK, Y_BASE, Y_TOOLOFFSET, TOOLCOUNT
Dim ToolNew, ToolOld
Dim SoftLimitsState As Boolean

'------------------------------------------------------------------------------
' CONFIG data
'------------------------------------------------------------------------------
	Y_TOOLOFFSET = 170.00	' distance between tools
	Y_BASE = 42.93		' Y position of first tool
	Z_TOOLGET = -311.3	' Z position (abs) of tool get/relase
	Z_SAFE = -220		' Safe Z for safe moving above tool holders
	Z_LIFT = 10		' rise distance for cone cleaning
	X_FORSLIDE = -50	' X position before slide tool into the holder
	X_LOCK = -100		' X position for lock tool into the holder
	TOOLCOUNT = 6		' Max. tool number
'------------------------------------------------------------------------------




'------------------------------------------------------------------------------
  ' ------------------------
  ' Macro start
  ' ------------------------
'------------------------------------------------------------------------------
  	SetUserLED(1001, 1)	' UserLED 1001 - private info, toolchange is in progress
  
	' Get the current tool # and new tool #
	ToolOld = GetCurrentTool()
	ToolNew = GetSelectedTool()
	
	' save softlimits state
	If(GetOEMLED(23)) Then
		SoftLimitsState = true
	Else
		SoftLimitsState = false
	End If
	
	' turn off softlimits - toolholders can be outside normal work area
	SoftLimitsOff
	
	' raise the curtain
	Call SetCsmioOut (OUT_CURTAIN, true)
	

	' check are the axes referenced
	If GetOEMLed(807) Then
		SetCurrentTool( ToolOld )
		SetUserLED(1001, 0)
		DoOEMButton(1003)
		DoOEMButton(1002)
		Sleep(150)
		Message( "Machine is not referenced" )
		Sleep(150)
		' restore softlimit
		SetSoftLimits(SoftLimitsState)
		' curtain down
		Call SetCsmioOut (OUT_CURTAIN, false)
		End
	End If
	If GetOEMLed(808) Then
		SetCurrentTool( ToolOld )
		SetUserLED(1001, 0)
		DoOEMButton(1003)
		DoOEMButton(1002)
		Sleep(150)
		Message( "Machine is not referenced" )
		Sleep(150)
		' restore softlimit
		SetSoftLimits(SoftLimitsState)
		' curtain down
		Call SetCsmioOut (OUT_CURTAIN, false)
		End
	End If
	If GetOEMLed(809) Then
		SetCurrentTool( ToolOld )
		SetUserLED(1001, 0)
		DoOEMButton(1003)
		DoOEMButton(1002)
		Sleep(150)
		Message( "Machine is not referenced" )
		Sleep(150)
		' restore softlimit
		SetSoftLimits(SoftLimitsState)
		' curtain down
		Call SetCsmioOut (OUT_CURTAIN, false)
		End
	End If
	If GetOEMLed(11) Then
		SetCurrentTool( ToolOld )
		SetUserLED(1001, 0)
		DoOEMButton(1003)
		DoOEMButton(1002)
		Sleep(150)
		Message( "Spidnle is running. Toolchange failed." )
		Sleep(150)
		' restore softlimit
		SetSoftLimits(SoftLimitsState)
		' curtain down
		Call SetCsmioOut (OUT_CURTAIN, false)
		End
	End If 
	
	' Spindle must be turned off for toolchange
	If (ToolNew = ToolOld) Then
		If (IsToolInside()) Then
			SetCurrentTool( ToolOld )
			SetUserLED(1001, 0)
			Sleep(150)
			Message("Tool is already in spindle.")
			Sleep(150)
			' restore softlimit
			SetSoftLimits(SoftLimitsState)
			' curtain down
			Call SetCsmioOut (OUT_CURTAIN, false)
			End
		End If
	End If
	
	' New tool # must be <= TOOLCOUNT
	If (ToolNew > TOOLCOUNT) Then
		SetCurrentTool( ToolOld )
		SetUserLED(1001, 0)
		DoOEMButton(1003)
		DoOEMButton(1002)
		Sleep(150)
		Message("There is no such a tool.")
		Sleep(150)
		' restore softlimit
		SetSoftLimits(SoftLimitsState)
		' curtain down
		Call SetCsmioOut (OUT_CURTAIN, false)
		End
	End If
	
	Code("G0G53 Z0.0")	' Z up to machine '0'
	WaitForMove
		
	' if current tool > 0 put it first into the holder
	If (ToolOld > 0) Then
		' if there is no tool in spindle, skip this part
		If (IsToolInside()) Then
			' move to the toolholder
			Code("G0G53 X"& X_FORSLIDE &" Y" & (Y_BASE + (Y_TOOLOFFSET * (ToolOld-1))))
			WaitForMove
			Code("G0G53 Z" & Z_TOOLGET)
			WaitForMove
			Code("G0G53 X" & X_LOCK)
			WaitForMove
			ColletOpen
			CleanConeOn
			Sleep(1000)
			' is collet opened?
			If (Not IsColletOpened()) Then
				ColletClose
				CleanConeOff
				Sleep(1000)
				Code("G0G53 X"& X_FORSLIDE)
				WaitForMove
				Code("G0G53 Z0")
				Code("G0G53 X0Y0")
				WaitForMove
				SetCurrentTool( ToolOld )
				SetUserLED(1001, 0)
				DoOEMButton(1021)
				DoOEMButton(1002)
				Sleep(150)
				Message("Can't open collet! Toolchange failed.")
				Sleep(150)
				' restore softlimit
				SetSoftLimits(SoftLimitsState)
				' curtain down
				Call SetCsmioOut (OUT_CURTAIN, false)
				End
			End If
			Code("G0G53 Z" & Z_SAFE)
			WaitForMove
			CleanConeOff
			ColletClose
		End If	
	End If

	' If newtool = 0, there is no need to get new tool
	If (ToolNew = 0) Then
		Code("G0G53 Z0")
		WaitForMove
		Code("G0G53 X0Y0")
		WaitForMove
		SetCurrentTool( ToolNew )
		Message("Tool loaded : " & ToolNew)
		Code("G43T" & ToolNew)	' G43 to sync tool length in system
		Code("G43H" & ToolNew)	' G43 to sync tool length in system
		WaitForMove
		SetUserLED(1001, 0)
		' restore softlimits
		SetSoftLimits(SoftLimitsState)
		' curtain down
		Call SetCsmioOut (OUT_CURTAIN, false)
		End
	End If

	' Is there no tool in the spindle?
	If (IsToolInside()) Then
		Code("G0G53 Z0")
		Code("G0G53 X0Y0")
		WaitForMove
		SetCurrentTool( ToolOld )
		SetUserLED(1001, 0)
		WaitForMove
		DoOEMButton(1021)
		DoOEMButton(1002)
		Sleep(150)
		Message("Can't get new tool, because other tool is already in spindle!")
		Sleep(150)
		' restore softlimits
		SetSoftLimits(SoftLimitsState)
		' curtain down
		Call SetCsmioOut (OUT_CURTAIN, false)
		End
	End If

	' move to the new tool position
	Code("G0G53 X" & X_LOCK &" Y" & (Y_BASE + (Y_TOOLOFFSET * (ToolNew-1))))
	WaitForMove
	ColletOpen
	Sleep(1000)
	' check is the collet opened
	If (Not IsColletOpened) Then
		CleanConeOff
		ColletClose

		Sleep(1000)
		Code("G0G53 X"& X_FORSLIDE)
		WaitForMove
		Code("G0G53 Z0")
		Code("G0G53 X0Y0")
		WaitForMove

		SetCurrentTool( 0 )
		SetUserLED(1001, 0)
		DoOEMButton(1021)
		DoOEMButton(1002)
		Sleep(150)
		Message("Can't open collet! Toolchange failed!")
		Sleep(150)
		' restore softlimits
		SetSoftLimits(SoftLimitsState)
		' curtain down
		Call SetCsmioOut (OUT_CURTAIN, false)
		End
	End If

	' cone cleaning
	CleanConeOn
	Code("G0G53 Z" & (Z_TOOLGET + Z_LIFT))
	Code("G1G53 Z" & Z_TOOLGET & " F3000")
	Code("G1G53 Z" & (Z_TOOLGET + Z_LIFT) & " F3000")
	Code("G1G53 Z" & Z_TOOLGET & " F1500")
	WaitForMove
	' colled close
	CleanConeOff
	Sleep(500)
	ColletClose
	Sleep(1000)
	' check is tool locked?
	If (Not IsToolInside()) Then
		Code("G0G53 X"& X_FORSLIDE)
		WaitForMove
		Code("G0G53 Z0")
		Code("G0G53 X0Y0")
		WaitForMove

		SetCurrentTool( 0 )
		SetUserLED(1001, 0)
		DoOEMButton(1021)
		DoOEMButton(1002)
		Sleep(150)
		Message("Toolchange failed. Tool should be locked!")
		Sleep(150)
		' restore softlimits
		SetSoftLimits(SoftLimitsState)
		' curtain down
		Call SetCsmioOut (OUT_CURTAIN, false)
		End
	End If
	
	' slide out from toolholder position
	Code("G0G53 X" & X_FORSLIDE)
	WaitForMove
	Code("G0G53 Z0")
	WaitForMove
	' Go to absolute 0,0
	Code("G0G53 X0 Y0")
	WaitForMove
  
	SetCurrentTool( ToolNew )
	Message("Tool Loaded: " & ToolNew)
	Code("G43T" & ToolNew)	' G43 to sync tool length in system
	Code("G43H" & ToolNew)	' G43 to sync tool length in system
	WaitForMove
	SetUserLED(1001, 0)
	' restore softlimit
	SetSoftLimits(SoftLimitsState)
	' curtain down
	Call SetCsmioOut (OUT_CURTAIN, false)
	End 
' -----------------------------------------------------------------------------
Public Function IsToolInside () As Boolean
	If(GetCsmioIn(IN_TOOLINSIDE)) Then
		IsToolInside = true
	Else
		IsToolInside = false
	End If
	Exit Function
End Function
' -----------------------------------------------------------------------------
Public Function IsColletOpened () As Boolean
	If(GetCsmioIn(IN_COLLETOPENED)) Then
		IsColletOpened = true
	Else
		IsColletOpened = false
	End If
	Exit Function
End Function
' -----------------------------------------------------------------------------
Public Function IsColletClosed () As Boolean
	If(GetCsmioIn(IN_COLLETCLOSED)) Then
		IsColletClosed = true
	Else
		IsColletClosed = false
	End If
	Exit Function
End Function
' -----------------------------------------------------------------------------
Sub ColletClose ()
	Call SetCsmioOut(OUT_OPENCOLLET, false)
End Sub
' -----------------------------------------------------------------------------
Sub ColletOpen ()
	Call SetCsmioOut(OUT_OPENCOLLET, true)
End Sub
' -----------------------------------------------------------------------------
Sub CleanConeOn ()
	Call SetCsmioOut(OUT_CLEANCONE, true)
End Sub
' -----------------------------------------------------------------------------
Sub CleanConeOff ()
	Call SetCsmioOut(OUT_CLEANCONE, false)
End Sub 
' -----------------------------------------------------------------------------
Sub SoftLimitsOff ()
	If(GetOEMLED(23)) Then
		DoOEMButton(1029)
	End If
End Sub
' -----------------------------------------------------------------------------
Sub SetSoftLimits (state As Boolean)
	If(state) Then
		If(GetOEMLED(23) = 0) Then
			DoOEMButton(1029)
		End If
	Else
		If(GetOEMLED(23)) Then
			DoOEMButton(1029)
		End If
	End If
End Sub
' -----------------------------------------------------------------------------
Sub WaitForMove ()
	While IsMoving() 
		Sleep(15) 
	Wend
End Sub
' -----------------------------------------------------------------------------
Public Function GetCsmioIn (n As Integer) As Boolean
	Dim reg As Integer
	
	If(n < 16) Then
		reg = 90
	Else
		reg = 91
		n = n - 16
	End If
	
	If(GetInBit(reg, n)) Then
		GetCsmioIn = true
	Else
		GetCsmioIn = false
	End If
	Exit Function
End Function 
' -----------------------------------------------------------------------------
Public Sub SetCsmioOut (ByVal n As Integer, ByVal state As Boolean)
	If(state) Then
		SetOutBit(90, n)
	Else
		ResetOutBit(90, n)
	End If
End Sub
' -----------------------------------------------------------------------------