'------------------------------------------------------------------------------
' Updated: 29Apr16 - 08:26
'
' CS-Lab s.c.
' Toolchanger VB macro (MILL - Flat ATC with toolholders on the work table)
' v1.3
' 
'------------------------------------------------------------------------------
' Gcodes, LEDs, Buttons
'------------------------------------------------------------------------------
' Gcode		G43=		G53=
' LED 		11=Spindle	23=SoftLimits	806=Tool Change	807=X-Ref	808=Y-Ref	809=Z-Ref
' Button	1002=Rewind	1003=Stop	1021=Reset	1029=SoftLimitToggle

'------------------------------------------------------------------------------
' Digital outputs CONFIG
'------------------------------------------------------------------------------
Const OUT_OPENCOLLET	=	10	' open collet CSMIO output

'------------------------------------------------------------------------------
' Digital inputs CONFIG
'------------------------------------------------------------------------------
Const IN_COLLETOPENED	=	21		' collet open sensor CSMIO input

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

'------------------------------------------------------------------------------
' CONFIG data
'------------------------------------------------------------------------------
	X_BASE = 258.668		' X position of first tool
	Y_BASE = 22.75			' Y position of first tool
	Z_SAFE = -5				' Safe Z for safe moving above tool holders
	
	Y_TOOLOFFSET = 24.800	' Distance between tools
	Z_TOOLGET = -71.500		' Z position (abs) of tool get/release. Final Z_TOOLGET=59
	TOOLCOUNT = 5			' Max. tool number
		
	SENS_X = 260			' Tool length sensor X
	SENS_Y = 147			' Tool length sensor Y
'------------------------------------------------------------------------------


'--------------------------- MACRO START --------------------------------------
  

    ' Slow down spindle and stop before getting to tool holder
	Message( "Stopping spindle ..." )
	Code("G0G53 Z" & Z_SAFE)
	WaitForMove
	
    Code("S500")
	Sleep (2000)
	Code("M05")
	Sleep(3000)
	
	' 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
	
	' check XYZ are the axes referenced
	If GetOEMLed(807) Then
		SetCurrentTool( ToolOld )
		DoOEMButton(1003)
		DoOEMButton(1002)
		Sleep(150)
		Message( "Machine X is not referenced" )
		Sleep(150)
		' restore softlimit
		SetSoftLimits(SoftLimitsState)
		End
	End If
	If GetOEMLed(808) Then
		SetCurrentTool( ToolOld )
		DoOEMButton(1003)
		DoOEMButton(1002)
		Sleep(150)
		Message( "Machine Y is not referenced" )
		Sleep(150)
		' restore softlimit
		SetSoftLimits(SoftLimitsState)
		End
	End If
	If GetOEMLed(809) Then
		SetCurrentTool( ToolOld )
		DoOEMButton(1003)
		DoOEMButton(1002)
		Sleep(150)
		Message( "Machine Z is not referenced" )
		Sleep(150)
		' restore softlimit
		SetSoftLimits(SoftLimitsState)
		End
	End If
	If GetOEMLed(11) Then
		SetCurrentTool( ToolOld )
		DoOEMButton(1003)
		DoOEMButton(1002)
		Sleep(150)
		Message( "Spindle is still running. Toolchange failed." )
		Sleep(150)
		' restore softlimit
		SetSoftLimits(SoftLimitsState)
		End
	End If 
	
	' tool requested already in spindle
	If (ToolNew = ToolOld) Then
		SetCurrentTool( ToolOld )
		Sleep(150)
		Message("Tool is already in spindle.")
		Sleep(150)
		' restore softlimit
		SetSoftLimits(SoftLimitsState)
		End
	End If
	
	' New tool # must be <= TOOLCOUNT
	If (ToolNew > TOOLCOUNT) Then
		SetCurrentTool( ToolOld )
		DoOEMButton(1003)
		DoOEMButton(1002)
		Sleep(150)
		Message("There is no such a tool.")
		Sleep(150)
		' restore softlimit
		SetSoftLimits(SoftLimitsState)
		End
	End If
	
	' New tool pickup ok - continue
	Code("G0G53 Z" & Z_SAFE)	' Z up to safe
	WaitForMove
		
	' if current tool > 0 put it first into the holder
	' Tool already in holder so unload
	If (ToolOld > 0) Then
		' move to the toolholder
		Code("G0G53 X"& X_BASE &" Y" & (Y_BASE + (Y_TOOLOFFSET * (ToolOld-1))))
		WaitForMove
				
		Code("G0G53 Z" & Z_TOOLGET)
		WaitForMove
		
		ColletOpen
		Sleep(1000)

		' move down and pickup tool
		Code("G0G53 Z" & Z_SAFE)
		WaitForMove
		
		ColletClose
	End If

	' If newtool = 0, there is no need to get new tool
	If (ToolNew = 0) Then
		Code("G0G53 Z" & Z_SAFE)
		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
		' restore softlimits
		SetSoftLimits(SoftLimitsState)
		
		Code("G0G53 X" & SENS_X &" Y" & SENS_Y)
	  End
	End If

	' move to the new tool position
	Code("G0G53 X" & X_BASE &" Y" & (Y_BASE + (Y_TOOLOFFSET * (ToolNew-1))))
	WaitForMove
	
	ColletOpen
	Sleep(1000)

    ' put tool in collect	
	Code("G0G53 Z" & (Z_TOOLGET))
	WaitForMove
	
	' collet close
	ColletClose
	Sleep(1000)
		
	' lift from toolholder position
	Code("G0G53 Z" & Z_SAFE)
	WaitForMove
	Sleep(1000)
	
	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
	
	' restore softlimit
	SetSoftLimits(SoftLimitsState)
	
	' Does this fix Z moving randomly?
	Code("G0G53 X" & SENS_X &" Y" & SENS_Y)
	End 


	
' -----------------------------------------------------------------------------
' Procedures & Functions
' -----------------------------------------------------------------------------
Sub ColletClose ()
	Call SetCsmioOut(OUT_OPENCOLLET, false)
End Sub

' -----------------------------------------------------------------------------
Sub ColletOpen ()
	Call SetCsmioOut(OUT_OPENCOLLET, true)
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
' -----------------------------------------------------------------------------