Sub Main()

' Auto Tool Measurement M6Start Script
' Author verser
' vers.by - touch probes, tool setters and precision home switches

Dim ToolChangeAbsXPos
Dim ToolChangeAbsYPos
Dim ToolSetterAbsXPos
Dim ToolSetterAbsYPos
Dim ZAbsSafePlane
Dim ZAbsStartPos
Dim ZMaxDist

Dim FRate1, FRate2
Dim DMax, Latch, EdgeLength
Dim XYclearance
Dim ToolNo
Dim ToolD
Dim ProbeD
Dim Zdepth
Dim XHit, YHit, ZHit
Dim XbHit, YbHit, Diameter 

Dim Zpos

Dim XScale, YScale, ZScale

Dim AbsIncFlag

Dim CurrentFeed
Dim CurrentSpindle


'Init vars
ToolChangeAbsXPos = 310
ToolChangeAbsYPos = 246
ToolSetterAbsXPos = 310
ToolSetterAbsYPos = 246
ZAbsSafePlane = 0
ZAbsStartPos = 0
ZMaxDist = 150
FRate1 = Abs(GetUserDRO(1821))
FRate2 = Abs(GetUserDRO(1822))
DMax = Abs(GetUserDRO(1823))
ToolNo = GetCurrentTool()
ToolD = GetToolParam(ToolNo,1)
   If GetUserDRO(1829) = 0 Then	
   	ProbeD = ToolD
	Else 
	ProbeD = GetUserDRO(1829)
   End If
Latch = Abs(GetUserDRO(1825))
XYclearance = GetUserDRO(1826)
EdgeLength = GetUserDRO(1828)
Zdepth = GetUserDRO(1830)

AbsIncFlag = GetOEMLED(49)   ' Get the current G91 state

'Temporary save all Axis Scale factors
XScale = GetOEMDRO(59)
YScale = GetOEMDRO(60)
ZScale = GetOEMDRO(61)

'Set All Axis' Scale to 1
SetOEMDRO(59,1)
SetOEMDRO(60,1)
SetOEMDRO(61,1)
Sleep(250)


'Check for Errors

If GetOemLED(16)<>0 Then ' Check for Machine Coordinates
Message "Please change to working coordinates"
SetOEMDRO(59,XScale)
SetOEMDRO(60,YScale)
SetOEMDRO(61,ZScale)
Sleep(250)
Exit Sub ' Exit if in Machine Coordinates
End If

If GetOemLED(825)<>0 Then
Message "Probe is active! Check connection and try again"
Call SetOEMDRO(59,XScale)
Call SetOEMDRO(60,YScale)
Call SetOEMDRO(61,ZScale)
Sleep(250)
Exit Sub ' Exit if probe is tripped
End If


CurrentFeed = GetOEMDRO(818) 'FeedRate()
CurrentSpindle = GetOEMDRO(39) 

'main working

    'Safe Go to start position under toolsetter
	If Not SafeMoveZ(ZAbsSafePlane-GetOEMDRO(85),1500) Then 
		PushMSG("M6End Script is interrupted")
		Exit Sub 
	End If
	If Not SafeMoveXY(ToolSetterAbsXPos-GetOEMDRO(83),ToolSetterAbsYPos-GetOEMDRO(84),1500) Then 
		PushMSG("M6End Script is interrupted")
		Exit Sub 
	End If
	If Not SafeMoveZ(ZAbsStartPos-GetOEMDRO(85),1500) Then 
		PushMSG("M6End Script is interrupted")
		Exit Sub 
	End If
	'Probe Z-
	ZHit=ProbeZ(-1,ZMaxDist,Latch,FRate1,FRate2)
	If ZHit=999999 Then 
		Exit Sub 
	End If
	
	SetUserDRO(1832, ZHit)
	
	'Save Z start position
	Zpos = GetOEMDRO(802)
	'Safe Go to start position under toolsetter
	If Not SafeMoveZ(ZAbsSafePlane-GetOEMDRO(85),1500) Then 
		PushMSG("M6Start Script is interrupted")
		Exit Sub 
	End If
	If Not SafeMoveXY(ToolChangeAbsXPos-GetOEMDRO(83),ToolChangeAbsYPos-GetOEMDRO(84),1500) Then 
		PushMSG("M6Start Script is interrupted")
		Exit Sub 
	End If
	
'epilog

SetOEMDRO(59,XScale)
SetOEMDRO(60,YScale)
SetOEMDRO(61,ZScale)
Sleep(250)

Call SetLED49(AbsIncFlag)
SetOEMDRO(818,CurrentFeed) 'SetFeedRate(CurrentFeed)
Sleep(125)

End Sub


'Functions

Function ProbeZ(Dir,DMax,Latch,FRate1,FRate2)
	Dim Res
    Dim Zstart, ZEnd
	Dim Ftmp
	Dim AbsIncF
	ProbeZ=999999
	Zstart = GetOEMDRO(802)
	Ftmp = FeedRate() 'FeedRate()
	AbsIncF=GetOEMLED(49)
	'Fast Probe Z
	Code "G91"
	Code "F" & FRate1
	Sleep(125)
	Call WaitProbeReady()	
	Code "G31 Z" & (Dir*DMax)
	While IsMoving()
	Wend
	Res = GetOEMDRO(802)
'	PushMSG("Res=" & Res &", Zstart=" & Zstart & ", DMax=" & DMax & ", FRate1=" & FRate1)
	If Abs(Res - Zstart - Dir*DMax) < 0.01 Then
		PushMSG("Error: G31 Z search finished without making contact")
		PushMSG("Manually return to the starting position and repeat the search")
		Call SetLED49(AbsIncF)
		Code "M05"
		Sleep(125)
		Code "S" & CurrentSpindle
		Sleep(125)
		SetOEMDRO(818,Ftmp)
		Sleep(125)
		Exit Function 
	End If 
	'Move back
	Code "G00 Z" & -Dir*Latch
	While IsMoving()
	Wend
	Call WaitProbeReady()	
	'Latch Probe Z
	Zstart = GetOEMDRO(802)
	Code "F" & FRate2
	Sleep(125)
	Code "G31 Z" & Dir*Latch*2
	While IsMoving()
	Wend
	'Save result
	ZEnd = GetOEMDRO(802)
	Res = GetOEMDRO(85)
	If Abs(ZEnd - Zstart - Dir*Latch*2) < 0.01 Then
		PushMSG("Error: G31 Z latch finished without making contact")
		PushMSG("Manually return to the starting position and repeat the search")
		Call SetLED49(AbsIncF)
		Code "M05"
		Sleep(125)
		Code "S" & CurrentSpindle
		Sleep(125)
		SetOEMDRO(818,Ftmp)
		Sleep(125)
		Exit Function 
	End If 
	Code "G00 Z" & -Dir*Latch
	While IsMoving()
	Wend
	Call SetLED49(AbsIncF)
	SetOEMDRO(818,Ftmp)
	Sleep(125)
	ProbeZ=Res
End Function


Function SafeMoveX(X1, F1) As Boolean 'return 1 (error) if probe tripped
    Dim Xstart
	Dim Ftmp
	Dim AbsIncF
	SafeMoveX=True
	Xstart = GetOEMDRO(800)
	Ftmp = FeedRate() 'FeedRate()
	AbsIncF=GetOEMLED(49)
	Code "G91"
	Code "F" & F1
	Sleep(125)
	Call WaitProbeReady()	
	Code "G31 X" & X1
	While IsMoving()
	Wend
	XHit = GetOEMDRO(800)
	Call SetLED49(AbsIncF)
	If Abs(XHit - Xstart - X1) > 0.01 Then
		SafeMoveX=False
		PushMSG("Error! Probe tripped during X movement")
		Code "M05"
		Sleep(125)
		Code "S" & CurrentSpindle
		Sleep(125)
		SetOEMDRO(818,Ftmp)
		Sleep(125)
	End If 
	SetOEMDRO(818,Ftmp)
	Sleep(125)
End Function

Function SafeMoveY(Y1, F1) As Boolean 'return 1 (error) if probe tripped
    Dim Ystart
	Dim Ftmp
	Dim AbsIncF
	SafeMoveY=True
	Ystart = GetOEMDRO(801)
	Ftmp = FeedRate() 'FeedRate()
	AbsIncF=GetOEMLED(49)
	Code "G91"
	Code "F" & F1
	Sleep(125)
	Call WaitProbeReady()	
	Code "G31 Y" & Y1
	While IsMoving()
	Wend
	YHit = GetOEMDRO(801)
	Call SetLED49(AbsIncF)
	If Abs(YHit - Ystart - Y1) > 0.01 Then
		SafeMoveY=False
		PushMSG("Error! Probe tripped during Y movement")
		Code "M05"
		Sleep(125)
		Code "S" & CurrentSpindle
		Sleep(125)
		SetOEMDRO(818,Ftmp)
		Sleep(125)
	End If 
	SetOEMDRO(818,Ftmp)
	Sleep(125)
End Function

Function SafeMoveXY(X1, Y1, F1) As Boolean 'return 1 (error) if probe tripped
    Dim Xstart
    Dim Ystart
	Dim Ftmp
	Dim AbsIncF
	SafeMoveXY=True
	Xstart = GetOEMDRO(800)
	Ystart = GetOEMDRO(801)
	Ftmp = FeedRate() 'FeedRate()
	AbsIncF=GetOEMLED(49)
	Code "G91"
	Code "F" & F1
	Sleep(125)
	Call WaitProbeReady()	
	Code "G31 X" & X1
	While IsMoving()
	Wend
	Code "G31 Y" & Y1
	While IsMoving()
	Wend
	XHit = GetOEMDRO(800)
	YHit = GetOEMDRO(801)
	Call SetLED49(AbsIncF)
	If (Abs(XHit - Xstart - X1) > 0.01) Or (Abs(YHit - Ystart - Y1) > 0.01) Then
		SafeMoveXY=False
		PushMSG("Error! Probe tripped during XY movement")
		Code "M05"
		Sleep(125)
		Code "S" & CurrentSpindle
		Sleep(125)
		SetOEMDRO(818,Ftmp)
		Sleep(125)
	End If 
	SetOEMDRO(818,Ftmp)
	Sleep(125)
End Function

Function SafeMoveZ(Z1, F1) As Boolean 'return 1 (error) if probe tripped
    Dim Zstart
	Dim Ftmp
	Dim AbsIncF
	SafeMoveZ=True
	Zstart = GetOEMDRO(802)
	Ftmp = FeedRate() 'FeedRate()
	AbsIncF=GetOEMLED(49)
	Code "G91"
	Code "F" & F1
	Sleep(125)
	Call WaitProbeReady()	
	Code "G31 Z" & Z1
	While IsMoving()
		Sleep(100)
	Wend
	ZHit = GetOEMDRO(802)
	Call SetLED49(AbsIncF)
	If Abs(ZHit - Zstart - Z1) > 0.01 Then
		SafeMoveZ=False
		PushMSG("Error! Probe tripped during Z movement")
		Code "M05"
		Sleep(125)
		Code "S" & CurrentSpindle
		Sleep(125)
		SetOEMDRO(818,Ftmp)
		Sleep(125)
	End If 
	SetOEMDRO(818,Ftmp)
	Sleep(125)
End Function

Function PushMSG(Str1 As String) As Boolean
	SetUserLabel (21,GetUserLabel(20))
	SetUserLabel (20,GetUserLabel(19))
	SetUserLabel (19,GetUserLabel(18))
	SetUserLabel (18,GetUserLabel(17))
	SetUserLabel (17,Str1)
	Message Str1
	PushMSG=True
End Function
   
Sub WaitProbeReady()
	While GetOemLED(825)
		Sleep(100)
	Wend
End Sub

Sub SetLED49(Flag)
	If Flag Then
		Code "G91"
		Sleep(125)
	Else
		Code "G90"
		Sleep(125)
	End If
End Sub