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

'Antasten links unten
Public Function TouchLU() 
	Message "Antasten links unten"
	'zuerst mal X antasten
	Touch=TouchX(50)
	'dann X wegfahren
	Code "F400"
	Code "G91 G01 X-5"
	WaitForMove(False)
	'jetzt Y-
	Code "G91 G01 Y-30"
	WaitForMove(True)
	' X+
	Code "G91 G01 X25"
	WaitForMove(True)
	'Y antasten
	Touch=TouchY(50)
	'Y freifahren
	Code "F400"
	Code "G91 G01 Y-5"
	WaitForMove(False)
	Code "G90"
End function

'Antasten rechts unten
Public Function TouchRU()
	Message "Antasten rechts unten"
	'zuerst mal X antasten
	Touch=TouchX(-50)
	'dann X wegfahren
	Code "F400"
	Code "G91 G01 X5"
	WaitForMove(False)
	'jetzt Y-
	Code "G91 G01 Y-30"
	WaitForMove(True)
	' X+
	Code "G91 G01 X-25"
	WaitForMove(True)
	'Y antasten
	Touch=TouchY(50)
	'Y freifahren
	Code "F400"
	Code "G91 G01 Y-5"
	WaitForMove(False)
	Code "G90"
End function

'Antasten links oben
Public Function TouchLO()
	Message "Antasten links oben"
	'zuerst mal X antasten
	Touch=TouchX(50)
	'dann X wegfahren
	Code "F400"
	Code "G91 G01 X-5"
	WaitForMove(False)
	'jetzt Y-
	Code "G91 G01 Y30"
	WaitForMove(True)
	' X+
	Code "G91 G01 X25"
	WaitForMove(True)
	'Y antasten
	Touch=TouchY(-50)
	'Y freifahren
	Code "F400"
	Code "G91 G01 Y5"
	WaitForMove(False)
	Code "G90"
End function

'Antasten rechts oben
Public Function TouchRO()
	Message "Antasten rechts oben"
	'zuerst mal X antasten
	Touch=TouchX(-50)
	'dann X wegfahren
	Code "F400"
	Code "G91 G01 X5"
	WaitForMove(False)
	'jetzt Y-
	Code "G91 G01 Y30"
	WaitForMove(True)
	' X+
	Code "G91 G01 X-25"
	WaitForMove(True)
	'Y antasten
	Touch=TouchY(-50)
	'Y freifahren
	Code "F400"
	Code "G91 G01 Y5"
	WaitForMove(False)
	Code "G90"
End function

'Antasten innen
Public Function TouchInnen()
	Message "Antasten Innen"
	Dim Touch
	'Probe bereits aktiv
	If GetOEMLed(825) Then
		MSGBox "Probe bereits aktiv",vbOK + vbCritical ,"Fehler !"
		Exit function
	End If	

	'Anzeigen löschen
	SetUserDro(1051,0)
	SetUserDro(1053,0)
	
	'1. Versuch X-
	Message "Antasten innen X-"
	Probe -100,0,0
	Touch = GetVar(2000)
	Code "G90 G01 X" + CStr(Touch)
	WaitForMove(True)
	Code "G92.2"
	SetOEMDro(800,0)
	
	'1. Versuch X+
	Message "Antasten innen X+"
	'Taster freifahren
	Code "F100"
	Code "G91 G01 X1"
	WaitForMove(False)
	Probe 200,0,0
	Touch = GetVar(2000)
	Code "G90 G01 X" + CStr(Touch)
	WaitForMove(False)
	'Wert abspeichern
	SetUserDRO(1051,GetOEMDro(800))
	'in die Mitte fahren
	Message "X Mitte fahren"
	Code "F150"
	Code "G90 G01 X" + CStr(GetUserDRO(1051)-(GetUserDRO(1051)/2))
	WaitForMove(False)
	'X nullen
	Code "G92.2"
	SetOEMDro(800,0)

	'1. Versuch Y-
	Message "Antasten innen Y-"
	Probe 0,-100,0
	Touch = GetVar(2001)
	Code "G90 G01 Y" + CStr(Touch)
	WaitForMove(False)
	Code "G92.2"
	SetOEMDro(801,0)
	
	'1. Versuch Y+
	Message "Antasten innen Y+"
	'Taster freifahren
	Code "F100"
	Code "G91 G01 Y1"
	WaitForMove(False)
	Probe 0,200,0
	Touch = GetVar(2001)
	Code "G90 G01 Y" + CStr(Touch)
	WaitForMove(False)
	'Wert abspeichern
	SetUserDRO(1053,GetOEMDro(801))
	'in die Mitte fahren
	Message "Y Mitte fahren"
	Code "F250"
	Code "G90 G01 Y" + CStr(GetUserDRO(1053)-(GetUserDRO(1053)/2))
	WaitForMove(False)
	'Y nullen
	Code "G92.2"
	SetOEMDro(801,0)

	'X Antasten in der Mitte von Y wiederholen
	'1. Versuch X-
	Message "Antasten innen X-"
	Probe -100,0,0
	Touch = GetVar(2000)
	Code "G90 G01 X" + CStr(Touch)
	WaitForMove(False)
	Code "G92.2"
	SetOEMDro(800,0)
	
	'1. Versuch X+
	Message "Antasten innen X+"
	'Taster freifahren
	Code "F100"
	Code "G91 G01 X1"
	WaitForMove(False)
	Probe 200,0,0
	Touch = GetVar(2000)
	Code "G90 G01 X" + CStr(Touch)
	WaitForMove(False)
	'Wert abspeichern
	SetUserDRO(1051,GetOEMDro(800))
	'in die Mitte fahren
	Message "X Mitte fahren"
	Code "F250"
	Code "G90 G01 X" + CStr(GetUserDRO(1051)-(GetUserDRO(1051)/2))
	WaitForMove(False)
	'X nullen
	Code "G92.2"
	SetOEMDro(800,0)
	Code "G90"
	Message " "
End Function

'Antasten aussen
Public function TouchAussen()
	Message "Antasten Aussen"
	Dim Touch
	'Probe bereits aktiv
	If GetOEMLed(825) Then
		MSGBox "Probe bereits aktiv",vbOK + vbCritical ,"Fehler !"
		Exit function
	End If	

	'Anzeigen löschen
	SetUserDro(1050,0)
	SetUserDro(1052,0)
	
	'1. Versuch X+
	Message "Antasten aussen X+"
	Probe 200,0,0
	Touch = GetVar(2000)
	Code "G90 G01 X" + CStr(Touch)
	WaitForMove(False)
	'X nullen
	Code "G92.2"
	SetOEMDro(800,0)

	'Auf X + Position fahren
	'Z freifahren
	Code "F400"
	Message "Z freifahren"
	Code "G91 G01 Z30"
	WaitForMove(False)
	'X  plus fahren
	Message "X+ anfahren"
	Code "G91 G01 X" + CStr(GetUserDro(1054) +20)
	WaitForMove(True)
	'Z wieder abfahren
	Message "Z abfahren"
	Code "G91 G01 Z-30"
	WaitForMove(True)

	'1. Versuch X-
	Message "Antasten aussen X-"
	Probe -100,0,0
	Touch = GetVar(2000)
	Code "G90 G01 X" + CStr(Touch)
	WaitForMove(False)
	'Wert abspeichern
	SetUserDRO(1050,GetOEMDro(800))

	'in die Mitte fahren
	'Z freifahren
	Message "Z freifahren"
	Code "F400"
	Code "G91 G01 Z30"
	WaitForMove(False)
	Message "X Mitte anfahren"
	Code "G90 G01 X" + CStr(GetUserDRO(1050)-(GetUserDRO(1050)/2))
	WaitForMove(False)
	
	'X nullen
	Code "G92.2"
	SetOEMDro(800,0)
	
	'Y- anfahren
	Message "Y- anfahren"
	Code "G91 G01 Y-" + CStr((GetUserDro(1054)/2) +20)
	WaitForMove(True)
	'Z wieder abfahren
	Message "Z abfahren"
	Code "G91 G01 Z-30"
	WaitForMove(True)
	
	'1. Versuch Y+
	Message "Antasten aussen Y+"
	Probe 0,200,0
	Touch = GetVar(2001)
	Code "G90 G01 Y" + CStr(Touch)
	WaitForMove(False)
	'Y nullen
	Code "G92.2"
	SetOEMDro(801,0)

	'Auf Y+ Position fahren
	'Z freifahren
	Message "Z freifahren"
	Code "F400"
	Code "G91 G01 Z30"
	WaitForMove(False)
	'Y  plus fahren
	Message "Y+ anfahren"
	Code "G91 G01 Y" + CStr(GetUserDro(1054) +20)
	WaitForMove(True)
	'Z wieder abfahren
	Message "Z abfahren"
	Code "G91 G01 Z-30"
	WaitForMove(True)

	'1. Versuch Y-
	Message "Antasten aussen Y-"
	Probe 0,-100,0
	Touch = GetVar(2001)
	Code "G90 G01 Y" + CStr(Touch)
	WaitForMove(False)
	'Wert abspeichern
	SetUserDRO(1052,GetOEMDro(801))
	
	'in die Mitte fahren
	'Z freifahren
	Message "Z freifahren"
	Code "F400"
	Code "G91 G01 Z30"
	WaitForMove(False)
	Message "Y Mitte anfahren"
	Code "G90 G01 Y" + CStr(GetUserDRO(1052)-(GetUserDRO(1052)/2))
	WaitForMove(False)	
	
	'Y nullen
	Code "G92.2"
	SetOEMDro(801,0)	
	
	'X- anfahren
	Message "X- anfahren"
	Code "G91 G01 X-" + CStr((GetUserDro(1054)/2) +20)
	WaitForMove(True)
	'Z wieder abfahren
	Message "Z abfahren"
	Code "G91 G01 Z-30"
	WaitForMove(True)	
	
	'1. Versuch X+
	Message "Antasten aussen X+"
	Probe 200,0,0
	Touch = GetVar(2000)
	Code "G90 G01 X" + CStr(Touch)
	WaitForMove(False)
	'X nullen
	Code "G92.2"
	SetOEMDro(800,0)

	'Auf X + Position fahren
	'Z freifahren
	Code "F400"
	Message "Z freifahren"
	Code "G91 G01 Z30"
	WaitForMove(False)
	'X  plus fahren
	Message "X+ anfahren"
	Code "G91 G01 X" + CStr(GetUserDro(1054) +20)
	WaitForMove(True)
	'Z wieder abfahren
	Message "Z abfahren"
	Code "G91 G01 Z-30"
	WaitForMove(True)

	'1. Versuch X-
	Message "Antasten aussen X-"
	Probe -100,0,0
	Touch = GetVar(2000)
	Code "G90 G01 X" + CStr(Touch)
	WaitForMove(False)
	'Wert abspeichern
	SetUserDRO(1050,GetOEMDro(800))

	'in die Mitte fahren
	'Z freifahren
	Message "Z freifahren"
	Code "F400"
	Code "G91 G01 Z30"
	WaitForMove(False)
	Message "X Mitte anfahren"
	Code "G90 G01 X" + CStr(GetUserDRO(1050)-(GetUserDRO(1050)/2))
	WaitForMove(False)
	
	'X nullen
	Code "G92.2"
	SetOEMDro(800,0)	
	
End Function

'Antasten Werkstück (gedreht)
Public Function TouchG()
	Message "Antasten schraeges Bauteil"

	'Parameter holen
	X = GetUserDro (1055)
	Y = GetUserDro (1056)
	CurrentToolDiameter = ProbeDia

	'Parameter prüfen
	If X = 0 Then
		MSGBox "Delta X zu klein!",vbOK + vbCritical ,"Fehler !"
		Exit function	
	End If
	If Y = 0 Then
		MSGBox "Delta Y zu klein!",vbOK + vbCritical ,"Fehler !"
		Exit function	
	End If
	
	'PI vorbesetzen -----------------------------------------------
	pi=4*Atn(1)

	'Koordinatendrehung ausschalten
	Code "G69"
	Sleep(200)
	
	'X / Y auf null setzen
    Call SetDro (0,0)
    Call SetDro (1,0)
	Sleep(200)

	'die Y Seite vermessen

	'P1 antasten
	Probe 100,0,0
	'Koordinaten P2
    X1 = GetVar(2000) 
    Y1=0 
	
	'Vorposition P2 anfahren
	Message("Vorposition P2 anfahren")
    Y2 = Y
    Code "G91 G1 X-" &X & "F1800"
	WaitForMove(True)
    Code "G91 G1 Y" &Y2
	WaitForMove(True)

	'P2 antasten
	Probe 100,0,0
	'Koordinaten P2
    X2 = GetVar(2000) 

    'die X Seite vermessen -----------------------------------------------

	'Vorposition P3 anfahren
	Message("Vorposition P3 anfahren")
	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)

	'P3 antasten
	Probe 0,100,0
	'Koordinaten P3
    Y3 = GetVar(2001)  

	'Vorposition P4 anfahren
	Message("Vorposition P4 anfahren")
    X4 = X3 + X
    Code "G91 G1 Y-20 F1800"
	WaitForMove(True)
    Code "G90 G1 X" &X4
	WaitForMove(True)
	
	'P4 antasten
	Probe 0,100,0
	'Koordinaten 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)
	
	'Brechnung
	t1= (X4-X3)*(Y1-Y3)-(Y4-Y3)*(X1-X3)
	b1= (Y4-Y3)*(X2-X1)-(X4-X3)*(Y2-Y1)
	ua=t1/b1

	'Nullpunkt berechnen
	XH=X1+ua*(X2-X1) ' X Home  distance away
	YH=Y1+ua*(Y2-Y1) ' Y Home  distance away

'	Message("Koordinaten Nullpunkt : X " &XH &" Y " &YH) 
'	Sleep(200)

	
	'Winkel aus der längeren Seite berechnen
	If X>Y Then
		'längere Seite=X
		a=(Y4-Y3)
		b=(X4-X3)
		v=a/b
		NewAngle=Atn(v)*(180/pi)
	Else
		'längere Seite=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)

	'zum neuen Nullpunkt fahren
	'Winkel = errechneter Winkel + 45°
	siWi = 2*Pi/360*(NewAngle+45)
	hyWi = SQR((CurrentToolDiameter/2)*(CurrentToolDiameter/2)*2)

	Dx = hyWi * Cos(siWi)
	Dy = hyWi * Sin(siWi)
'	Message("Koordinaten Berechnung : X " &Dx &" Y " &Dy) 
'	Sleep(200)

	'Zu neuen Nullpunkt fahren
    Code "G90 G1 X" &XH+Dx & " Y" &YH+Dy 
	WaitForMove(True)
	
	'neuen Nullpunkt setzen
    Call SetDro (0,0)
    Call SetDro (1,0)

	'Koordinatendrehung setzen
    Call SetOemDro (118,NewAngle)
	
	'Koordinaten ausgeben
'	Message "X1 " &X1 &" Y1 " &Y1 & "   X2 " &X2 &" Y2 " &Y2 & "   X3 " &X3 &" Y3 " &Y3 & "   X4 " &X4 &" Y4 " &Y4 
	
	'wieder auf absolut stellen
	Code "G90"
	Message ""
	
End Function
				


'Antasten 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 nullen
	If Dist > 0 Then
		SetOEMDro(800,(ProbeDia/2)*-1)
	else
		SetOEMDro(800,ProbeDia/2)
	End if
	Code "G90"
End Function

'Antasten 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 nullen
	If Dist > 0 Then
		SetOEMDro(801,(ProbeDia/2)*-1)
	else
		SetOEMDro(801,ProbeDia/2)
	End if
	Code "G90"
End Function

'Antastfunktion
Public Function Probe(X as Double, Y as Double , Z as Double)
	Dim axis
	
	'prüfen ob Probe bereits aktiv
	If GetOEMLed(825) Then
		MSGBox "Probe aktiv",vbOK + vbCritical ,"Fehler !"
		DoButton(21)
		End
	End If	
	'Antasten immer mit Konturdrehung 0 Grad
	Code "G69"
	
	'Messergebniss löschen
	SetVar(2000,0)	
	SetVar(2001,0)	
	SetVar(2002,0)	
	
	
	'Meldungen ausgeben
	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.Versuch
	Code "F200"
	NotifyPlugins(10103)
	
	SetUserDRO(1510, 0)
	While(GetUserDRO(1510) = 0)
		Sleep(50)
		NotifyPlugins(10104)
	Wend 
	
	If(GetUserDRO(1510) = 1) Then
		Message "Probe 1.Kontakt bei: " &GetVar(2000) &"," &GetVar(2001) &"," &GetVar(2002)
		Sleep(200)
	End If
	
	If(GetUserDRO(1510) = 2) Then
		Message("Probe Ende (keine Antastung)")
		End
	End If
	
	'Probe freifahren
	Message "Probe freifahren"
	While GetOEMLed(825) 
		Code "G91 G01 " &axis &"0.1 F600"
		WaitForMove(false)
	Wend	
	Code "G90"
	
	'2.Versuch
	Code "F2"
	NotifyPlugins(10103)
	
	SetUserDRO(1510, 0)
	While(GetUserDRO(1510) = 0)
		Sleep(50)
		NotifyPlugins(10104)
	Wend 
	
	If(GetUserDRO(1510) = 1) Then
		Message "Probe 2.Kontakt bei: " &GetVar(2000) &"," &GetVar(2001) &"," &GetVar(2002)
		Sleep(200)
	End If
	
	If(GetUserDRO(1510) = 2) Then
		Message("Probe Ende (keine Antastung)")
		End
	End If

	'Probe freifahren
	Message "Probe freifahren"
	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

'Bewegung abwarten
Sub WaitForMove (Check as Boolean)
	While IsMoving() 
		'prüfen ob wir gegen das Bauteil fahren
		If ((GetOEMLed(825) = True) and (Check = True)) Then
			MSGBox "Probe während Positionierfahrt aktiv",vbOK + vbCritical ,"Fehler !"
			DoButton(21)
			End
		End If	
		Sleep(5) 
	Wend
End Sub

'CSMIO Eingang lesen
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