1program main
2
3
4
5
6call manage_compensation(tool_diameter, programmed_path, compensation_mode)
7
8end program
9
10
11function manage_compensation(tool_dia, path, mode)
12
13
14
15
16
17
18
19declare compensated_path array
20declare tool_radius float
21
22tool_radius = tool_dia / 2.0
23
24if mode == 'G40' then
25
26return path
27elseif mode == 'G41' then
28
29compensated_path = calculate_left_compensation(tool_radius, path)
30elseif mode == 'G42' then
31
32compensated_path = calculate_right_compensation(tool_radius, path)
33else
34
35
36return path
37end if
38
39return compensated_path
40
41end function
42
43
44function calculate_left_compensation(radius, points)
45
46
47
48
49declare offset_points array
50declare current_point, next_point, prev_point point
51declare offset_vector vector
52
53if length(points) < 2 then
54return points
55end if
56
57
58current_point = points[0]
59next_point = points[1]
60offset_vector = calculate_offset_vector(current_point, next_point, null, radius, 'left')
61add_vector_to_point(current_point, offset_vector)
62add_point_to_array(offset_points, current_point)
63
64
65for i from 1 to length(points) - 2
66prev_point = points[i-1]
67current_point = points[i]
68next_point = points[i+1]
69offset_vector = calculate_offset_vector(current_point, next_point, prev_point, radius, 'left')
70add_vector_to_point(current_point, offset_vector)
71add_point_to_array(offset_points, current_point)
72end for
73
74
75current_point = points[length(points) - 1]
76prev_point = points[length(points) - 2]
77offset_vector = calculate_offset_vector(current_point, null, prev_point, radius, 'left')
78add_vector_to_point(current_point, offset_vector)
79add_point_to_array(offset_points, current_point)
80
81return offset_points
82
83end function
84
85
86function calculate_right_compensation(radius, points)
87
88
89
90
91
92declare offset_points array
93declare current_point, next_point, prev_point point
94declare offset_vector vector
95
96if length(points) < 2 then
97return points
98end if
99
100
101current_point = points[0]
102next_point = points[1]
103offset_vector = calculate_offset_vector(current_point, next_point, null, radius, 'right')
104add_vector_to_point(current_point, offset_vector)
105add_point_to_array(offset_points, current_point)
106
107
108for i from 1 to length(points) - 2
109prev_point = points[i-1]
110current_point = points[i]
111next_point = points[i+1]
112offset_vector = calculate_offset_vector(current_point, next_point, prev_point, radius, 'right')
113add_vector_to_point(current_point, offset_vector)
114add_point_to_array(offset_points, current_point)
115end for
116
117
118current_point = points[length(points) - 1]
119prev_point = points[length(points) - 2]
120offset_vector = calculate_offset_vector(current_point, null, prev_point, radius, 'right')
121add_vector_to_point(current_point, offset_vector)
122add_point_to_array(offset_points, current_point)
123
124return offset_points
125
126end function
127
128
129function calculate_offset_vector(current, next, prev, radius, comp_type)
130
131
132
133
134declare direction_vector vector
135declare offset_vector vector
136
137if next == null then
138direction_vector = subtract_points(current, prev)
139elseif prev == null then
140direction_vector = subtract_points(next, current)
141else
142
143declare incoming_vector vector
144declare outgoing_vector vector
145incoming_vector = subtract_points(current, prev)
146outgoing_vector = subtract_points(next, current)
147normalize_vector(incoming_vector)
148normalize_vector(outgoing_vector)
149
150direction_vector = outgoing_vector
151end if
152
153normalize_vector(direction_vector)
154
155
156declare perpendicular_vector vector
157perpendicular_vector.x = -direction_vector.y
158perpendicular_vector.y = direction_vector.x
159perpendicular_vector.z = 0
160
161offset_vector = scale_vector(perpendicular_vector, radius)
162
163if comp_type == 'right' then
164offset_vector = negate_vector(offset_vector)
165end if
166
167return offset_vector
168
169end function
170
171
172function subtract_points(p1, p2)
173return {x: p1.x - p2.x, y: p1.y - p2.y, z: p1.z - p2.z}
174end function
175
176function normalize_vector(v)
177
178end function
179
180function scale_vector(v, scalar)
181return {x: v.x * scalar, y: v.y * scalar, z: v.z * scalar}
182end function
183
184function add_vector_to_point(p, v)
185p.x = p.x + v.x
186p.y = p.y + v.y
187p.z = p.z + v.z
188end function
189
190function add_point_to_array(arr, p)
191
192end function
193
194function negate_vector(v)
195return {x: -v.x, y: -v.y, z: -v.z}
196end function