forked from grbl/grbl
-
Notifications
You must be signed in to change notification settings - Fork 0
/
limits.c
306 lines (262 loc) · 13.3 KB
/
limits.c
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
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
/*
limits.c - code pertaining to limit-switches and performing the homing cycle
Part of Grbl v0.9
Copyright (c) 2012-2014 Sungeun K. Jeon
Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Grbl is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
/*
This file is based on work from Grbl v0.8, distributed under the
terms of the MIT-license. See COPYING for more details.
Copyright (c) 2009-2011 Simen Svale Skogsrud
Copyright (c) 2012 Sungeun K. Jeon
*/
#include "system.h"
#include "settings.h"
#include "protocol.h"
#include "planner.h"
#include "stepper.h"
#include "motion_control.h"
#include "limits.h"
#include "report.h"
// Homing axis search distance multiplier. Computed by this value times the axis max travel.
#define HOMING_AXIS_SEARCH_SCALAR 1.5 // Must be > 1 to ensure limit switch will be engaged.
void limits_init()
{
LIMIT_DDR &= ~(LIMIT_MASK); // Set as input pins
if (bit_istrue(settings.flags,BITFLAG_INVERT_LIMIT_PINS)) {
LIMIT_PORT &= ~(LIMIT_MASK); // Normal low operation. Requires external pull-down.
} else {
LIMIT_PORT |= (LIMIT_MASK); // Enable internal pull-up resistors. Normal high operation.
}
if (bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)) {
LIMIT_PCMSK |= LIMIT_MASK; // Enable specific pins of the Pin Change Interrupt
PCICR |= (1 << LIMIT_INT); // Enable Pin Change Interrupt
} else {
limits_disable();
}
#ifdef ENABLE_SOFTWARE_DEBOUNCE
MCUSR &= ~(1<<WDRF);
WDTCSR |= (1<<WDCE) | (1<<WDE);
WDTCSR = (1<<WDP0); // Set time-out at ~32msec.
#endif
}
void limits_disable()
{
LIMIT_PCMSK &= ~LIMIT_MASK; // Disable specific pins of the Pin Change Interrupt
PCICR &= ~(1 << LIMIT_INT); // Disable Pin Change Interrupt
}
// This is the Limit Pin Change Interrupt, which handles the hard limit feature. A bouncing
// limit switch can cause a lot of problems, like false readings and multiple interrupt calls.
// If a switch is triggered at all, something bad has happened and treat it as such, regardless
// if a limit switch is being disengaged. It's impossible to reliably tell the state of a
// bouncing pin without a debouncing method. A simple software debouncing feature may be enabled
// through the config.h file, where an extra timer delays the limit pin read by several milli-
// seconds to help with, not fix, bouncing switches.
// NOTE: Do not attach an e-stop to the limit pins, because this interrupt is disabled during
// homing cycles and will not respond correctly. Upon user request or need, there may be a
// special pinout for an e-stop, but it is generally recommended to just directly connect
// your e-stop switch to the Arduino reset pin, since it is the most correct way to do this.
#ifndef ENABLE_SOFTWARE_DEBOUNCE
ISR(LIMIT_INT_vect) // DEFAULT: Limit pin change interrupt process.
{
// Ignore limit switches if already in an alarm state or in-process of executing an alarm.
// When in the alarm state, Grbl should have been reset or will force a reset, so any pending
// moves in the planner and serial buffers are all cleared and newly sent blocks will be
// locked out until a homing cycle or a kill lock command. Allows the user to disable the hard
// limit setting if their limits are constantly triggering after a reset and move their axes.
if (sys.state != STATE_ALARM) {
if (bit_isfalse(sys.execute,EXEC_ALARM)) {
mc_reset(); // Initiate system kill.
bit_true_atomic(sys.execute, (EXEC_ALARM | EXEC_CRIT_EVENT)); // Indicate hard limit critical event
}
}
}
#else // OPTIONAL: Software debounce limit pin routine.
// Upon limit pin change, enable watchdog timer to create a short delay.
ISR(LIMIT_INT_vect) { if (!(WDTCSR & (1<<WDIE))) { WDTCSR |= (1<<WDIE); } }
ISR(WDT_vect) // Watchdog timer ISR
{
WDTCSR &= ~(1<<WDIE); // Disable watchdog timer.
if (sys.state != STATE_ALARM) { // Ignore if already in alarm state.
if (bit_isfalse(sys.execute,EXEC_ALARM)) {
uint8_t bits = LIMIT_PIN;
// Check limit pin state.
if (bit_istrue(settings.flags,BITFLAG_INVERT_LIMIT_PINS)) { bits ^= LIMIT_MASK; }
if (bits & LIMIT_MASK) {
mc_reset(); // Initiate system kill.
bit_true_atomic(sys.execute, (EXEC_ALARM | EXEC_CRIT_EVENT)); // Indicate hard limit critical event
}
}
}
}
#endif
// Homes the specified cycle axes, sets the machine position, and performs a pull-off motion after
// completing. Homing is a special motion case, which involves rapid uncontrolled stops to locate
// the trigger point of the limit switches. The rapid stops are handled by a system level axis lock
// mask, which prevents the stepper algorithm from executing step pulses. Homing motions typically
// circumvent the processes for executing motions in normal operation.
// NOTE: Only the abort runtime command can interrupt this process.
void limits_go_home(uint8_t cycle_mask)
{
if (sys.abort) { return; } // Block if system reset has been issued.
// Initialize homing in search mode to quickly engage the specified cycle_mask limit switches.
bool approach = true;
float homing_rate = settings.homing_seek_rate;
uint8_t invert_pin, idx;
uint8_t n_cycle = (2*N_HOMING_LOCATE_CYCLE+1);
float target[N_AXIS];
uint8_t limit_pin[N_AXIS], step_pin[N_AXIS];
float max_travel = 0.0;
for (idx=0; idx<N_AXIS; idx++) {
// Initialize limit and step pin masks
limit_pin[idx] = get_limit_pin_mask(idx);
step_pin[idx] = get_step_pin_mask(idx);
// Determine travel distance to the furthest homing switch based on user max travel settings.
// NOTE: settings.max_travel[] is stored as a negative value.
if (max_travel > settings.max_travel[idx]) { max_travel = settings.max_travel[idx]; }
}
max_travel *= -HOMING_AXIS_SEARCH_SCALAR; // Ensure homing switches engaged by over-estimating max travel.
plan_reset(); // Reset planner buffer to zero planner current position and to clear previous motions.
do {
// Initialize invert_pin boolean based on approach and invert pin user setting.
if (bit_isfalse(settings.flags,BITFLAG_INVERT_LIMIT_PINS)) { invert_pin = approach; }
else { invert_pin = !approach; }
// Initialize and declare variables needed for homing routine.
uint8_t n_active_axis = 0;
uint8_t axislock = 0;
for (idx=0; idx<N_AXIS; idx++) {
// Set target location for active axes and setup computation for homing rate.
if (bit_istrue(cycle_mask,bit(idx))) {
n_active_axis++;
if (!approach) { target[idx] = -max_travel; }
else { target[idx] = max_travel; }
} else {
target[idx] = 0.0;
}
// Set target direction based on cycle mask
if (bit_istrue(settings.homing_dir_mask,bit(idx))) { target[idx] = -target[idx]; }
// Apply axislock to the step port pins active in this cycle.
if (bit_istrue(cycle_mask,bit(idx))) { axislock |= step_pin[idx]; }
}
homing_rate *= sqrt(n_active_axis); // [sqrt(N_AXIS)] Adjust so individual axes all move at homing rate.
sys.homing_axis_lock = axislock;
// Perform homing cycle. Planner buffer should be empty, as required to initiate the homing cycle.
uint8_t limit_state;
#ifdef USE_LINE_NUMBERS
plan_buffer_line(target, homing_rate, false, HOMING_CYCLE_LINE_NUMBER); // Bypass mc_line(). Directly plan homing motion.
#else
plan_buffer_line(target, homing_rate, false); // Bypass mc_line(). Directly plan homing motion.
#endif
st_prep_buffer(); // Prep and fill segment buffer from newly planned block.
st_wake_up(); // Initiate motion
do {
// Check limit state. Lock out cycle axes when they change.
limit_state = LIMIT_PIN;
if (invert_pin) { limit_state ^= LIMIT_MASK; }
for (idx=0; idx<N_AXIS; idx++) {
if (axislock & step_pin[idx]) {
if (limit_state & limit_pin[idx]) { axislock &= ~(step_pin[idx]); }
}
}
sys.homing_axis_lock = axislock;
st_prep_buffer(); // Check and prep segment buffer. NOTE: Should take no longer than 200us.
// Check only for user reset. No time to run protocol_execute_runtime() in this loop.
if (sys.execute & EXEC_RESET) { protocol_execute_runtime(); return; }
} while (STEP_MASK & axislock);
st_reset(); // Immediately force kill steppers and reset step segment buffer.
plan_reset(); // Reset planner buffer. Zero planner positions. Ensure homing motion is cleared.
delay_ms(settings.homing_debounce_delay); // Delay to allow transient dynamics to dissipate.
// Reverse direction and reset homing rate for locate cycle(s).
homing_rate = settings.homing_feed_rate;
approach = !approach;
} while (n_cycle-- > 0);
// The active cycle axes should now be homed and machine limits have been located. By
// default, grbl defines machine space as all negative, as do most CNCs. Since limit switches
// can be on either side of an axes, check and set axes machine zero appropriately. Also,
// set up pull-off maneuver from axes limit switches that have been homed. This provides
// some initial clearance off the switches and should also help prevent them from falsely
// triggering when hard limits are enabled or when more than one axes shares a limit pin.
for (idx=0; idx<N_AXIS; idx++) {
// Set up pull off targets and machine positions for limit switches homed in the negative
// direction, rather than the traditional positive. Leave non-homed positions as zero and
// do not move them.
// NOTE: settings.max_travel[] is stored as a negative value.
if (cycle_mask & bit(idx)) {
#ifdef HOMING_FORCE_SET_ORIGIN
sys.position[idx] = 0; // Set axis homed location as axis origin
target[idx] = settings.homing_pulloff;
if ( bit_isfalse(settings.homing_dir_mask,bit(idx)) ) { target[idx] = -target[idx]; }
#else
if ( bit_istrue(settings.homing_dir_mask,bit(idx)) ) {
target[idx] = settings.homing_pulloff+settings.max_travel[idx];
sys.position[idx] = lround(settings.max_travel[idx]*settings.steps_per_mm[idx]);
} else {
target[idx] = -settings.homing_pulloff;
sys.position[idx] = 0;
}
#endif
} else { // Non-active cycle axis. Set target to not move during pull-off.
target[idx] = (float)sys.position[idx]/settings.steps_per_mm[idx];
}
}
plan_sync_position(); // Sync planner position to current machine position for pull-off move.
#ifdef USE_LINE_NUMBERS
plan_buffer_line(target, settings.homing_seek_rate, false, HOMING_CYCLE_LINE_NUMBER); // Bypass mc_line(). Directly plan motion.
#else
plan_buffer_line(target, settings.homing_seek_rate, false); // Bypass mc_line(). Directly plan motion.
#endif
// Initiate pull-off using main motion control routines.
// TODO : Clean up state routines so that this motion still shows homing state.
sys.state = STATE_QUEUED;
bit_true_atomic(sys.execute, EXEC_CYCLE_START);
protocol_execute_runtime();
protocol_buffer_synchronize(); // Complete pull-off motion.
// Set system state to homing before returning.
sys.state = STATE_HOMING;
}
// Performs a soft limit check. Called from mc_line() only. Assumes the machine has been homed,
// the workspace volume is in all negative space, and the system is in normal operation.
void limits_soft_check(float *target)
{
uint8_t idx;
uint8_t soft_limit_error = false;
for (idx=0; idx<N_AXIS; idx++) {
#ifdef HOMING_FORCE_SET_ORIGIN
// When homing forced set origin is enabled, soft limits checks need to account for directionality.
// NOTE: max_travel is stored as negative
if (bit_istrue(settings.homing_dir_mask,bit(idx))) {
if (target[idx] < 0 || target[idx] > -settings.max_travel[idx]) { soft_limit_error = true; }
} else {
if (target[idx] > 0 || target[idx] < settings.max_travel[idx]) { soft_limit_error = true; }
}
#else
// NOTE: max_travel is stored as negative
if (target[idx] > 0 || target[idx] < settings.max_travel[idx]) { soft_limit_error = true; }
#endif
if (soft_limit_error) {
// Force feed hold if cycle is active. All buffered blocks are guaranteed to be within
// workspace volume so just come to a controlled stop so position is not lost. When complete
// enter alarm mode.
if (sys.state == STATE_CYCLE) {
bit_true_atomic(sys.execute, EXEC_FEED_HOLD);
do {
protocol_execute_runtime();
if (sys.abort) { return; }
} while ( sys.state != STATE_IDLE || sys.state != STATE_QUEUED);
}
mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown.
bit_true_atomic(sys.execute, (EXEC_ALARM | EXEC_CRIT_EVENT)); // Indicate soft limit critical event
protocol_execute_runtime(); // Execute to enter critical event loop and system abort
return;
}
}
}