| User | Revision | Line number | New contents of line |
|---|
| Sergunb |
0:8f0d870509fe
|
1
|
/*
|
| Sergunb |
0:8f0d870509fe
|
2
|
motion_control.c - high level interface for issuing motion commands
|
| Sergunb |
0:8f0d870509fe
|
3
|
Part of Grbl
|
| Sergunb |
0:8f0d870509fe
|
4
|
|
| Sergunb |
0:8f0d870509fe
|
5
|
Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
|
| Sergunb |
0:8f0d870509fe
|
6
|
Copyright (c) 2009-2011 Simen Svale Skogsrud
|
| Sergunb |
0:8f0d870509fe
|
7
|
|
| Sergunb |
0:8f0d870509fe
|
8
|
Grbl is free software: you can redistribute it and/or modify
|
| Sergunb |
0:8f0d870509fe
|
9
|
it under the terms of the GNU General Public License as published by
|
| Sergunb |
0:8f0d870509fe
|
10
|
the Free Software Foundation, either version 3 of the License, or
|
| Sergunb |
0:8f0d870509fe
|
11
|
(at your option) any later version.
|
| Sergunb |
0:8f0d870509fe
|
12
|
|
| Sergunb |
0:8f0d870509fe
|
13
|
Grbl is distributed in the hope that it will be useful,
|
| Sergunb |
0:8f0d870509fe
|
14
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
| Sergunb |
0:8f0d870509fe
|
15
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
| Sergunb |
0:8f0d870509fe
|
16
|
GNU General Public License for more details.
|
| Sergunb |
0:8f0d870509fe
|
17
|
|
| Sergunb |
0:8f0d870509fe
|
18
|
You should have received a copy of the GNU General Public License
|
| Sergunb |
0:8f0d870509fe
|
19
|
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
|
| Sergunb |
0:8f0d870509fe
|
20
|
*/
|
| Sergunb |
0:8f0d870509fe
|
21
|
|
| Sergunb |
0:8f0d870509fe
|
22
|
#include "grbl.h"
|
| Sergunb |
0:8f0d870509fe
|
23
|
|
| Sergunb |
0:8f0d870509fe
|
24
|
|
| Sergunb |
0:8f0d870509fe
|
25
|
// Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
|
| Sergunb |
0:8f0d870509fe
|
26
|
// unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in
|
| Sergunb |
0:8f0d870509fe
|
27
|
// (1 minute)/feed_rate time.
|
| Sergunb |
0:8f0d870509fe
|
28
|
// NOTE: This is the primary gateway to the grbl planner. All line motions, including arc line
|
| Sergunb |
0:8f0d870509fe
|
29
|
// segments, must pass through this routine before being passed to the planner. The seperation of
|
| Sergunb |
0:8f0d870509fe
|
30
|
// mc_line and plan_buffer_line is done primarily to place non-planner-type functions from being
|
| Sergunb |
0:8f0d870509fe
|
31
|
// in the planner and to let backlash compensation or canned cycle integration simple and direct.
|
| Sergunb |
0:8f0d870509fe
|
32
|
void mc_line(float *target, plan_line_data_t *pl_data)
|
| Sergunb |
0:8f0d870509fe
|
33
|
{
|
| Sergunb |
0:8f0d870509fe
|
34
|
// If enabled, check for soft limit violations. Placed here all line motions are picked up
|
| Sergunb |
0:8f0d870509fe
|
35
|
// from everywhere in Grbl.
|
| Sergunb |
0:8f0d870509fe
|
36
|
if (bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)) {
|
| Sergunb |
0:8f0d870509fe
|
37
|
// NOTE: Block jog state. Jogging is a special case and soft limits are handled independently.
|
| Sergunb |
0:8f0d870509fe
|
38
|
if (sys.state != STATE_JOG) { limits_soft_check(target); }
|
| Sergunb |
0:8f0d870509fe
|
39
|
}
|
| Sergunb |
0:8f0d870509fe
|
40
|
|
| Sergunb |
0:8f0d870509fe
|
41
|
// If in check gcode mode, prevent motion by blocking planner. Soft limits still work.
|
| Sergunb |
0:8f0d870509fe
|
42
|
if (sys.state == STATE_CHECK_MODE) { return; }
|
| Sergunb |
0:8f0d870509fe
|
43
|
|
| Sergunb |
0:8f0d870509fe
|
44
|
// NOTE: Backlash compensation may be installed here. It will need direction info to track when
|
| Sergunb |
0:8f0d870509fe
|
45
|
// to insert a backlash line motion(s) before the intended line motion and will require its own
|
| Sergunb |
0:8f0d870509fe
|
46
|
// plan_check_full_buffer() and check for system abort loop. Also for position reporting
|
| Sergunb |
0:8f0d870509fe
|
47
|
// backlash steps will need to be also tracked, which will need to be kept at a system level.
|
| Sergunb |
0:8f0d870509fe
|
48
|
// There are likely some other things that will need to be tracked as well. However, we feel
|
| Sergunb |
0:8f0d870509fe
|
49
|
// that backlash compensation should NOT be handled by Grbl itself, because there are a myriad
|
| Sergunb |
0:8f0d870509fe
|
50
|
// of ways to implement it and can be effective or ineffective for different CNC machines. This
|
| Sergunb |
0:8f0d870509fe
|
51
|
// would be better handled by the interface as a post-processor task, where the original g-code
|
| Sergunb |
0:8f0d870509fe
|
52
|
// is translated and inserts backlash motions that best suits the machine.
|
| Sergunb |
0:8f0d870509fe
|
53
|
// NOTE: Perhaps as a middle-ground, all that needs to be sent is a flag or special command that
|
| Sergunb |
0:8f0d870509fe
|
54
|
// indicates to Grbl what is a backlash compensation motion, so that Grbl executes the move but
|
| Sergunb |
0:8f0d870509fe
|
55
|
// doesn't update the machine position values. Since the position values used by the g-code
|
| Sergunb |
0:8f0d870509fe
|
56
|
// parser and planner are separate from the system machine positions, this is doable.
|
| Sergunb |
0:8f0d870509fe
|
57
|
|
| Sergunb |
0:8f0d870509fe
|
58
|
// If the buffer is full: good! That means we are well ahead of the robot.
|
| Sergunb |
0:8f0d870509fe
|
59
|
// Remain in this loop until there is room in the buffer.
|
| Sergunb |
0:8f0d870509fe
|
60
|
do {
|
| Sergunb |
0:8f0d870509fe
|
61
|
protocol_execute_realtime(); // Check for any run-time commands
|
| Sergunb |
0:8f0d870509fe
|
62
|
if (sys.abort) { return; } // Bail, if system abort.
|
| Sergunb |
0:8f0d870509fe
|
63
|
if ( plan_check_full_buffer() ) { protocol_auto_cycle_start(); } // Auto-cycle start when buffer is full.
|
| Sergunb |
0:8f0d870509fe
|
64
|
else { break; }
|
| Sergunb |
0:8f0d870509fe
|
65
|
} while (1);
|
| Sergunb |
0:8f0d870509fe
|
66
|
|
| Sergunb |
0:8f0d870509fe
|
67
|
// Plan and queue motion into planner buffer
|
| Sergunb |
0:8f0d870509fe
|
68
|
if (plan_buffer_line(target, pl_data) == PLAN_EMPTY_BLOCK) {
|
| Sergunb |
0:8f0d870509fe
|
69
|
if (bit_istrue(settings.flags, BITFLAG_LASER_MODE)) {
|
| Sergunb |
0:8f0d870509fe
|
70
|
// Correctly set spindle state, if there is a coincident position passed. Forces a buffer
|
| Sergunb |
0:8f0d870509fe
|
71
|
// sync while in M3 laser mode only.
|
| Sergunb |
0:8f0d870509fe
|
72
|
if (pl_data->condition & PL_COND_FLAG_SPINDLE_CW) {
|
| Sergunb |
0:8f0d870509fe
|
73
|
spindle_sync(PL_COND_FLAG_SPINDLE_CW, pl_data->spindle_speed);
|
| Sergunb |
0:8f0d870509fe
|
74
|
}
|
| Sergunb |
0:8f0d870509fe
|
75
|
}
|
| Sergunb |
0:8f0d870509fe
|
76
|
}
|
| Sergunb |
0:8f0d870509fe
|
77
|
}
|
| Sergunb |
0:8f0d870509fe
|
78
|
|
| Sergunb |
0:8f0d870509fe
|
79
|
|
| Sergunb |
0:8f0d870509fe
|
80
|
// Execute an arc in offset mode format. position == current xyz, target == target xyz,
|
| Sergunb |
0:8f0d870509fe
|
81
|
// offset == offset from current xyz, axis_X defines circle plane in tool space, axis_linear is
|
| Sergunb |
0:8f0d870509fe
|
82
|
// the direction of helical travel, radius == circle radius, isclockwise boolean. Used
|
| Sergunb |
0:8f0d870509fe
|
83
|
// for vector transformation direction.
|
| Sergunb |
0:8f0d870509fe
|
84
|
// The arc is approximated by generating a huge number of tiny, linear segments. The chordal tolerance
|
| Sergunb |
0:8f0d870509fe
|
85
|
// of each segment is configured in settings.arc_tolerance, which is defined to be the maximum normal
|
| Sergunb |
0:8f0d870509fe
|
86
|
// distance from segment to the circle when the end points both lie on the circle.
|
| Sergunb |
0:8f0d870509fe
|
87
|
void mc_arc(float *target, plan_line_data_t *pl_data, float *position, float *offset, float radius,
|
| Sergunb |
0:8f0d870509fe
|
88
|
uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear, uint8_t is_clockwise_arc)
|
| Sergunb |
0:8f0d870509fe
|
89
|
{
|
| Sergunb |
0:8f0d870509fe
|
90
|
float center_axis0 = position[axis_0] + offset[axis_0];
|
| Sergunb |
0:8f0d870509fe
|
91
|
float center_axis1 = position[axis_1] + offset[axis_1];
|
| Sergunb |
0:8f0d870509fe
|
92
|
float r_axis0 = -offset[axis_0]; // Radius vector from center to current location
|
| Sergunb |
0:8f0d870509fe
|
93
|
float r_axis1 = -offset[axis_1];
|
| Sergunb |
0:8f0d870509fe
|
94
|
float rt_axis0 = target[axis_0] - center_axis0;
|
| Sergunb |
0:8f0d870509fe
|
95
|
float rt_axis1 = target[axis_1] - center_axis1;
|
| Sergunb |
0:8f0d870509fe
|
96
|
|
| Sergunb |
0:8f0d870509fe
|
97
|
// CCW angle between position and target from circle center. Only one atan2() trig computation required.
|
| Sergunb |
0:8f0d870509fe
|
98
|
float angular_travel = atan2f(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
|
| Sergunb |
0:8f0d870509fe
|
99
|
if (is_clockwise_arc) { // Correct atan2 output per direction
|
| Sergunb |
0:8f0d870509fe
|
100
|
if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel -= 2*M_PI; }
|
| Sergunb |
0:8f0d870509fe
|
101
|
} else {
|
| Sergunb |
0:8f0d870509fe
|
102
|
if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel += 2*M_PI; }
|
| Sergunb |
0:8f0d870509fe
|
103
|
}
|
| Sergunb |
0:8f0d870509fe
|
104
|
|
| Sergunb |
0:8f0d870509fe
|
105
|
// NOTE: Segment end points are on the arc, which can lead to the arc diameter being smaller by up to
|
| Sergunb |
0:8f0d870509fe
|
106
|
// (2x) settings.arc_tolerance. For 99% of users, this is just fine. If a different arc segment fit
|
| Sergunb |
0:8f0d870509fe
|
107
|
// is desired, i.e. least-squares, midpoint on arc, just change the mm_per_arc_segment calculation.
|
| Sergunb |
0:8f0d870509fe
|
108
|
// For the intended uses of Grbl, this value shouldn't exceed 2000 for the strictest of cases.
|
| Sergunb |
0:8f0d870509fe
|
109
|
uint16_t segments = (uint16_t)floorf(fabsf(0.5f*angular_travel*radius) /
|
| Sergunb |
0:8f0d870509fe
|
110
|
sqrtf(settings.arc_tolerance*(2*radius - settings.arc_tolerance)) );
|
| Sergunb |
0:8f0d870509fe
|
111
|
|
| Sergunb |
0:8f0d870509fe
|
112
|
if (segments) {
|
| Sergunb |
0:8f0d870509fe
|
113
|
// Multiply inverse feed_rate to compensate for the fact that this movement is approximated
|
| Sergunb |
0:8f0d870509fe
|
114
|
// by a number of discrete segments. The inverse feed_rate should be correct for the sum of
|
| Sergunb |
0:8f0d870509fe
|
115
|
// all segments.
|
| Sergunb |
0:8f0d870509fe
|
116
|
if (pl_data->condition & PL_COND_FLAG_INVERSE_TIME) {
|
| Sergunb |
0:8f0d870509fe
|
117
|
pl_data->feed_rate *= segments;
|
| Sergunb |
0:8f0d870509fe
|
118
|
bit_false(pl_data->condition,PL_COND_FLAG_INVERSE_TIME); // Force as feed absolute mode over arc segments.
|
| Sergunb |
0:8f0d870509fe
|
119
|
}
|
| Sergunb |
0:8f0d870509fe
|
120
|
|
| Sergunb |
0:8f0d870509fe
|
121
|
float theta_per_segment = angular_travel/segments;
|
| Sergunb |
0:8f0d870509fe
|
122
|
float linear_per_segment = (target[axis_linear] - position[axis_linear])/segments;
|
| Sergunb |
0:8f0d870509fe
|
123
|
|
| Sergunb |
0:8f0d870509fe
|
124
|
/* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
|
| Sergunb |
0:8f0d870509fe
|
125
|
and phi is the angle of rotation. Solution approach by Jens Geisler.
|
| Sergunb |
0:8f0d870509fe
|
126
|
r_T = [cos(phi) -sin(phi);
|
| Sergunb |
0:8f0d870509fe
|
127
|
sin(phi) cos(phi] * r ;
|
| Sergunb |
0:8f0d870509fe
|
128
|
|
| Sergunb |
0:8f0d870509fe
|
129
|
For arc generation, the center of the circle is the axis of rotation and the radius vector is
|
| Sergunb |
0:8f0d870509fe
|
130
|
defined from the circle center to the initial position. Each line segment is formed by successive
|
| Sergunb |
0:8f0d870509fe
|
131
|
vector rotations. Single precision values can accumulate error greater than tool precision in rare
|
| Sergunb |
0:8f0d870509fe
|
132
|
cases. So, exact arc path correction is implemented. This approach avoids the problem of too many very
|
| Sergunb |
0:8f0d870509fe
|
133
|
expensive trig operations [sin(),cos(),tan()] which can take 100-200 usec each to compute.
|
| Sergunb |
0:8f0d870509fe
|
134
|
|
| Sergunb |
0:8f0d870509fe
|
135
|
Small angle approximation may be used to reduce computation overhead further. A third-order approximation
|
| Sergunb |
0:8f0d870509fe
|
136
|
(second order sin() has too much error) holds for most, if not, all CNC applications. Note that this
|
| Sergunb |
0:8f0d870509fe
|
137
|
approximation will begin to accumulate a numerical drift error when theta_per_segment is greater than
|
| Sergunb |
0:8f0d870509fe
|
138
|
~0.25 rad(14 deg) AND the approximation is successively used without correction several dozen times. This
|
| Sergunb |
0:8f0d870509fe
|
139
|
scenario is extremely unlikely, since segment lengths and theta_per_segment are automatically generated
|
| Sergunb |
0:8f0d870509fe
|
140
|
and scaled by the arc tolerance setting. Only a very large arc tolerance setting, unrealistic for CNC
|
| Sergunb |
0:8f0d870509fe
|
141
|
applications, would cause this numerical drift error. However, it is best to set N_ARC_CORRECTION from a
|
| Sergunb |
0:8f0d870509fe
|
142
|
low of ~4 to a high of ~20 or so to avoid trig operations while keeping arc generation accurate.
|
| Sergunb |
0:8f0d870509fe
|
143
|
|
| Sergunb |
0:8f0d870509fe
|
144
|
This approximation also allows mc_arc to immediately insert a line segment into the planner
|
| Sergunb |
0:8f0d870509fe
|
145
|
without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
|
| Sergunb |
0:8f0d870509fe
|
146
|
a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
|
| Sergunb |
0:8f0d870509fe
|
147
|
This is important when there are successive arc motions.
|
| Sergunb |
0:8f0d870509fe
|
148
|
*/
|
| Sergunb |
0:8f0d870509fe
|
149
|
// Computes: cos_T = 1 - theta_per_segment^2/2, sin_T = theta_per_segment - theta_per_segment^3/6) in ~52usec
|
| Sergunb |
0:8f0d870509fe
|
150
|
float cos_T = 2.0f - theta_per_segment*theta_per_segment;
|
| Sergunb |
0:8f0d870509fe
|
151
|
float sin_T = theta_per_segment*0.16666667f*(cos_T + 4.0f);
|
| Sergunb |
0:8f0d870509fe
|
152
|
cos_T *= 0.5;
|
| Sergunb |
0:8f0d870509fe
|
153
|
|
| Sergunb |
0:8f0d870509fe
|
154
|
float sin_Ti;
|
| Sergunb |
0:8f0d870509fe
|
155
|
float cos_Ti;
|
| Sergunb |
0:8f0d870509fe
|
156
|
float r_axisi;
|
| Sergunb |
0:8f0d870509fe
|
157
|
uint16_t i;
|
| Sergunb |
0:8f0d870509fe
|
158
|
uint8_t count = 0;
|
| Sergunb |
0:8f0d870509fe
|
159
|
|
| Sergunb |
0:8f0d870509fe
|
160
|
for (i = 1; i<segments; i++) { // Increment (segments-1).
|
| Sergunb |
0:8f0d870509fe
|
161
|
|
| Sergunb |
0:8f0d870509fe
|
162
|
if (count < N_ARC_CORRECTION) {
|
| Sergunb |
0:8f0d870509fe
|
163
|
// Apply vector rotation matrix. ~40 usec
|
| Sergunb |
0:8f0d870509fe
|
164
|
r_axisi = r_axis0*sin_T + r_axis1*cos_T;
|
| Sergunb |
0:8f0d870509fe
|
165
|
r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
|
| Sergunb |
0:8f0d870509fe
|
166
|
r_axis1 = r_axisi;
|
| Sergunb |
0:8f0d870509fe
|
167
|
count++;
|
| Sergunb |
0:8f0d870509fe
|
168
|
} else {
|
| Sergunb |
0:8f0d870509fe
|
169
|
// Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments. ~375 usec
|
| Sergunb |
0:8f0d870509fe
|
170
|
// Compute exact location by applying transformation matrix from initial radius vector(=-offset).
|
| Sergunb |
0:8f0d870509fe
|
171
|
cos_Ti = cosf(i*theta_per_segment);
|
| Sergunb |
0:8f0d870509fe
|
172
|
sin_Ti = sinf(i*theta_per_segment);
|
| Sergunb |
0:8f0d870509fe
|
173
|
r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
|
| Sergunb |
0:8f0d870509fe
|
174
|
r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
|
| Sergunb |
0:8f0d870509fe
|
175
|
count = 0;
|
| Sergunb |
0:8f0d870509fe
|
176
|
}
|
| Sergunb |
0:8f0d870509fe
|
177
|
|
| Sergunb |
0:8f0d870509fe
|
178
|
// Update arc_target location
|
| Sergunb |
0:8f0d870509fe
|
179
|
position[axis_0] = center_axis0 + r_axis0;
|
| Sergunb |
0:8f0d870509fe
|
180
|
position[axis_1] = center_axis1 + r_axis1;
|
| Sergunb |
0:8f0d870509fe
|
181
|
position[axis_linear] += linear_per_segment;
|
| Sergunb |
0:8f0d870509fe
|
182
|
|
| Sergunb |
0:8f0d870509fe
|
183
|
mc_line(position, pl_data);
|
| Sergunb |
0:8f0d870509fe
|
184
|
|
| Sergunb |
0:8f0d870509fe
|
185
|
// Bail mid-circle on system abort. Runtime command check already performed by mc_line.
|
| Sergunb |
0:8f0d870509fe
|
186
|
if (sys.abort) { return; }
|
| Sergunb |
0:8f0d870509fe
|
187
|
}
|
| Sergunb |
0:8f0d870509fe
|
188
|
}
|
| Sergunb |
0:8f0d870509fe
|
189
|
// Ensure last segment arrives at target location.
|
| Sergunb |
0:8f0d870509fe
|
190
|
mc_line(target, pl_data);
|
| Sergunb |
0:8f0d870509fe
|
191
|
}
|
| Sergunb |
0:8f0d870509fe
|
192
|
|
| Sergunb |
0:8f0d870509fe
|
193
|
|
| Sergunb |
0:8f0d870509fe
|
194
|
// Execute dwell in seconds.
|
| Sergunb |
0:8f0d870509fe
|
195
|
void mc_dwell(float seconds)
|
| Sergunb |
0:8f0d870509fe
|
196
|
{
|
| Sergunb |
0:8f0d870509fe
|
197
|
if (sys.state == STATE_CHECK_MODE) { return; }
|
| Sergunb |
0:8f0d870509fe
|
198
|
protocol_buffer_synchronize();
|
| Sergunb |
0:8f0d870509fe
|
199
|
delay_sec(seconds, DELAY_MODE_DWELL);
|
| Sergunb |
0:8f0d870509fe
|
200
|
}
|
| Sergunb |
0:8f0d870509fe
|
201
|
|
| Sergunb |
0:8f0d870509fe
|
202
|
|
| Sergunb |
0:8f0d870509fe
|
203
|
// Perform homing cycle to locate and set machine zero. Only '$H' executes this command.
|
| Sergunb |
0:8f0d870509fe
|
204
|
// NOTE: There should be no motions in the buffer and Grbl must be in an idle state before
|
| Sergunb |
0:8f0d870509fe
|
205
|
// executing the homing cycle. This prevents incorrect buffered plans after homing.
|
| Sergunb |
0:8f0d870509fe
|
206
|
void mc_homing_cycle(uint8_t cycle_mask)
|
| Sergunb |
0:8f0d870509fe
|
207
|
{
|
| Sergunb |
0:8f0d870509fe
|
208
|
// Check and abort homing cycle, if hard limits are already enabled. Helps prevent problems
|
| Sergunb |
0:8f0d870509fe
|
209
|
// with machines with limits wired on both ends of travel to one limit pin.
|
| Sergunb |
0:8f0d870509fe
|
210
|
// TODO: Move the pin-specific LIMIT_PIN call to limits.c as a function.
|
| Sergunb |
0:8f0d870509fe
|
211
|
#ifdef LIMITS_TWO_SWITCHES_ON_AXES
|
| Sergunb |
0:8f0d870509fe
|
212
|
if (limits_get_state()) {
|
| Sergunb |
0:8f0d870509fe
|
213
|
mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown.
|
| Sergunb |
0:8f0d870509fe
|
214
|
system_set_exec_alarm(EXEC_ALARM_HARD_LIMIT);
|
| Sergunb |
0:8f0d870509fe
|
215
|
return;
|
| Sergunb |
0:8f0d870509fe
|
216
|
}
|
| Sergunb |
0:8f0d870509fe
|
217
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
218
|
|
| Sergunb |
0:8f0d870509fe
|
219
|
limits_disable(); // Disable hard limits pin change register for cycle duration
|
| Sergunb |
0:8f0d870509fe
|
220
|
|
| Sergunb |
0:8f0d870509fe
|
221
|
// -------------------------------------------------------------------------------------
|
| Sergunb |
0:8f0d870509fe
|
222
|
// Perform homing routine. NOTE: Special motion case. Only system reset works.
|
| Sergunb |
0:8f0d870509fe
|
223
|
|
| Sergunb |
0:8f0d870509fe
|
224
|
#ifdef HOMING_SINGLE_AXIS_COMMANDS
|
| Sergunb |
0:8f0d870509fe
|
225
|
if (cycle_mask) { limits_go_home(cycle_mask); } // Perform homing cycle based on mask.
|
| Sergunb |
0:8f0d870509fe
|
226
|
else
|
| Sergunb |
0:8f0d870509fe
|
227
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
228
|
{
|
| Sergunb |
0:8f0d870509fe
|
229
|
// Search to engage all axes limit switches at faster homing seek rate.
|
| Sergunb |
0:8f0d870509fe
|
230
|
limits_go_home(HOMING_CYCLE_0); // Homing cycle 0
|
| Sergunb |
0:8f0d870509fe
|
231
|
#ifdef HOMING_CYCLE_1
|
| Sergunb |
0:8f0d870509fe
|
232
|
limits_go_home(HOMING_CYCLE_1); // Homing cycle 1
|
| Sergunb |
0:8f0d870509fe
|
233
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
234
|
#ifdef HOMING_CYCLE_2
|
| Sergunb |
0:8f0d870509fe
|
235
|
limits_go_home(HOMING_CYCLE_2); // Homing cycle 2
|
| Sergunb |
0:8f0d870509fe
|
236
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
237
|
}
|
| Sergunb |
0:8f0d870509fe
|
238
|
|
| Sergunb |
0:8f0d870509fe
|
239
|
protocol_execute_realtime(); // Check for reset and set system abort.
|
| Sergunb |
0:8f0d870509fe
|
240
|
if (sys.abort) { return; } // Did not complete. Alarm state set by mc_alarm.
|
| Sergunb |
0:8f0d870509fe
|
241
|
|
| Sergunb |
0:8f0d870509fe
|
242
|
// Homing cycle complete! Setup system for normal operation.
|
| Sergunb |
0:8f0d870509fe
|
243
|
// -------------------------------------------------------------------------------------
|
| Sergunb |
0:8f0d870509fe
|
244
|
|
| Sergunb |
0:8f0d870509fe
|
245
|
// Sync gcode parser and planner positions to homed position.
|
| Sergunb |
0:8f0d870509fe
|
246
|
gc_sync_position();
|
| Sergunb |
0:8f0d870509fe
|
247
|
plan_sync_position();
|
| Sergunb |
0:8f0d870509fe
|
248
|
|
| Sergunb |
0:8f0d870509fe
|
249
|
// If hard limits feature enabled, re-enable hard limits pin change register after homing cycle.
|
| Sergunb |
0:8f0d870509fe
|
250
|
#ifdef STM32F103C8
|
| Sergunb |
0:8f0d870509fe
|
251
|
EXTI_ClearITPendingBit((1 << X_LIMIT_BIT) | (1 << Y_LIMIT_BIT) | (1 << Z_LIMIT_BIT));
|
| Sergunb |
0:8f0d870509fe
|
252
|
NVIC_ClearPendingIRQ(EXTI15_10_IRQn);
|
| Sergunb |
0:8f0d870509fe
|
253
|
NVIC_EnableIRQ(EXTI15_10_IRQn);
|
| Sergunb |
0:8f0d870509fe
|
254
|
#else
|
| Sergunb |
0:8f0d870509fe
|
255
|
limits_init();
|
| Sergunb |
0:8f0d870509fe
|
256
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
257
|
}
|
| Sergunb |
0:8f0d870509fe
|
258
|
|
| Sergunb |
0:8f0d870509fe
|
259
|
|
| Sergunb |
0:8f0d870509fe
|
260
|
// Perform tool length probe cycle. Requires probe switch.
|
| Sergunb |
0:8f0d870509fe
|
261
|
// NOTE: Upon probe failure, the program will be stopped and placed into ALARM state.
|
| Sergunb |
0:8f0d870509fe
|
262
|
uint8_t mc_probe_cycle(float *target, plan_line_data_t *pl_data, uint8_t parser_flags)
|
| Sergunb |
0:8f0d870509fe
|
263
|
{
|
| Sergunb |
0:8f0d870509fe
|
264
|
// TODO: Need to update this cycle so it obeys a non-auto cycle start.
|
| Sergunb |
0:8f0d870509fe
|
265
|
if (sys.state == STATE_CHECK_MODE) { return(GC_PROBE_CHECK_MODE); }
|
| Sergunb |
0:8f0d870509fe
|
266
|
|
| Sergunb |
0:8f0d870509fe
|
267
|
// Finish all queued commands and empty planner buffer before starting probe cycle.
|
| Sergunb |
0:8f0d870509fe
|
268
|
protocol_buffer_synchronize();
|
| Sergunb |
0:8f0d870509fe
|
269
|
if (sys.abort) { return(GC_PROBE_ABORT); } // Return if system reset has been issued.
|
| Sergunb |
0:8f0d870509fe
|
270
|
|
| Sergunb |
0:8f0d870509fe
|
271
|
// Initialize probing control variables
|
| Sergunb |
0:8f0d870509fe
|
272
|
uint8_t is_probe_away = bit_istrue(parser_flags, GC_PARSER_PROBE_IS_AWAY);
|
| Sergunb |
0:8f0d870509fe
|
273
|
uint8_t is_no_error = bit_istrue(parser_flags, GC_PARSER_PROBE_IS_NO_ERROR);
|
| Sergunb |
0:8f0d870509fe
|
274
|
sys.probe_succeeded = false; // Re-initialize probe history before beginning cycle.
|
| Sergunb |
0:8f0d870509fe
|
275
|
probe_configure_invert_mask(is_probe_away);
|
| Sergunb |
0:8f0d870509fe
|
276
|
|
| Sergunb |
0:8f0d870509fe
|
277
|
// After syncing, check if probe is already triggered. If so, halt and issue alarm.
|
| Sergunb |
0:8f0d870509fe
|
278
|
// NOTE: This probe initialization error applies to all probing cycles.
|
| Sergunb |
0:8f0d870509fe
|
279
|
if ( probe_get_state() ) { // Check probe pin state.
|
| Sergunb |
0:8f0d870509fe
|
280
|
system_set_exec_alarm(EXEC_ALARM_PROBE_FAIL_INITIAL);
|
| Sergunb |
0:8f0d870509fe
|
281
|
protocol_execute_realtime();
|
| Sergunb |
0:8f0d870509fe
|
282
|
probe_configure_invert_mask(false); // Re-initialize invert mask before returning.
|
| Sergunb |
0:8f0d870509fe
|
283
|
return(GC_PROBE_FAIL_INIT); // Nothing else to do but bail.
|
| Sergunb |
0:8f0d870509fe
|
284
|
}
|
| Sergunb |
0:8f0d870509fe
|
285
|
|
| Sergunb |
0:8f0d870509fe
|
286
|
// Setup and queue probing motion. Auto cycle-start should not start the cycle.
|
| Sergunb |
0:8f0d870509fe
|
287
|
mc_line(target, pl_data);
|
| Sergunb |
0:8f0d870509fe
|
288
|
|
| Sergunb |
0:8f0d870509fe
|
289
|
// Activate the probing state monitor in the stepper module.
|
| Sergunb |
0:8f0d870509fe
|
290
|
sys_probe_state = PROBE_ACTIVE;
|
| Sergunb |
0:8f0d870509fe
|
291
|
|
| Sergunb |
0:8f0d870509fe
|
292
|
// Perform probing cycle. Wait here until probe is triggered or motion completes.
|
| Sergunb |
0:8f0d870509fe
|
293
|
system_set_exec_state_flag(EXEC_CYCLE_START);
|
| Sergunb |
0:8f0d870509fe
|
294
|
do {
|
| Sergunb |
0:8f0d870509fe
|
295
|
protocol_execute_realtime();
|
| Sergunb |
0:8f0d870509fe
|
296
|
if (sys.abort) { return(GC_PROBE_ABORT); } // Check for system abort
|
| Sergunb |
0:8f0d870509fe
|
297
|
} while (sys.state != STATE_IDLE);
|
| Sergunb |
0:8f0d870509fe
|
298
|
|
| Sergunb |
0:8f0d870509fe
|
299
|
// Probing cycle complete!
|
| Sergunb |
0:8f0d870509fe
|
300
|
|
| Sergunb |
0:8f0d870509fe
|
301
|
// Set state variables and error out, if the probe failed and cycle with error is enabled.
|
| Sergunb |
0:8f0d870509fe
|
302
|
if (sys_probe_state == PROBE_ACTIVE) {
|
| Sergunb |
0:8f0d870509fe
|
303
|
if (is_no_error) { memcpy(sys_probe_position, sys_position, sizeof(sys_position)); }
|
| Sergunb |
0:8f0d870509fe
|
304
|
else { system_set_exec_alarm(EXEC_ALARM_PROBE_FAIL_CONTACT); }
|
| Sergunb |
0:8f0d870509fe
|
305
|
} else {
|
| Sergunb |
0:8f0d870509fe
|
306
|
sys.probe_succeeded = true; // Indicate to system the probing cycle completed successfully.
|
| Sergunb |
0:8f0d870509fe
|
307
|
}
|
| Sergunb |
0:8f0d870509fe
|
308
|
sys_probe_state = PROBE_OFF; // Ensure probe state monitor is disabled.
|
| Sergunb |
0:8f0d870509fe
|
309
|
probe_configure_invert_mask(false); // Re-initialize invert mask.
|
| Sergunb |
0:8f0d870509fe
|
310
|
protocol_execute_realtime(); // Check and execute run-time commands
|
| Sergunb |
0:8f0d870509fe
|
311
|
|
| Sergunb |
0:8f0d870509fe
|
312
|
// Reset the stepper and planner buffers to remove the remainder of the probe motion.
|
| Sergunb |
0:8f0d870509fe
|
313
|
st_reset(); // Reset step segment buffer.
|
| Sergunb |
0:8f0d870509fe
|
314
|
plan_reset(); // Reset planner buffer. Zero planner positions. Ensure probing motion is cleared.
|
| Sergunb |
0:8f0d870509fe
|
315
|
plan_sync_position(); // Sync planner position to current machine position.
|
| Sergunb |
0:8f0d870509fe
|
316
|
|
| Sergunb |
0:8f0d870509fe
|
317
|
#ifdef MESSAGE_PROBE_COORDINATES
|
| Sergunb |
0:8f0d870509fe
|
318
|
// All done! Output the probe position as message.
|
| Sergunb |
0:8f0d870509fe
|
319
|
report_probe_parameters();
|
| Sergunb |
0:8f0d870509fe
|
320
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
321
|
|
| Sergunb |
0:8f0d870509fe
|
322
|
if (sys.probe_succeeded) { return(GC_PROBE_FOUND); } // Successful probe cycle.
|
| Sergunb |
0:8f0d870509fe
|
323
|
else { return(GC_PROBE_FAIL_END); } // Failed to trigger probe within travel. With or without error.
|
| Sergunb |
0:8f0d870509fe
|
324
|
}
|
| Sergunb |
0:8f0d870509fe
|
325
|
|
| Sergunb |
0:8f0d870509fe
|
326
|
#ifdef PARKING_ENABLE
|
| Sergunb |
0:8f0d870509fe
|
327
|
void mc_parking_motion(float *parking_target, plan_line_data_t *pl_data)
|
| Sergunb |
0:8f0d870509fe
|
328
|
{
|
| Sergunb |
0:8f0d870509fe
|
329
|
if (sys.abort) { return; } // Block during abort.
|
| Sergunb |
0:8f0d870509fe
|
330
|
|
| Sergunb |
0:8f0d870509fe
|
331
|
uint8_t plan_status = plan_buffer_line(parking_target, pl_data);
|
| Sergunb |
0:8f0d870509fe
|
332
|
|
| Sergunb |
0:8f0d870509fe
|
333
|
if (plan_status) {
|
| Sergunb |
0:8f0d870509fe
|
334
|
bit_true(sys.step_control, STEP_CONTROL_EXECUTE_SYS_MOTION);
|
| Sergunb |
0:8f0d870509fe
|
335
|
bit_false(sys.step_control, STEP_CONTROL_END_MOTION); // Allow parking motion to execute, if feed hold is active.
|
| Sergunb |
0:8f0d870509fe
|
336
|
st_parking_setup_buffer(); // Setup step segment buffer for special parking motion case
|
| Sergunb |
0:8f0d870509fe
|
337
|
st_prep_buffer();
|
| Sergunb |
0:8f0d870509fe
|
338
|
st_wake_up();
|
| Sergunb |
0:8f0d870509fe
|
339
|
do {
|
| Sergunb |
0:8f0d870509fe
|
340
|
protocol_exec_rt_system();
|
| Sergunb |
0:8f0d870509fe
|
341
|
if (sys.abort) { return; }
|
| Sergunb |
0:8f0d870509fe
|
342
|
} while (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION);
|
| Sergunb |
0:8f0d870509fe
|
343
|
st_parking_restore_buffer(); // Restore step segment buffer to normal run state.
|
| Sergunb |
0:8f0d870509fe
|
344
|
}
|
| Sergunb |
0:8f0d870509fe
|
345
|
else {
|
| Sergunb |
0:8f0d870509fe
|
346
|
bit_false(sys.step_control, STEP_CONTROL_EXECUTE_SYS_MOTION);
|
| Sergunb |
0:8f0d870509fe
|
347
|
protocol_exec_rt_system();
|
| Sergunb |
0:8f0d870509fe
|
348
|
}
|
| Sergunb |
0:8f0d870509fe
|
349
|
|
| Sergunb |
0:8f0d870509fe
|
350
|
}
|
| Sergunb |
0:8f0d870509fe
|
351
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
352
|
|
| Sergunb |
0:8f0d870509fe
|
353
|
|
| Sergunb |
0:8f0d870509fe
|
354
|
#ifdef ENABLE_PARKING_OVERRIDE_CONTROL
|
| Sergunb |
0:8f0d870509fe
|
355
|
void mc_override_ctrl_update(uint8_t override_state)
|
| Sergunb |
0:8f0d870509fe
|
356
|
{
|
| Sergunb |
0:8f0d870509fe
|
357
|
// Finish all queued commands before altering override control state
|
| Sergunb |
0:8f0d870509fe
|
358
|
protocol_buffer_synchronize();
|
| Sergunb |
0:8f0d870509fe
|
359
|
if (sys.abort) { return; }
|
| Sergunb |
0:8f0d870509fe
|
360
|
sys.override_ctrl = override_state;
|
| Sergunb |
0:8f0d870509fe
|
361
|
}
|
| Sergunb |
0:8f0d870509fe
|
362
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
363
|
// Method to ready the system to reset by setting the realtime reset command and killing any
|
| Sergunb |
0:8f0d870509fe
|
364
|
// active processes in the system. This also checks if a system reset is issued while Grbl
|
| Sergunb |
0:8f0d870509fe
|
365
|
// is in a motion state. If so, kills the steppers and sets the system alarm to flag position
|
| Sergunb |
0:8f0d870509fe
|
366
|
// lost, since there was an abrupt uncontrolled deceleration. Called at an interrupt level by
|
| Sergunb |
0:8f0d870509fe
|
367
|
// realtime abort command and hard limits. So, keep to a minimum.
|
| Sergunb |
0:8f0d870509fe
|
368
|
void mc_reset()
|
| Sergunb |
0:8f0d870509fe
|
369
|
{
|
| Sergunb |
0:8f0d870509fe
|
370
|
// Only this function can set the system reset. Helps prevent multiple kill calls.
|
| Sergunb |
0:8f0d870509fe
|
371
|
if (bit_isfalse(sys_rt_exec_state, EXEC_RESET)) {
|
| Sergunb |
0:8f0d870509fe
|
372
|
system_set_exec_state_flag(EXEC_RESET);
|
| Sergunb |
0:8f0d870509fe
|
373
|
|
| Sergunb |
0:8f0d870509fe
|
374
|
// Kill spindle and coolant.
|
| Sergunb |
0:8f0d870509fe
|
375
|
spindle_stop();
|
| Sergunb |
0:8f0d870509fe
|
376
|
coolant_stop();
|
| Sergunb |
0:8f0d870509fe
|
377
|
|
| Sergunb |
0:8f0d870509fe
|
378
|
// Kill steppers only if in any motion state, i.e. cycle, actively holding, or homing.
|
| Sergunb |
0:8f0d870509fe
|
379
|
// NOTE: If steppers are kept enabled via the step idle delay setting, this also keeps
|
| Sergunb |
0:8f0d870509fe
|
380
|
// the steppers enabled by avoiding the go_idle call altogether, unless the motion state is
|
| Sergunb |
0:8f0d870509fe
|
381
|
// violated, by which, all bets are off.
|
| Sergunb |
0:8f0d870509fe
|
382
|
if ((sys.state & (STATE_CYCLE | STATE_HOMING | STATE_JOG)) ||
|
| Sergunb |
0:8f0d870509fe
|
383
|
(sys.step_control & (STEP_CONTROL_EXECUTE_HOLD | STEP_CONTROL_EXECUTE_SYS_MOTION))) {
|
| Sergunb |
0:8f0d870509fe
|
384
|
if (sys.state == STATE_HOMING) {
|
| Sergunb |
0:8f0d870509fe
|
385
|
if (!sys_rt_exec_alarm) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_RESET); }
|
| Sergunb |
0:8f0d870509fe
|
386
|
}
|
| Sergunb |
0:8f0d870509fe
|
387
|
else { system_set_exec_alarm(EXEC_ALARM_ABORT_CYCLE); }
|
| Sergunb |
0:8f0d870509fe
|
388
|
st_go_idle(); // Force kill steppers. Position has likely been lost.
|
| Sergunb |
0:8f0d870509fe
|
389
|
}
|
| Sergunb |
0:8f0d870509fe
|
390
|
}
|
| Sergunb |
0:8f0d870509fe
|
391
|
}
|