Global X_SENS as Double
Global Y_SENS as Double
Global Z_SENS as Double
Global Const ProbeDia = 10.0
Global Const vbCritical = 16
Global Touch as Double

'probe left lower
Public Function TouchLLO() 
	Message "probe left lower corner"
	'first X+ 
	Touch=TouchX(50)
	'then X back
	Code "F400"
	Code "G91 G01 X-5"
	WaitForMove(False)
	'now Y-
	Code "G91 G01 Y-30"
	WaitForMove(True)
	' X+
	Code "G91 G01 X25"
	WaitForMove(True)
	'probe Y 
	Touch=TouchY(50)
	'Y back
	Code "F400"
	Code "G91 G01 Y-5"
	WaitForMove(False)
	Code "G90"
End function

'probe right lower
Public Function TouchRLO()
	Message "probe right lower corner"
	'first X-
	Touch=TouchX(-50)
	'then X back
	Code "F400"
	Code "G91 G01 X5"
	WaitForMove(False)
	'now Y-
	Code "G91 G01 Y-30"
	WaitForMove(True)
	' X-
	Code "G91 G01 X-25"
	WaitForMove(True)
	'probe Y 
	Touch=TouchY(50)
	'Y back
	Code "F400"
	Code "G91 G01 Y-5"
	WaitForMove(False)
	Code "G90"
End function

'probe left top
Public Function TouchLO()
	Message "probe left top corner"
	'first X+
	Touch=TouchX(50)
	'X back
	Code "F400"
	Code "G91 G01 X-5"
	WaitForMove(False)
	'now Y+
	Code "G91 G01 Y30"
	WaitForMove(True)
	' X+
	Code "G91 G01 X25"
	WaitForMove(True)
	'probe Y
	Touch=TouchY(-50)
	'Y back
	Code "F400"
	Code "G91 G01 Y5"
	WaitForMove(False)
	Code "G90"
End function

'probe right top
Public Function TouchRO()
	Message "probe right top corner"
	'first X-
	Touch=TouchX(-50)
	'X back
	Code "F400"
	Code "G91 G01 X5"
	WaitForMove(False)
	'now Y-
	Code "G91 G01 Y30"
	WaitForMove(True)
	' X+
	Code "G91 G01 X-25"
	WaitForMove(True)
	'probe Y
	Touch=TouchY(-50)
	'Y back
	Code "F400"
	Code "G91 G01 Y5"
	WaitForMove(False)
	Code "G90"
End function

'probe inner diameter
Public Function TouchInner()
	Message "probe inner hole"
	Dim Touch
	'probe active ?
	If GetOEMLed(825) Then
		MSGBox "probe input activ",vbOK + vbCritical ,"error !"
		Exit function
	End If	

	'clear display
	SetUserDro(1051,0)
	SetUserDro(1053,0)
	
	'probe X-
	Message "probe inner X-"
	Probe -100,0,0
	Touch = GetVar(2000)
	Code "G90 G01 X" + CStr(Touch)
	WaitForMove(True)
	Code "G92.2"
	SetOEMDro(800,0)
	
	'probe X+
	Message "probe inner X+"
	'clear probe
	Code "F100"
	Code "G91 G01 X1"
	WaitForMove(False)
	Probe 200,0,0
	Touch = GetVar(2000)
	Code "G90 G01 X" + CStr(Touch)
	WaitForMove(False)
	'save value
	SetUserDRO(1051,GetOEMDro(800))
	'goto middle
	Message "X goto middle"
	Code "F150"
	Code "G90 G01 X" + CStr(GetUserDRO(1051)-(GetUserDRO(1051)/2))
	WaitForMove(False)
	'X zero
	Code "G92.2"
	SetOEMDro(800,0)

	'probe Y-
	Message "probe inner Y-"
	Probe 0,-100,0
	Touch = GetVar(2001)
	Code "G90 G01 Y" + CStr(Touch)
	WaitForMove(False)
	Code "G92.2"
	SetOEMDro(801,0)
	
	'probe Y+
	Message "probe inner Y+"
	'clear probe
	Code "F100"
	Code "G91 G01 Y1"
	WaitForMove(False)
	Probe 0,200,0
	Touch = GetVar(2001)
	Code "G90 G01 Y" + CStr(Touch)
	WaitForMove(False)
	'save value
	SetUserDRO(1053,GetOEMDro(801))
	'goto middle
	Message "Y goto middle"
	Code "F250"
	Code "G90 G01 Y" + CStr(GetUserDRO(1053)-(GetUserDRO(1053)/2))
	WaitForMove(False)
	'Y zero
	Code "G92.2"
	SetOEMDro(801,0)

	'do X probe in middle of Y again
	'probe X-
	Message "probe inner X-"
	Probe -100,0,0
	Touch = GetVar(2000)
	Code "G90 G01 X" + CStr(Touch)
	WaitForMove(False)
	Code "G92.2"
	SetOEMDro(800,0)
	
	'probe X+
	Message "probe inner X+"
	'clear probe
	Code "F100"
	Code "G91 G01 X1"
	WaitForMove(False)
	Probe 200,0,0
	Touch = GetVar(2000)
	Code "G90 G01 X" + CStr(Touch)
	WaitForMove(False)
	'save value
	SetUserDRO(1051,GetOEMDro(800))
	'goto middle
	Message "X goto middle"
	Code "F250"
	Code "G90 G01 X" + CStr(GetUserDRO(1051)-(GetUserDRO(1051)/2))
	WaitForMove(False)
	'X zero
	Code "G92.2"
	SetOEMDro(800,0)
	Code "G90"
	Message " "
End Function

'probe outer diameter
Public function TouchOuter()
	Message "probe outer"
	Dim Touch
	'probe input activ
	If GetOEMLed(825) Then
		MSGBox "probe input activ",vbOK + vbCritical ,"error !"
		Exit function
	End If	

	'clear display
	SetUserDro(1050,0)
	SetUserDro(1052,0)
	
	'probe X+
	Message "probe outer X+"
	Probe 200,0,0
	Touch = GetVar(2000)
	Code "G90 G01 X" + CStr(Touch)
	WaitForMove(False)
	'X zero
	Code "G92.2"
	SetOEMDro(800,0)

	'got X + Position 
	'Z clear
	Code "F400"
	Message "Z clear"
	Code "G91 G01 Z30"
	WaitForMove(False)
	'X+
	Message "got X+ "
	Code "G91 G01 X" + CStr(GetUserDro(1054) +20)
	WaitForMove(True)
	'Z down again
	Message "Z down"
	Code "G91 G01 Z-30"
	WaitForMove(True)

	'probe X-
	Message "probe outer X-"
	Probe -100,0,0
	Touch = GetVar(2000)
	Code "G90 G01 X" + CStr(Touch)
	WaitForMove(False)
	'save value
	SetUserDRO(1050,GetOEMDro(800))

	'goto middle
	'Z clear
	Message "Z clear"
	Code "F400"
	Code "G91 G01 Z30"
	WaitForMove(False)
	Message "got X middle"
	Code "G90 G01 X" + CStr(GetUserDRO(1050)-(GetUserDRO(1050)/2))
	WaitForMove(False)
	
	'X zero
	Code "G92.2"
	SetOEMDro(800,0)
	
	'Y-
	Message "Y- "
	Code "G91 G01 Y-" + CStr((GetUserDro(1054)/2) +20)
	WaitForMove(True)
	'Z down again
	Message "Z down"
	Code "G91 G01 Z-30"
	WaitForMove(True)
	
	'probe Y+
	Message "probe outer Y+"
	Probe 0,200,0
	Touch = GetVar(2001)
	Code "G90 G01 Y" + CStr(Touch)
	WaitForMove(False)
	'Y zero
	Code "G92.2"
	SetOEMDro(801,0)

	'goto Y+ 
	'Z clear
	Message "Z clear"
	Code "F400"
	Code "G91 G01 Z30"
	WaitForMove(False)
	'Y+
	Message "Y+"
	Code "G91 G01 Y" + CStr(GetUserDro(1054) +20)
	WaitForMove(True)
	'Z down again
	Message "Z down"
	Code "G91 G01 Z-30"
	WaitForMove(True)

	'probe Y-
	Message "probe outer Y-"
	Probe 0,-100,0
	Touch = GetVar(2001)
	Code "G90 G01 Y" + CStr(Touch)
	WaitForMove(False)
	'save value
	SetUserDRO(1052,GetOEMDro(801))
	
	'goto middle
	'Z clear
	Message "Z clear"
	Code "F400"
	Code "G91 G01 Z30"
	WaitForMove(False)
	Message "Y Mitte drive"
	Code "G90 G01 Y" + CStr(GetUserDRO(1052)-(GetUserDRO(1052)/2))
	WaitForMove(False)	
	
	'Y zero
	Code "G92.2"
	SetOEMDro(801,0)	
	
	'X-
	Message "X-"
	Code "G91 G01 X-" + CStr((GetUserDro(1054)/2) +20)
	WaitForMove(True)
	'Z down again
	Message "Z down"
	Code "G91 G01 Z-30"
	WaitForMove(True)	
	
	'probe X+
	Message "probe outer X+"
	Probe 200,0,0
	Touch = GetVar(2000)
	Code "G90 G01 X" + CStr(Touch)
	WaitForMove(False)
	'X zero
	Code "G92.2"
	SetOEMDro(800,0)

	'X + 
	'Z clear
	Code "F400"
	Message "Z clear"
	Code "G91 G01 Z30"
	WaitForMove(False)
	'X+
	Message "X+"
	Code "G91 G01 X" + CStr(GetUserDro(1054) +20)
	WaitForMove(True)
	'Z down again
	Message "Z down"
	Code "G91 G01 Z-30"
	WaitForMove(True)

	'probe X-
	Message "probe outer X-"
	Probe -100,0,0
	Touch = GetVar(2000)
	Code "G90 G01 X" + CStr(Touch)
	WaitForMove(False)
	'save value
	SetUserDRO(1050,GetOEMDro(800))

	'goto middle
	'Z clear
	Message "Z clear"
	Code "F400"
	Code "G91 G01 Z30"
	WaitForMove(False)
	Message "X middle"
	Code "G90 G01 X" + CStr(GetUserDRO(1050)-(GetUserDRO(1050)/2))
	WaitForMove(False)
	
	'X zero
	Code "G92.2"
	SetOEMDro(800,0)	
	
End Function

'probe twisted surface
Public Function TouchTwisted()
	Message "probe twisted surface"

	'get parameters
	X = GetUserDro (1055)
	Y = GetUserDro (1056)
	CurrentToolDiameter = ProbeDia

	'check parameter
	If X = 0 Then
		MSGBox "Delta X to smal!",vbOK + vbCritical ,"error !"
		Exit function	
	End If
	If Y = 0 Then
		MSGBox "Delta Y to smal!",vbOK + vbCritical ,"error !"
		Exit function	
	End If
	
	'PI -----------------------------------------------
	pi=4*Atn(1)

	'turn off coordinate rotation
	Code "G69"
	Sleep(200)
	
	'X / Y to zero
    Call SetDro (0,0)
    Call SetDro (1,0)
	Sleep(200)

	'probe the Y side

	'probe P1 
	Probe 100,0,0
	'coordinates P2
    X1 = GetVar(2000) 
    Y1=0 
	
	'preposition P2 drive
	Message("preposition P2 drive")
    Y2 = Y
    Code "G91 G1 X-" &X & "F1800"
	WaitForMove(True)
    Code "G91 G1 Y" &Y2
	WaitForMove(True)

	'probe P2 
	Probe 100,0,0
	'coordinates P2
    X2 = GetVar(2000) 

    'probe the X side -----------------------------------------------

	'preposition P3 drive
	Message("preposition P3 drive")
	X3 = X1 + (CurrentToolDiameter/2) + 10
    Code "G91 G1 X-" &X & "F1800"
	WaitForMove(True)
    Code "G90 G1 Y-30"
	WaitForMove(True)
    Code "G90 G1 X" &X3 
	WaitForMove(True)

	'probe P3 
	Probe 0,100,0
	'coordinates P3
    Y3 = GetVar(2001)  

	'preposition P4 drive
	Message("preposition P4 drive")
    X4 = X3 + X
    Code "G91 G1 Y-20 F1800"
	WaitForMove(True)
    Code "G90 G1 X" &X4
	WaitForMove(True)
	
	'probe P4 
	Probe 0,100,0
	'coordinates P4
    Y4 = GetVar(2001) 'get the probe touch location

	'Z auffahren
	Code "G91 G01 Y-0.2 F1800"
	WaitForMove(True)
    Code "G91 G1 Z20"
	WaitForMove(True)
	
	'calculation
	t1= (X4-X3)*(Y1-Y3)-(Y4-Y3)*(X1-X3)
	b1= (Y4-Y3)*(X2-X1)-(X4-X3)*(Y2-Y1)
	ua=t1/b1

	'calculate X0/Y0
	XH=X1+ua*(X2-X1) ' X Home  distance away
	YH=Y1+ua*(Y2-Y1) ' Y Home  distance away


	
	'calculate angle out longer side
	If X>Y Then
		'longer side=X
		a=(Y4-Y3)
		b=(X4-X3)
		v=a/b
		NewAngle=Atn(v)*(180/pi)
	Else
		'longer side=Y
		a=(X2-X1)
		b=(Y2-Y1)
		v=b/a
		NewAngle=Atn(v)*(180/pi)

		If NewAngle > 0 Then
			NewAngle=NewAngle-90
		Else
			NewAngle=NewAngle+90
		End If
	End If
'	Message("Neuer Winkel : " &NewAngle ) 
'	Sleep(200)

	'got new zeropoint
	'angle = calculated angle + 45°
	siWi = 2*Pi/360*(NewAngle+45)
	hyWi = SQR((CurrentToolDiameter/2)*(CurrentToolDiameter/2)*2)

	Dx = hyWi * Cos(siWi)
	Dy = hyWi * Sin(siWi)

	'goto new zeropoint
    Code "G90 G1 X" &XH+Dx & " Y" &YH+Dy 
	WaitForMove(True)
	
	'set new zeropoint
    Call SetDro (0,0)
    Call SetDro (1,0)

	'set cordinate rotation
    Call SetOemDro (118,NewAngle)
	
	
	'go back to absolute
	Code "G90"
	Message ""
	
End Function
				


'probe X
Public Function TouchX(Dist as Double) as Double
	Probe Dist,0,0
	TouchX = GetVar(2000)
	Code "G90 G01 X" + CStr(TouchX)
	WaitForMove(False)

	Code "F50"
	Code "G92.2"
	'um 1/2 Tastkopf versetzt zero
	If Dist > 0 Then
		SetOEMDro(800,(ProbeDia/2)*-1)
	else
		SetOEMDro(800,ProbeDia/2)
	End if
	Code "G90"
End Function

'probe Y
Public Function TouchY(Dist as Double) as Double
	Probe 0,Dist,0
	TouchY = GetVar(2001)
	Code "G90 G01 Y" + CStr(TouchY)
	WaitForMove(False)

	Code "F50"
	Code "G92.2"
	'um 1/2 Tastkopf versetzt zero
	If Dist > 0 Then
		SetOEMDro(801,(ProbeDia/2)*-1)
	else
		SetOEMDro(801,ProbeDia/2)
	End if
	Code "G90"
End Function

'probe function
Public Function Probe(X as Double, Y as Double , Z as Double)
	Dim axis
	
	'check probe activ?
	If GetOEMLed(825) Then
		MSGBox "Probe aktiv",vbOK + vbCritical ,"error !"
		DoButton(21)
		End
	End If	
	'probing allways with 0 Grad
	Code "G69"
	
	'clear esult
	SetVar(2000,0)	
	SetVar(2001,0)	
	SetVar(2002,0)	
	
	
	'messages
	IF X > 0 Then
		Message("Probe X+")
		axis = "X-"
	End if	
	IF X < 0 Then
		Message("Probe X-")
		axis = "X+"
	End if	
	IF Y > 0 Then
		Message("Probe Y+")
		axis = "Y-"
	End if	
	IF Y < 0 Then
		Message("Probe Y-")
		axis = "Y+"
	End if	
	IF Z > 0 Then
		Message("Probe Z+")
		axis = "Z-"
	End if	
	IF Z < 0 Then
		Message("Probe Z-")
		axis = "Z+"
	End if	
	Sleep(200)
	
	SetUserDRO(1511, X)	' X
	SetUserDRO(1512, Y)	' Y
	SetUserDRO(1513, Z)	' Z
	
	'1.probe
	Code "F200"
	NotifyPlugins(10103)
	
	SetUserDRO(1510, 0)
	While(GetUserDRO(1510) = 0)
		Sleep(50)
		NotifyPlugins(10104)
	Wend 
	
	If(GetUserDRO(1510) = 1) Then
		Message "Probe 1.hit at: " &GetVar(2000) &"," &GetVar(2001) &"," &GetVar(2002)
		Sleep(200)
	End If
	
	If(GetUserDRO(1510) = 2) Then
		Message("Probe End (no hit)")
		End
	End If
	
	'Probe clear
	Message "Probe clear"
	While GetOEMLed(825) 
		Code "G91 G01 " &axis &"0.1 F600"
		WaitForMove(false)
	Wend	
	Code "G90"
	
	'2.probe
	Code "F2"
	NotifyPlugins(10103)
	
	SetUserDRO(1510, 0)
	While(GetUserDRO(1510) = 0)
		Sleep(50)
		NotifyPlugins(10104)
	Wend 
	
	If(GetUserDRO(1510) = 1) Then
		Message "Probe 2.hit at: " &GetVar(2000) &"," &GetVar(2001) &"," &GetVar(2002)
		Sleep(200)
	End If
	
	If(GetUserDRO(1510) = 2) Then
		Message("Probe End (no hit)")
		End
	End If

	'Probe clear
	Message "Probe clear"
	While GetOEMLed(825) 
		Code "G91 G01 " &axis &"0.1 F600"
		WaitForMove(false)
	Wend	
	Code "G90"
	
	X_SENS = GetVar(2000)	
	Y_SENS = GetVar(2001)	
	Z_SENS = GetVar(2002)	

End Function

'wait for move
Sub WaitForMove (Check as Boolean)
	While IsMoving() 
		'check if we are driving against probe
		If ((GetOEMLed(825) = True) and (Check = True)) Then
			MSGBox "Probe hit during probe",vbOK + vbCritical ,"error !"
			DoButton(21)
			End
		End If	
		Sleep(5) 
	Wend
End Sub

'read CSMIO input
Public Function GetCsmioIn (ByVal adr As Integer, ByVal n As Integer) As Boolean
	If(GetInBit(adr, n)) Then
		GetCsmioIn = true
	Else
		GetCsmioIn = false
	End If
End Function