'------------------------------------------------------------------------------
' CS-Lab s.c. - Auto toolchanger for lathe (rotary type of ATC)
' (C) 2011
' v1.2 - 2011-11-05
'------------------------------------------------------------------------------
' CONFIG
'------------------------------------------------------------------------------
Const TOOLCOUNT			= 8 	' max Tool #
' Actual ATC position inputs (CSMIO-IP inputs)
Const InT1 = 14
Const InT2 = 13
Const InT3 = 14 And 13
Const InT4 = 12
Const InT5 = 14 And 12
Const InT6 = 13 And 12
Const InT7 = 14 And 13 And 12
Const InT8 = 11
' ATC sensors inputs
Const GenevaSensor		= 10	' Geneva Cam on position
' digital outputs for ATC control (CSMIO outputs)
Const OutRun			= 2		' unlock and rotate
'------------------------------------------------------------------------------
' program variables - don't modify
'------------------------------------------------------------------------------
Dim ToolNew, ToolOld, ToolSlot As Integer
Dim ToolRdy As Boolean
Dim ATC_CloseRetry
Dim InToolSens(TOOLCOUNT)
'------------------------------------------------------------------------------
  ' MACRO START
'------------------------------------------------------------------------------
	if(GetOEMLed(800)) Then ' MACH is at RESET condition
		Message("System is at ESTOP - Tool Change terminated")
		Sleep(150)
		WaitForMove
		End
	End If

  	SetUserLED(1001, 1) ' User LED - toolchange in progress (Pause Feed Hold condition)
  	
  	InToolSens(0) = InT1
  	InToolSens(1) = InT2
  	InToolSens(2) = InT3
  	InToolSens(3) = InT4
  	InToolSens(4) = InT5
  	InToolSens(5) = InT6
  	InToolSens(6) = InT7
  	InToolSens(7) = InT8

	' Get actual tool # and new tool #
	ToolOld = GetCurrentTool()
	ToolNew = GetSelectedTool()

	' little trick for double slots ATC
	If(ToolNew >= 10) Then
		ToolSlot = ToolNew - 10
	Else
		ToolSlot = ToolNew
	End If
	
	If(ToolOld >= 10) Then
		ToolOld = ToolOld - 10
	End If
	
	' The same position - no need to move
	If (ToolSlot = ToolOld) Then
		SetCurrentTool( ToolNew )
		Message("ATC is on position "& ToolSlot & " and new tool is " & ToolNew)
		WaitForMove
		SetUserLED(1001, 0) ' Pause Feed Hold off
		End
	End If
	
	If (ToolSlot > TOOLCOUNT) Then
		SetCurrentTool( ToolOld )
		SetUserLED(1001, 0) ' Pause Feed Holf off
		DoOEMButton(1003) ' Stop
		DoOEMButton(1002) ' G-Code Rewind
		Sleep(150)
		Message("There is no tool T" & ToolNew & " in ATC")
		Sleep(150)
		End
	End If
	
	
	' T0 is a fake tool, no need to change
	If (ToolSlot = 0) Then
		ToolNew = 0
		SetCurrentTool( ToolNew )
		Message("Succesful loaded tool T" & ToolNew)
		WaitForMove
		SetUserLED(1001, 0) ' Pause Feed Hold off
		End
	End If
	

	' unlock and rotate ATC
	RunATC
	Sleep(150)
	' wait to ATC will be on the desired position
	ToolRdy = false
	While(Not ToolRdy)
		If(GetCsmioIn((InToolSens(ToolSlot - 1)))) Then
			ToolRdy = true
		End If
	Wend
	
	
	' stop rotation and lock ATC
	ATC_CloseRetry = 0
	' Check lock sensor, retry lock if needed
	While(Not GetCsmioIn(GenevaSensor) And ATC_CloseRetry < 4)
		StopATC
		ATC_CloseRetry = ATC_CloseRetry + 1
	Wend
	
	' double check for succesful ATC lock
	If (Not GetCsmioIn(GenevaSensor)) Then
		SetCurrentTool( 0 )
		SetUserLED(1001, 0) ' Pause Feed Hold off
'		DoOEMButton(1021) ' Reset
		DoOEMButton(1002) ' G-Code Rewind
		Sleep(150)
		Message("ATC lock alert. Check ATC.")
		Sleep(150)
		End
	End If
  
	SetCurrentTool( ToolNew )
	Message("Succesful loaded tool T" & ToolNew)
	Sleep(150)
	WaitForMove
	SetUserLED(1001, 0) ' Pause Feed Hold off
	End
' -----------------------------------------------------------------------------
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 
' -----------------------------------------------------------------------------
Sub RunATC ()
	Call SetCsmioOut(OutRun, true)
	Sleep(150)
End Sub
' -----------------------------------------------------------------------------
Sub StopATC ()
	Call SetCsmioOut(OutRun, false)
	Sleep(150)
End Sub
' -----------------------------------------------------------------------------
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
' -----------------------------------------------------------------------------
Sub WaitForMove ()
	While IsMoving() 
		Sleep(15) 
	Wend
End Sub