| User | Revision | Line number | New contents of line |
|---|
| Sergunb |
0:8f0d870509fe
|
1
|
/*
|
| Sergunb |
0:8f0d870509fe
|
2
|
limits.c - code pertaining to limit-switches and performing the homing cycle
|
| Sergunb |
0:8f0d870509fe
|
3
|
Part of Grbl
|
| Sergunb |
0:8f0d870509fe
|
4
|
|
| Sergunb |
0:8f0d870509fe
|
5
|
Copyright (c) 2012-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
|
// Homing axis search distance multiplier. Computed by this value times the cycle travel.
|
| Sergunb |
0:8f0d870509fe
|
26
|
#ifndef HOMING_AXIS_SEARCH_SCALAR
|
| Sergunb |
0:8f0d870509fe
|
27
|
#define HOMING_AXIS_SEARCH_SCALAR 1.5f // Must be > 1 to ensure limit switch will be engaged.
|
| Sergunb |
0:8f0d870509fe
|
28
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
29
|
#ifndef HOMING_AXIS_LOCATE_SCALAR
|
| Sergunb |
0:8f0d870509fe
|
30
|
#define HOMING_AXIS_LOCATE_SCALAR 5.0f // Must be > 1 to ensure limit switch is cleared.
|
| Sergunb |
0:8f0d870509fe
|
31
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
32
|
|
| Sergunb |
0:8f0d870509fe
|
33
|
void limits_init()
|
| Sergunb |
0:8f0d870509fe
|
34
|
{
|
| Sergunb |
0:8f0d870509fe
|
35
|
#ifdef AVRTARGET
|
| Sergunb |
0:8f0d870509fe
|
36
|
LIMIT_DDR &= ~(LIMIT_MASK); // Set as input pins
|
| Sergunb |
0:8f0d870509fe
|
37
|
|
| Sergunb |
0:8f0d870509fe
|
38
|
#ifdef DISABLE_LIMIT_PIN_PULL_UP
|
| Sergunb |
0:8f0d870509fe
|
39
|
LIMIT_PORT &= ~(LIMIT_MASK); // Normal low operation. Requires external pull-down.
|
| Sergunb |
0:8f0d870509fe
|
40
|
#else
|
| Sergunb |
0:8f0d870509fe
|
41
|
LIMIT_PORT |= (LIMIT_MASK); // Enable internal pull-up resistors. Normal high operation.
|
| Sergunb |
0:8f0d870509fe
|
42
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
43
|
|
| Sergunb |
0:8f0d870509fe
|
44
|
if (bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)) {
|
| Sergunb |
0:8f0d870509fe
|
45
|
LIMIT_PCMSK |= LIMIT_MASK; // Enable specific pins of the Pin Change Interrupt
|
| Sergunb |
0:8f0d870509fe
|
46
|
PCICR |= (1 << LIMIT_INT); // Enable Pin Change Interrupt
|
| Sergunb |
0:8f0d870509fe
|
47
|
} else {
|
| Sergunb |
0:8f0d870509fe
|
48
|
limits_disable();
|
| Sergunb |
0:8f0d870509fe
|
49
|
}
|
| Sergunb |
0:8f0d870509fe
|
50
|
|
| Sergunb |
0:8f0d870509fe
|
51
|
#ifdef ENABLE_SOFTWARE_DEBOUNCE
|
| Sergunb |
0:8f0d870509fe
|
52
|
MCUSR &= ~(1<<WDRF);
|
| Sergunb |
0:8f0d870509fe
|
53
|
WDTCSR |= (1<<WDCE) | (1<<WDE);
|
| Sergunb |
0:8f0d870509fe
|
54
|
WDTCSR = (1<<WDP0); // Set time-out at ~32msec.
|
| Sergunb |
0:8f0d870509fe
|
55
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
56
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
57
|
#ifdef STM32F103C8
|
| Sergunb |
0:8f0d870509fe
|
58
|
GPIO_InitTypeDef GPIO_InitStructure;
|
| Sergunb |
0:8f0d870509fe
|
59
|
RCC_APB2PeriphClockCmd(RCC_LIMIT_PORT | RCC_APB2Periph_AFIO, ENABLE);
|
| Sergunb |
0:8f0d870509fe
|
60
|
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
| Sergunb |
0:8f0d870509fe
|
61
|
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
|
| Sergunb |
0:8f0d870509fe
|
62
|
GPIO_InitStructure.GPIO_Pin = LIMIT_MASK;
|
| Sergunb |
0:8f0d870509fe
|
63
|
GPIO_Init(LIMIT_PORT, &GPIO_InitStructure);
|
| Sergunb |
0:8f0d870509fe
|
64
|
|
| Sergunb |
0:8f0d870509fe
|
65
|
if (bit_istrue(settings.flags, BITFLAG_HARD_LIMIT_ENABLE))
|
| Sergunb |
0:8f0d870509fe
|
66
|
{
|
| Sergunb |
0:8f0d870509fe
|
67
|
GPIO_EXTILineConfig(GPIO_LIMIT_PORT, X_LIMIT_BIT);
|
| Sergunb |
0:8f0d870509fe
|
68
|
GPIO_EXTILineConfig(GPIO_LIMIT_PORT, Y_LIMIT_BIT);
|
| Sergunb |
0:8f0d870509fe
|
69
|
GPIO_EXTILineConfig(GPIO_LIMIT_PORT, Z_LIMIT_BIT);
|
| Sergunb |
0:8f0d870509fe
|
70
|
|
| Sergunb |
0:8f0d870509fe
|
71
|
EXTI_InitTypeDef EXTI_InitStructure;
|
| Sergunb |
0:8f0d870509fe
|
72
|
EXTI_InitStructure.EXTI_Line = LIMIT_MASK; //
|
| Sergunb |
0:8f0d870509fe
|
73
|
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt; //Interrupt mode, optional values for the interrupt EXTI_Mode_Interrupt and event EXTI_Mode_Event.
|
| Sergunb |
0:8f0d870509fe
|
74
|
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling; //Trigger mode, can be a falling edge trigger EXTI_Trigger_Falling, the rising edge triggered EXTI_Trigger_Rising, or any level (rising edge and falling edge trigger EXTI_Trigger_Rising_Falling)
|
| Sergunb |
0:8f0d870509fe
|
75
|
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
|
| Sergunb |
0:8f0d870509fe
|
76
|
EXTI_Init(&EXTI_InitStructure);
|
| Sergunb |
0:8f0d870509fe
|
77
|
|
| Sergunb |
0:8f0d870509fe
|
78
|
NVIC_InitTypeDef NVIC_InitStructure;
|
| Sergunb |
0:8f0d870509fe
|
79
|
NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn; //Enable keypad external interrupt channel
|
| Sergunb |
0:8f0d870509fe
|
80
|
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x02; //Priority 2,
|
| Sergunb |
0:8f0d870509fe
|
81
|
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x02; //Sub priority 2
|
| Sergunb |
0:8f0d870509fe
|
82
|
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //Enable external interrupt channel
|
| Sergunb |
0:8f0d870509fe
|
83
|
NVIC_Init(&NVIC_InitStructure);
|
| Sergunb |
0:8f0d870509fe
|
84
|
}
|
| Sergunb |
0:8f0d870509fe
|
85
|
else
|
| Sergunb |
0:8f0d870509fe
|
86
|
{
|
| Sergunb |
0:8f0d870509fe
|
87
|
limits_disable();
|
| Sergunb |
0:8f0d870509fe
|
88
|
}
|
| Sergunb |
0:8f0d870509fe
|
89
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
90
|
}
|
| Sergunb |
0:8f0d870509fe
|
91
|
|
| Sergunb |
0:8f0d870509fe
|
92
|
|
| Sergunb |
0:8f0d870509fe
|
93
|
// Disables hard limits.
|
| Sergunb |
0:8f0d870509fe
|
94
|
void limits_disable()
|
| Sergunb |
0:8f0d870509fe
|
95
|
{
|
| Sergunb |
0:8f0d870509fe
|
96
|
#ifdef AVRTARGET
|
| Sergunb |
0:8f0d870509fe
|
97
|
LIMIT_PCMSK &= ~LIMIT_MASK; // Disable specific pins of the Pin Change Interrupt
|
| Sergunb |
0:8f0d870509fe
|
98
|
PCICR &= ~(1 << LIMIT_INT); // Disable Pin Change Interrupt
|
| Sergunb |
0:8f0d870509fe
|
99
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
100
|
#ifdef STM32F103C8
|
| Sergunb |
0:8f0d870509fe
|
101
|
NVIC_DisableIRQ(EXTI15_10_IRQn);
|
| Sergunb |
0:8f0d870509fe
|
102
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
103
|
}
|
| Sergunb |
0:8f0d870509fe
|
104
|
|
| Sergunb |
0:8f0d870509fe
|
105
|
|
| Sergunb |
0:8f0d870509fe
|
106
|
// Returns limit state as a bit-wise uint8 variable. Each bit indicates an axis limit, where
|
| Sergunb |
0:8f0d870509fe
|
107
|
// triggered is 1 and not triggered is 0. Invert mask is applied. Axes are defined by their
|
| Sergunb |
0:8f0d870509fe
|
108
|
// number in bit position, i.e. Z_AXIS is (1<<2) or bit 2, and Y_AXIS is (1<<1) or bit 1.
|
| Sergunb |
0:8f0d870509fe
|
109
|
uint8_t limits_get_state()
|
| Sergunb |
0:8f0d870509fe
|
110
|
{
|
| Sergunb |
0:8f0d870509fe
|
111
|
uint8_t limit_state = 0;
|
| Sergunb |
0:8f0d870509fe
|
112
|
#if defined(AVRTARGET) || defined(STM32F103C8)
|
| Sergunb |
0:8f0d870509fe
|
113
|
#if defined(AVRTARGET)
|
| Sergunb |
0:8f0d870509fe
|
114
|
uint8_t pin = (LIMIT_PIN & LIMIT_MASK);
|
| Sergunb |
0:8f0d870509fe
|
115
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
116
|
#if defined(STM32F103C8)
|
| Sergunb |
0:8f0d870509fe
|
117
|
uint16_t pin = GPIO_ReadInputData(LIMIT_PIN);
|
| Sergunb |
0:8f0d870509fe
|
118
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
119
|
#ifdef INVERT_LIMIT_PIN_MASK
|
| Sergunb |
0:8f0d870509fe
|
120
|
pin ^= INVERT_LIMIT_PIN_MASK;
|
| Sergunb |
0:8f0d870509fe
|
121
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
122
|
if (bit_isfalse(settings.flags,BITFLAG_INVERT_LIMIT_PINS)) { pin ^= LIMIT_MASK; }
|
| Sergunb |
0:8f0d870509fe
|
123
|
if (pin) {
|
| Sergunb |
0:8f0d870509fe
|
124
|
uint8_t idx;
|
| Sergunb |
0:8f0d870509fe
|
125
|
for (idx=0; idx<N_AXIS; idx++) {
|
| Sergunb |
0:8f0d870509fe
|
126
|
if (pin & limit_pin_mask[idx]) { limit_state |= (1 << idx); }
|
| Sergunb |
0:8f0d870509fe
|
127
|
}
|
| Sergunb |
0:8f0d870509fe
|
128
|
}
|
| Sergunb |
0:8f0d870509fe
|
129
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
130
|
return(limit_state);
|
| Sergunb |
0:8f0d870509fe
|
131
|
}
|
| Sergunb |
0:8f0d870509fe
|
132
|
|
| Sergunb |
0:8f0d870509fe
|
133
|
|
| Sergunb |
0:8f0d870509fe
|
134
|
// This is the Limit Pin Change Interrupt, which handles the hard limit feature. A bouncing
|
| Sergunb |
0:8f0d870509fe
|
135
|
// limit switch can cause a lot of problems, like false readings and multiple interrupt calls.
|
| Sergunb |
0:8f0d870509fe
|
136
|
// If a switch is triggered at all, something bad has happened and treat it as such, regardless
|
| Sergunb |
0:8f0d870509fe
|
137
|
// if a limit switch is being disengaged. It's impossible to reliably tell the state of a
|
| Sergunb |
0:8f0d870509fe
|
138
|
// bouncing pin because the Arduino microcontroller does not retain any state information when
|
| Sergunb |
0:8f0d870509fe
|
139
|
// detecting a pin change. If we poll the pins in the ISR, you can miss the correct reading if the
|
| Sergunb |
0:8f0d870509fe
|
140
|
// switch is bouncing.
|
| Sergunb |
0:8f0d870509fe
|
141
|
// NOTE: Do not attach an e-stop to the limit pins, because this interrupt is disabled during
|
| Sergunb |
0:8f0d870509fe
|
142
|
// homing cycles and will not respond correctly. Upon user request or need, there may be a
|
| Sergunb |
0:8f0d870509fe
|
143
|
// special pinout for an e-stop, but it is generally recommended to just directly connect
|
| Sergunb |
0:8f0d870509fe
|
144
|
// your e-stop switch to the Arduino reset pin, since it is the most correct way to do this.
|
| Sergunb |
0:8f0d870509fe
|
145
|
#ifndef ENABLE_SOFTWARE_DEBOUNCE
|
| Sergunb |
0:8f0d870509fe
|
146
|
#if defined(AVRTARGET) || defined (STM32F103C8)
|
| Sergunb |
0:8f0d870509fe
|
147
|
#if defined(AVRTARGET)
|
| Sergunb |
0:8f0d870509fe
|
148
|
ISR(LIMIT_INT_vect) // DEFAULT: Limit pin change interrupt process.
|
| Sergunb |
0:8f0d870509fe
|
149
|
#else
|
| Sergunb |
0:8f0d870509fe
|
150
|
void EXTI15_10_IRQHandler(void)
|
| Sergunb |
0:8f0d870509fe
|
151
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
152
|
{
|
| Sergunb |
0:8f0d870509fe
|
153
|
#if defined (STM32F103C8)
|
| Sergunb |
0:8f0d870509fe
|
154
|
if (EXTI_GetITStatus(1 << X_LIMIT_BIT) != RESET)
|
| Sergunb |
0:8f0d870509fe
|
155
|
{
|
| Sergunb |
0:8f0d870509fe
|
156
|
EXTI_ClearITPendingBit(1 << X_LIMIT_BIT);
|
| Sergunb |
0:8f0d870509fe
|
157
|
}
|
| Sergunb |
0:8f0d870509fe
|
158
|
if (EXTI_GetITStatus(1 << Y_LIMIT_BIT) != RESET)
|
| Sergunb |
0:8f0d870509fe
|
159
|
{
|
| Sergunb |
0:8f0d870509fe
|
160
|
EXTI_ClearITPendingBit(1 << Y_LIMIT_BIT);
|
| Sergunb |
0:8f0d870509fe
|
161
|
}
|
| Sergunb |
0:8f0d870509fe
|
162
|
if (EXTI_GetITStatus(1 << Z_LIMIT_BIT) != RESET)
|
| Sergunb |
0:8f0d870509fe
|
163
|
{
|
| Sergunb |
0:8f0d870509fe
|
164
|
EXTI_ClearITPendingBit(1 << Z_LIMIT_BIT);
|
| Sergunb |
0:8f0d870509fe
|
165
|
}
|
| Sergunb |
0:8f0d870509fe
|
166
|
NVIC_ClearPendingIRQ(EXTI15_10_IRQn);
|
| Sergunb |
0:8f0d870509fe
|
167
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
168
|
// Ignore limit switches if already in an alarm state or in-process of executing an alarm.
|
| Sergunb |
0:8f0d870509fe
|
169
|
// When in the alarm state, Grbl should have been reset or will force a reset, so any pending
|
| Sergunb |
0:8f0d870509fe
|
170
|
// moves in the planner and serial buffers are all cleared and newly sent blocks will be
|
| Sergunb |
0:8f0d870509fe
|
171
|
// locked out until a homing cycle or a kill lock command. Allows the user to disable the hard
|
| Sergunb |
0:8f0d870509fe
|
172
|
// limit setting if their limits are constantly triggering after a reset and move their axes.
|
| Sergunb |
0:8f0d870509fe
|
173
|
if (sys.state != STATE_ALARM) {
|
| Sergunb |
0:8f0d870509fe
|
174
|
if (!(sys_rt_exec_alarm)) {
|
| Sergunb |
0:8f0d870509fe
|
175
|
#ifdef HARD_LIMIT_FORCE_STATE_CHECK
|
| Sergunb |
0:8f0d870509fe
|
176
|
// Check limit pin state.
|
| Sergunb |
0:8f0d870509fe
|
177
|
if (limits_get_state()) {
|
| Sergunb |
0:8f0d870509fe
|
178
|
mc_reset(); // Initiate system kill.
|
| Sergunb |
0:8f0d870509fe
|
179
|
system_set_exec_alarm(EXEC_ALARM_HARD_LIMIT); // Indicate hard limit critical event
|
| Sergunb |
0:8f0d870509fe
|
180
|
}
|
| Sergunb |
0:8f0d870509fe
|
181
|
#else
|
| Sergunb |
0:8f0d870509fe
|
182
|
mc_reset(); // Initiate system kill.
|
| Sergunb |
0:8f0d870509fe
|
183
|
system_set_exec_alarm(EXEC_ALARM_HARD_LIMIT); // Indicate hard limit critical event
|
| Sergunb |
0:8f0d870509fe
|
184
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
185
|
}
|
| Sergunb |
0:8f0d870509fe
|
186
|
}
|
| Sergunb |
0:8f0d870509fe
|
187
|
}
|
| Sergunb |
0:8f0d870509fe
|
188
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
189
|
#else // OPTIONAL: Software debounce limit pin routine.
|
| Sergunb |
0:8f0d870509fe
|
190
|
#if defined(AVRTARGET)
|
| Sergunb |
0:8f0d870509fe
|
191
|
// Upon limit pin change, enable watchdog timer to create a short delay.
|
| Sergunb |
0:8f0d870509fe
|
192
|
ISR(LIMIT_INT_vect) { if (!(WDTCSR & (1 << WDIE))) { WDTCSR |= (1 << WDIE); } }
|
| Sergunb |
0:8f0d870509fe
|
193
|
ISR(WDT_vect) // Watchdog timer ISR
|
| Sergunb |
0:8f0d870509fe
|
194
|
{
|
| Sergunb |
0:8f0d870509fe
|
195
|
WDTCSR &= ~(1 << WDIE); // Disable watchdog timer.
|
| Sergunb |
0:8f0d870509fe
|
196
|
if (sys.state != STATE_ALARM) { // Ignore if already in alarm state.
|
| Sergunb |
0:8f0d870509fe
|
197
|
if (!(sys_rt_exec_alarm)) {
|
| Sergunb |
0:8f0d870509fe
|
198
|
// Check limit pin state.
|
| Sergunb |
0:8f0d870509fe
|
199
|
if (limits_get_state()) {
|
| Sergunb |
0:8f0d870509fe
|
200
|
mc_reset(); // Initiate system kill.
|
| Sergunb |
0:8f0d870509fe
|
201
|
system_set_exec_alarm(EXEC_ALARM_HARD_LIMIT); // Indicate hard limit critical event
|
| Sergunb |
0:8f0d870509fe
|
202
|
}
|
| Sergunb |
0:8f0d870509fe
|
203
|
}
|
| Sergunb |
0:8f0d870509fe
|
204
|
}
|
| Sergunb |
0:8f0d870509fe
|
205
|
}
|
| Sergunb |
0:8f0d870509fe
|
206
|
#else
|
| Sergunb |
0:8f0d870509fe
|
207
|
#error ENABLE_SOFTWARE_DEBOUNCE is not supported yet
|
| Sergunb |
0:8f0d870509fe
|
208
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
209
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
210
|
|
| Sergunb |
0:8f0d870509fe
|
211
|
// Homes the specified cycle axes, sets the machine position, and performs a pull-off motion after
|
| Sergunb |
0:8f0d870509fe
|
212
|
// completing. Homing is a special motion case, which involves rapid uncontrolled stops to locate
|
| Sergunb |
0:8f0d870509fe
|
213
|
// the trigger point of the limit switches. The rapid stops are handled by a system level axis lock
|
| Sergunb |
0:8f0d870509fe
|
214
|
// mask, which prevents the stepper algorithm from executing step pulses. Homing motions typically
|
| Sergunb |
0:8f0d870509fe
|
215
|
// circumvent the processes for executing motions in normal operation.
|
| Sergunb |
0:8f0d870509fe
|
216
|
// NOTE: Only the abort realtime command can interrupt this process.
|
| Sergunb |
0:8f0d870509fe
|
217
|
// TODO: Move limit pin-specific calls to a general function for portability.
|
| Sergunb |
0:8f0d870509fe
|
218
|
void limits_go_home(uint8_t cycle_mask)
|
| Sergunb |
0:8f0d870509fe
|
219
|
{
|
| Sergunb |
0:8f0d870509fe
|
220
|
if (sys.abort) { return; } // Block if system reset has been issued.
|
| Sergunb |
0:8f0d870509fe
|
221
|
|
| Sergunb |
0:8f0d870509fe
|
222
|
// Initialize plan data struct for homing motion. Spindle and coolant are disabled.
|
| Sergunb |
0:8f0d870509fe
|
223
|
plan_line_data_t plan_data;
|
| Sergunb |
0:8f0d870509fe
|
224
|
plan_line_data_t *pl_data = &plan_data;
|
| Sergunb |
0:8f0d870509fe
|
225
|
memset(pl_data,0,sizeof(plan_line_data_t));
|
| Sergunb |
0:8f0d870509fe
|
226
|
pl_data->condition = (PL_COND_FLAG_SYSTEM_MOTION|PL_COND_FLAG_NO_FEED_OVERRIDE);
|
| Sergunb |
0:8f0d870509fe
|
227
|
#ifdef USE_LINE_NUMBERS
|
| Sergunb |
0:8f0d870509fe
|
228
|
pl_data->line_number = HOMING_CYCLE_LINE_NUMBER;
|
| Sergunb |
0:8f0d870509fe
|
229
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
230
|
|
| Sergunb |
0:8f0d870509fe
|
231
|
// Initialize variables used for homing computations.
|
| Sergunb |
0:8f0d870509fe
|
232
|
uint8_t n_cycle = (2*N_HOMING_LOCATE_CYCLE+1);
|
| Sergunb |
0:8f0d870509fe
|
233
|
uint8_t step_pin[N_AXIS];
|
| Sergunb |
0:8f0d870509fe
|
234
|
float target[N_AXIS];
|
| Sergunb |
0:8f0d870509fe
|
235
|
float max_travel = 0.0f;
|
| Sergunb |
0:8f0d870509fe
|
236
|
uint8_t idx;
|
| Sergunb |
0:8f0d870509fe
|
237
|
for (idx=0; idx<N_AXIS; idx++) {
|
| Sergunb |
0:8f0d870509fe
|
238
|
// Initialize step pin masks
|
| Sergunb |
0:8f0d870509fe
|
239
|
step_pin[idx] = step_pin_mask[idx];
|
| Sergunb |
0:8f0d870509fe
|
240
|
#ifdef COREXY
|
| Sergunb |
0:8f0d870509fe
|
241
|
if ((idx==A_MOTOR)||(idx==B_MOTOR)) { step_pin[idx] = (step_pin_mask[X_AXIS]| step_pin_mask[Y_AXIS]); }
|
| Sergunb |
0:8f0d870509fe
|
242
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
243
|
|
| Sergunb |
0:8f0d870509fe
|
244
|
if (bit_istrue(cycle_mask,bit(idx))) {
|
| Sergunb |
0:8f0d870509fe
|
245
|
// Set target based on max_travel setting. Ensure homing switches engaged with search scalar.
|
| Sergunb |
0:8f0d870509fe
|
246
|
// NOTE: settings.max_travel[] is stored as a negative value.
|
| Sergunb |
0:8f0d870509fe
|
247
|
max_travel = max(max_travel,(-HOMING_AXIS_SEARCH_SCALAR)*settings.max_travel[idx]);
|
| Sergunb |
0:8f0d870509fe
|
248
|
}
|
| Sergunb |
0:8f0d870509fe
|
249
|
}
|
| Sergunb |
0:8f0d870509fe
|
250
|
|
| Sergunb |
0:8f0d870509fe
|
251
|
// Set search mode with approach at seek rate to quickly engage the specified cycle_mask limit switches.
|
| Sergunb |
0:8f0d870509fe
|
252
|
bool approach = true;
|
| Sergunb |
0:8f0d870509fe
|
253
|
float homing_rate = settings.homing_seek_rate;
|
| Sergunb |
0:8f0d870509fe
|
254
|
|
| Sergunb |
0:8f0d870509fe
|
255
|
uint8_t limit_state, axislock, n_active_axis;
|
| Sergunb |
0:8f0d870509fe
|
256
|
do {
|
| Sergunb |
0:8f0d870509fe
|
257
|
|
| Sergunb |
0:8f0d870509fe
|
258
|
system_convert_array_steps_to_mpos(target,sys_position);
|
| Sergunb |
0:8f0d870509fe
|
259
|
|
| Sergunb |
0:8f0d870509fe
|
260
|
// Initialize and declare variables needed for homing routine.
|
| Sergunb |
0:8f0d870509fe
|
261
|
axislock = 0;
|
| Sergunb |
0:8f0d870509fe
|
262
|
n_active_axis = 0;
|
| Sergunb |
0:8f0d870509fe
|
263
|
for (idx=0; idx<N_AXIS; idx++) {
|
| Sergunb |
0:8f0d870509fe
|
264
|
// Set target location for active axes and setup computation for homing rate.
|
| Sergunb |
0:8f0d870509fe
|
265
|
if (bit_istrue(cycle_mask,bit(idx))) {
|
| Sergunb |
0:8f0d870509fe
|
266
|
n_active_axis++;
|
| Sergunb |
0:8f0d870509fe
|
267
|
#ifdef COREXY
|
| Sergunb |
0:8f0d870509fe
|
268
|
if (idx == X_AXIS) {
|
| Sergunb |
0:8f0d870509fe
|
269
|
int32_t axis_position = system_convert_corexy_to_y_axis_steps(sys_position);
|
| Sergunb |
0:8f0d870509fe
|
270
|
sys_position[A_MOTOR] = axis_position;
|
| Sergunb |
0:8f0d870509fe
|
271
|
sys_position[B_MOTOR] = -axis_position;
|
| Sergunb |
0:8f0d870509fe
|
272
|
} else if (idx == Y_AXIS) {
|
| Sergunb |
0:8f0d870509fe
|
273
|
int32_t axis_position = system_convert_corexy_to_x_axis_steps(sys_position);
|
| Sergunb |
0:8f0d870509fe
|
274
|
sys_position[A_MOTOR] = sys_position[B_MOTOR] = axis_position;
|
| Sergunb |
0:8f0d870509fe
|
275
|
} else {
|
| Sergunb |
0:8f0d870509fe
|
276
|
sys_position[Z_AXIS] = 0;
|
| Sergunb |
0:8f0d870509fe
|
277
|
}
|
| Sergunb |
0:8f0d870509fe
|
278
|
#else
|
| Sergunb |
0:8f0d870509fe
|
279
|
sys_position[idx] = 0;
|
| Sergunb |
0:8f0d870509fe
|
280
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
281
|
// Set target direction based on cycle mask and homing cycle approach state.
|
| Sergunb |
0:8f0d870509fe
|
282
|
// NOTE: This happens to compile smaller than any other implementation tried.
|
| Sergunb |
0:8f0d870509fe
|
283
|
if (bit_istrue(settings.homing_dir_mask,bit(idx))) {
|
| Sergunb |
0:8f0d870509fe
|
284
|
if (approach) { target[idx] = -max_travel; }
|
| Sergunb |
0:8f0d870509fe
|
285
|
else { target[idx] = max_travel; }
|
| Sergunb |
0:8f0d870509fe
|
286
|
} else {
|
| Sergunb |
0:8f0d870509fe
|
287
|
if (approach) { target[idx] = max_travel; }
|
| Sergunb |
0:8f0d870509fe
|
288
|
else { target[idx] = -max_travel; }
|
| Sergunb |
0:8f0d870509fe
|
289
|
}
|
| Sergunb |
0:8f0d870509fe
|
290
|
// Apply axislock to the step port pins active in this cycle.
|
| Sergunb |
0:8f0d870509fe
|
291
|
axislock |= step_pin[idx];
|
| Sergunb |
0:8f0d870509fe
|
292
|
}
|
| Sergunb |
0:8f0d870509fe
|
293
|
|
| Sergunb |
0:8f0d870509fe
|
294
|
}
|
| Sergunb |
0:8f0d870509fe
|
295
|
homing_rate *= sqrtf(n_active_axis); // [sqrt(N_AXIS)] Adjust so individual axes all move at homing rate.
|
| Sergunb |
0:8f0d870509fe
|
296
|
sys.homing_axis_lock = axislock;
|
| Sergunb |
0:8f0d870509fe
|
297
|
|
| Sergunb |
0:8f0d870509fe
|
298
|
// Perform homing cycle. Planner buffer should be empty, as required to initiate the homing cycle.
|
| Sergunb |
0:8f0d870509fe
|
299
|
pl_data->feed_rate = homing_rate; // Set current homing rate.
|
| Sergunb |
0:8f0d870509fe
|
300
|
plan_buffer_line(target, pl_data); // Bypass mc_line(). Directly plan homing motion.
|
| Sergunb |
0:8f0d870509fe
|
301
|
|
| Sergunb |
0:8f0d870509fe
|
302
|
sys.step_control = STEP_CONTROL_EXECUTE_SYS_MOTION; // Set to execute homing motion and clear existing flags.
|
| Sergunb |
0:8f0d870509fe
|
303
|
st_prep_buffer(); // Prep and fill segment buffer from newly planned block.
|
| Sergunb |
0:8f0d870509fe
|
304
|
st_wake_up(); // Initiate motion
|
| Sergunb |
0:8f0d870509fe
|
305
|
do {
|
| Sergunb |
0:8f0d870509fe
|
306
|
if (approach) {
|
| Sergunb |
0:8f0d870509fe
|
307
|
// Check limit state. Lock out cycle axes when they change.
|
| Sergunb |
0:8f0d870509fe
|
308
|
limit_state = limits_get_state();
|
| Sergunb |
0:8f0d870509fe
|
309
|
for (idx=0; idx<N_AXIS; idx++) {
|
| Sergunb |
0:8f0d870509fe
|
310
|
if (axislock & step_pin[idx]) {
|
| Sergunb |
0:8f0d870509fe
|
311
|
if (limit_state & (1 << idx)) {
|
| Sergunb |
0:8f0d870509fe
|
312
|
#ifdef COREXY
|
| Sergunb |
0:8f0d870509fe
|
313
|
if (idx==Z_AXIS) { axislock &= ~(step_pin[Z_AXIS]); }
|
| Sergunb |
0:8f0d870509fe
|
314
|
else { axislock &= ~(step_pin[A_MOTOR]|step_pin[B_MOTOR]); }
|
| Sergunb |
0:8f0d870509fe
|
315
|
#else
|
| Sergunb |
0:8f0d870509fe
|
316
|
axislock &= ~(step_pin[idx]);
|
| Sergunb |
0:8f0d870509fe
|
317
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
318
|
}
|
| Sergunb |
0:8f0d870509fe
|
319
|
}
|
| Sergunb |
0:8f0d870509fe
|
320
|
}
|
| Sergunb |
0:8f0d870509fe
|
321
|
sys.homing_axis_lock = axislock;
|
| Sergunb |
0:8f0d870509fe
|
322
|
}
|
| Sergunb |
0:8f0d870509fe
|
323
|
|
| Sergunb |
0:8f0d870509fe
|
324
|
st_prep_buffer(); // Check and prep segment buffer. NOTE: Should take no longer than 200us.
|
| Sergunb |
0:8f0d870509fe
|
325
|
|
| Sergunb |
0:8f0d870509fe
|
326
|
// Exit routines: No time to run protocol_execute_realtime() in this loop.
|
| Sergunb |
0:8f0d870509fe
|
327
|
if (sys_rt_exec_state & (EXEC_SAFETY_DOOR | EXEC_RESET | EXEC_CYCLE_STOP)) {
|
| Sergunb |
0:8f0d870509fe
|
328
|
uint8_t rt_exec = sys_rt_exec_state;
|
| Sergunb |
0:8f0d870509fe
|
329
|
// Homing failure condition: Reset issued during cycle.
|
| Sergunb |
0:8f0d870509fe
|
330
|
if (rt_exec & EXEC_RESET) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_RESET); }
|
| Sergunb |
0:8f0d870509fe
|
331
|
// Homing failure condition: Safety door was opened.
|
| Sergunb |
0:8f0d870509fe
|
332
|
if (rt_exec & EXEC_SAFETY_DOOR) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_DOOR); }
|
| Sergunb |
0:8f0d870509fe
|
333
|
// Homing failure condition: Limit switch still engaged after pull-off motion
|
| Sergunb |
0:8f0d870509fe
|
334
|
if (!approach && (limits_get_state() & cycle_mask)) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_PULLOFF); }
|
| Sergunb |
0:8f0d870509fe
|
335
|
// Homing failure condition: Limit switch not found during approach.
|
| Sergunb |
0:8f0d870509fe
|
336
|
if (approach && (rt_exec & EXEC_CYCLE_STOP)) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_APPROACH); }
|
| Sergunb |
0:8f0d870509fe
|
337
|
if (sys_rt_exec_alarm) {
|
| Sergunb |
0:8f0d870509fe
|
338
|
mc_reset(); // Stop motors, if they are running.
|
| Sergunb |
0:8f0d870509fe
|
339
|
protocol_execute_realtime();
|
| Sergunb |
0:8f0d870509fe
|
340
|
return;
|
| Sergunb |
0:8f0d870509fe
|
341
|
} else {
|
| Sergunb |
0:8f0d870509fe
|
342
|
// Pull-off motion complete. Disable CYCLE_STOP from executing.
|
| Sergunb |
0:8f0d870509fe
|
343
|
system_clear_exec_state_flag(EXEC_CYCLE_STOP);
|
| Sergunb |
0:8f0d870509fe
|
344
|
break;
|
| Sergunb |
0:8f0d870509fe
|
345
|
}
|
| Sergunb |
0:8f0d870509fe
|
346
|
}
|
| Sergunb |
0:8f0d870509fe
|
347
|
|
| Sergunb |
0:8f0d870509fe
|
348
|
} while (STEP_MASK & axislock);
|
| Sergunb |
0:8f0d870509fe
|
349
|
|
| Sergunb |
0:8f0d870509fe
|
350
|
st_reset(); // Immediately force kill steppers and reset step segment buffer.
|
| Sergunb |
0:8f0d870509fe
|
351
|
delay_ms(settings.homing_debounce_delay); // Delay to allow transient dynamics to dissipate.
|
| Sergunb |
0:8f0d870509fe
|
352
|
|
| Sergunb |
0:8f0d870509fe
|
353
|
// Reverse direction and reset homing rate for locate cycle(s).
|
| Sergunb |
0:8f0d870509fe
|
354
|
approach = !approach;
|
| Sergunb |
0:8f0d870509fe
|
355
|
|
| Sergunb |
0:8f0d870509fe
|
356
|
// After first cycle, homing enters locating phase. Shorten search to pull-off distance.
|
| Sergunb |
0:8f0d870509fe
|
357
|
if (approach) {
|
| Sergunb |
0:8f0d870509fe
|
358
|
max_travel = settings.homing_pulloff*HOMING_AXIS_LOCATE_SCALAR;
|
| Sergunb |
0:8f0d870509fe
|
359
|
homing_rate = settings.homing_feed_rate;
|
| Sergunb |
0:8f0d870509fe
|
360
|
} else {
|
| Sergunb |
0:8f0d870509fe
|
361
|
max_travel = settings.homing_pulloff;
|
| Sergunb |
0:8f0d870509fe
|
362
|
homing_rate = settings.homing_seek_rate;
|
| Sergunb |
0:8f0d870509fe
|
363
|
}
|
| Sergunb |
0:8f0d870509fe
|
364
|
|
| Sergunb |
0:8f0d870509fe
|
365
|
} while (n_cycle-- > 0);
|
| Sergunb |
0:8f0d870509fe
|
366
|
|
| Sergunb |
0:8f0d870509fe
|
367
|
// The active cycle axes should now be homed and machine limits have been located. By
|
| Sergunb |
0:8f0d870509fe
|
368
|
// default, Grbl defines machine space as all negative, as do most CNCs. Since limit switches
|
| Sergunb |
0:8f0d870509fe
|
369
|
// can be on either side of an axes, check and set axes machine zero appropriately. Also,
|
| Sergunb |
0:8f0d870509fe
|
370
|
// set up pull-off maneuver from axes limit switches that have been homed. This provides
|
| Sergunb |
0:8f0d870509fe
|
371
|
// some initial clearance off the switches and should also help prevent them from falsely
|
| Sergunb |
0:8f0d870509fe
|
372
|
// triggering when hard limits are enabled or when more than one axes shares a limit pin.
|
| Sergunb |
0:8f0d870509fe
|
373
|
int32_t set_axis_position;
|
| Sergunb |
0:8f0d870509fe
|
374
|
// Set machine positions for homed limit switches. Don't update non-homed axes.
|
| Sergunb |
0:8f0d870509fe
|
375
|
for (idx=0; idx<N_AXIS; idx++) {
|
| Sergunb |
0:8f0d870509fe
|
376
|
// NOTE: settings.max_travel[] is stored as a negative value.
|
| Sergunb |
0:8f0d870509fe
|
377
|
if (cycle_mask & bit(idx)) {
|
| Sergunb |
0:8f0d870509fe
|
378
|
#ifdef HOMING_FORCE_SET_ORIGIN
|
| Sergunb |
0:8f0d870509fe
|
379
|
set_axis_position = 0;
|
| Sergunb |
0:8f0d870509fe
|
380
|
#else
|
| Sergunb |
0:8f0d870509fe
|
381
|
if ( bit_istrue(settings.homing_dir_mask,bit(idx)) ) {
|
| Sergunb |
0:8f0d870509fe
|
382
|
set_axis_position = lroundf((settings.max_travel[idx]+settings.homing_pulloff)*settings.steps_per_mm[idx]);
|
| Sergunb |
0:8f0d870509fe
|
383
|
} else {
|
| Sergunb |
0:8f0d870509fe
|
384
|
set_axis_position = lroundf(-settings.homing_pulloff*settings.steps_per_mm[idx]);
|
| Sergunb |
0:8f0d870509fe
|
385
|
}
|
| Sergunb |
0:8f0d870509fe
|
386
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
387
|
|
| Sergunb |
0:8f0d870509fe
|
388
|
#ifdef COREXY
|
| Sergunb |
0:8f0d870509fe
|
389
|
if (idx==X_AXIS) {
|
| Sergunb |
0:8f0d870509fe
|
390
|
int32_t off_axis_position = system_convert_corexy_to_y_axis_steps(sys_position);
|
| Sergunb |
0:8f0d870509fe
|
391
|
sys_position[A_MOTOR] = set_axis_position + off_axis_position;
|
| Sergunb |
0:8f0d870509fe
|
392
|
sys_position[B_MOTOR] = set_axis_position - off_axis_position;
|
| Sergunb |
0:8f0d870509fe
|
393
|
} else if (idx==Y_AXIS) {
|
| Sergunb |
0:8f0d870509fe
|
394
|
int32_t off_axis_position = system_convert_corexy_to_x_axis_steps(sys_position);
|
| Sergunb |
0:8f0d870509fe
|
395
|
sys_position[A_MOTOR] = off_axis_position + set_axis_position;
|
| Sergunb |
0:8f0d870509fe
|
396
|
sys_position[B_MOTOR] = off_axis_position - set_axis_position;
|
| Sergunb |
0:8f0d870509fe
|
397
|
} else {
|
| Sergunb |
0:8f0d870509fe
|
398
|
sys_position[idx] = set_axis_position;
|
| Sergunb |
0:8f0d870509fe
|
399
|
}
|
| Sergunb |
0:8f0d870509fe
|
400
|
#else
|
| Sergunb |
0:8f0d870509fe
|
401
|
sys_position[idx] = set_axis_position;
|
| Sergunb |
0:8f0d870509fe
|
402
|
#endif
|
| Sergunb |
0:8f0d870509fe
|
403
|
|
| Sergunb |
0:8f0d870509fe
|
404
|
}
|
| Sergunb |
0:8f0d870509fe
|
405
|
}
|
| Sergunb |
0:8f0d870509fe
|
406
|
sys.step_control = STEP_CONTROL_NORMAL_OP; // Return step control to normal operation.
|
| Sergunb |
0:8f0d870509fe
|
407
|
}
|
| Sergunb |
0:8f0d870509fe
|
408
|
|
| Sergunb |
0:8f0d870509fe
|
409
|
|
| Sergunb |
0:8f0d870509fe
|
410
|
// Performs a soft limit check. Called from mc_line() only. Assumes the machine has been homed,
|
| Sergunb |
0:8f0d870509fe
|
411
|
// the workspace volume is in all negative space, and the system is in normal operation.
|
| Sergunb |
0:8f0d870509fe
|
412
|
// NOTE: Used by jogging to limit travel within soft-limit volume.
|
| Sergunb |
0:8f0d870509fe
|
413
|
void limits_soft_check(float *target)
|
| Sergunb |
0:8f0d870509fe
|
414
|
{
|
| Sergunb |
0:8f0d870509fe
|
415
|
if (system_check_travel_limits(target)) {
|
| Sergunb |
0:8f0d870509fe
|
416
|
sys.soft_limit = true;
|
| Sergunb |
0:8f0d870509fe
|
417
|
// Force feed hold if cycle is active. All buffered blocks are guaranteed to be within
|
| Sergunb |
0:8f0d870509fe
|
418
|
// workspace volume so just come to a controlled stop so position is not lost. When complete
|
| Sergunb |
0:8f0d870509fe
|
419
|
// enter alarm mode.
|
| Sergunb |
0:8f0d870509fe
|
420
|
if (sys.state == STATE_CYCLE) {
|
| Sergunb |
0:8f0d870509fe
|
421
|
system_set_exec_state_flag(EXEC_FEED_HOLD);
|
| Sergunb |
0:8f0d870509fe
|
422
|
do {
|
| Sergunb |
0:8f0d870509fe
|
423
|
protocol_execute_realtime();
|
| Sergunb |
0:8f0d870509fe
|
424
|
if (sys.abort) { return; }
|
| Sergunb |
0:8f0d870509fe
|
425
|
} while ( sys.state != STATE_IDLE );
|
| Sergunb |
0:8f0d870509fe
|
426
|
}
|
| Sergunb |
0:8f0d870509fe
|
427
|
mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown.
|
| Sergunb |
0:8f0d870509fe
|
428
|
system_set_exec_alarm(EXEC_ALARM_SOFT_LIMIT); // Indicate soft limit critical event
|
| Sergunb |
0:8f0d870509fe
|
429
|
protocol_execute_realtime(); // Execute to enter critical event loop and system abort
|
| Sergunb |
0:8f0d870509fe
|
430
|
return;
|
| Sergunb |
0:8f0d870509fe
|
431
|
}
|
| Sergunb |
0:8f0d870509fe
|
432
|
}
|