-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathM6End.vb
162 lines (122 loc) · 3.85 KB
/
M6End.vb
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
REM The default script here moves the tool back to m6start if any movement has occured during the tool change..
'Tool height measurement
Dim ZOFFSET, ZTOP, ZBOTTOM, ZGODOWN, ZABS, ZUPFINE, Z_SAFE
Dim ToolLen
Dim Tool
Dim SENS_Z, SENS_X, SENS_Y, Z_LIFT, Z_PARK, MAX_DTRAVEL
Dim SPD_FAST, SPD_FINE
' -----------------------------------------------------
' Configuration parameters
' -----------------------------------------------------
SENS_X = 15.9 ' Sensor X position
SENS_Y = 10.75 ' Sensor Y position
SENS_Z = -43.8031 ' Sensor Z position
Z_SAFE = -5 ' Safe Z above sensor (for rapid move)
MAX_DTRAVEL = 70 ' Max down travel
SPD_FAST = 1000
SPD_FINE = 100 ' Fast and fine speeds
Z_LIFT = 1.5 ' Z lift for fine probing
Z_PARK = -5 ' Park after probe absolute Z position
' -----------------------------------------------------
Message( "Dokonuję pomiaru długości narzędzia..." )
SanityCheck
ToolLengthCompensationOff
MoveInPosition
ToolLen = Probe()
SetToolLength(ToolLen)
ToolLengthCompensationOn
Message ("Measured tool length: " & ToolLen)
'Restore position
x = GetVar(1)
y = GetVar(2)
z = GetVar(3)
x = GetToolChangeStart( 0 )
y = GetToolChangeStart( 1 )
z = GetToolChangeStart( 2 )
a = GetToolChangeStart( 3 )
b = GetToolChangeStart( 4 )
c = GetToolChangeStart( 5 )
Code("G53 G0 Z " & Z_SAFE)
WaitForMove
Code("G90 G0 X" & x & "Y" & y)
WaitForMove
' -----------------------------------------------------------------------------
Sub SanityCheck ()
If GetOEMLed(825) Then
Message( "TC ERROR: Czujnik pomiaru cały czas załączony! pomiar przerwany." )
End
End If
If GetOEMLed(11) Then
Message( "TC ERROR: Do pomiaru narzędzia wyłącz wrzeciono!" )
End
End If
Tool = GetDRO(24) ' Get current tool index
' narzedzie 0 nie podlega pomiarowi
If(Tool = 0) Then
Message( "TC INFO: Narzędzie 0 nie podlega pomiarom długości" )
End
End If
End Sub
Sub MoveInPosition ()
' Move to PARK
Code("G0G53 Z" & Z_PARK)
WaitForMove
' Move to XY position of tool correction sensor
' Code("G0G53 X" & SENS_X & " Y" & SENS_Y)
Code("G0G53 X" & SENS_X)
Code("G0G53 Y" & SENS_Y)
WaitForMove
' Get actual Z offset
ZOFFSET = GetOEMDRO(49)
' Rapid go down to safe distance above sensor
Code("G0G53 Z" & Z_SAFE)
WaitForMove
End Sub
Function Probe ()
' Probe
ZTOP = GetDRO(2) ' actual Z position
ZGODOWN = ZTOP - MAX_DTRAVEL ' max down travel
Code("G31 Z" & ZGODOWN & "f" & SPD_FAST)
WaitForMove
ZBOTTOM = GetProbeActivationPoint()
If ZBOTTOM=GetDRO(2) Then
Message( "TC ERROR: Probe not hit - tool too short/long")
End
End If
ZUPFINE = GetOEMDRO(85) + Z_LIFT
Code("G1G53 Z" & ZUPFINE & "f" & SPD_FAST) ' go up Z_LIFT
WaitForMove
' fine probe
Code("G31 Z" & ZGODOWN & "f" & SPD_FINE)
WaitForMove
ZBOTTOM = GetProbeActivationPoint()
' Lift Z to abs park position
Sleep(50)
Code("G0G53 Z" & Z_PARK)
WaitForMove
ZABS = ZBOTTOM + ZOFFSET
ToolLen = -(SENS_Z - ZABS)
WaitForMove
Probe=ToolLen
End Function
Sub SetToolLength(length)
SetOEMDRO(42, length)
End Sub
Sub ToolLengthCompensationOff()
Code("G43T0")
Code("G43H0") ' turn off tool length compensation
WaitForMove
End Sub
Sub ToolLengthCompensationOn()
Code("G43T" & Tool) ' G43 to sync value in system
Code("G43H" & Tool) ' G43 to sync value in system
WaitForMove
End Sub
Sub WaitForMove ()
While IsMoving()
Sleep(15)
Wend
End Sub
Function GetProbeActivationPoint ()
GetProbeActivationPoint=GetVar(2002)
End Function