Commit | Line | Data |
---|---|---|
df27a6a3 | 1 | /* |
201bcb94 AW |
2 | This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). |
3 | Smoothie 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. | |
4 | Smoothie 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. | |
df27a6a3 | 5 | You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>. |
201bcb94 AW |
6 | */ |
7 | ||
8 | #include "libs/Module.h" | |
9 | #include "libs/Kernel.h" | |
10 | #include "modules/communication/utils/Gcode.h" | |
3fceb8eb | 11 | #include "modules/robot/Conveyor.h" |
807b9b57 | 12 | #include "modules/robot/ActuatorCoordinates.h" |
201bcb94 AW |
13 | #include "Endstops.h" |
14 | #include "libs/nuts_bolts.h" | |
750277f8 | 15 | #include "libs/Pin.h" |
670fa10b | 16 | #include "libs/StepperMotor.h" |
201bcb94 | 17 | #include "wait_api.h" // mbed.h lib |
61134a65 | 18 | #include "Robot.h" |
61134a65 JM |
19 | #include "Config.h" |
20 | #include "SlowTicker.h" | |
21 | #include "Planner.h" | |
7af0714f JM |
22 | #include "checksumm.h" |
23 | #include "utils.h" | |
8d54c34c | 24 | #include "ConfigValue.h" |
fc7b9a7b | 25 | #include "libs/StreamOutput.h" |
9f6f04a5 JM |
26 | #include "PublicDataRequest.h" |
27 | #include "EndstopsPublicAccess.h" | |
3c947f85 | 28 | #include "StreamOutputPool.h" |
a157d099 | 29 | #include "StepTicker.h" |
7552475b | 30 | #include "BaseSolution.h" |
e551657a | 31 | #include "SerialMessage.h" |
201bcb94 | 32 | |
80605954 | 33 | #include <ctype.h> |
bab1e318 | 34 | #include <algorithm> |
80605954 | 35 | |
718bfc9c | 36 | // OLD deprecated syntax |
33e4cc02 | 37 | #define endstops_module_enable_checksum CHECKSUM("endstops_enable") |
33e4cc02 | 38 | |
eea58a01 JM |
39 | #define ENDSTOP_CHECKSUMS(X) { \ |
40 | CHECKSUM(X "_min_endstop"), \ | |
41 | CHECKSUM(X "_max_endstop"), \ | |
42 | CHECKSUM(X "_max_travel"), \ | |
43 | CHECKSUM(X "_fast_homing_rate_mm_s"), \ | |
44 | CHECKSUM(X "_slow_homing_rate_mm_s"), \ | |
45 | CHECKSUM(X "_homing_retract_mm"), \ | |
46 | CHECKSUM(X "_homing_direction"), \ | |
47 | CHECKSUM(X "_min"), \ | |
718bfc9c JM |
48 | CHECKSUM(X "_max"), \ |
49 | CHECKSUM(X "_limit_enable"), \ | |
eea58a01 | 50 | } |
33e4cc02 | 51 | |
eea58a01 | 52 | // checksum defns |
718bfc9c | 53 | enum DEFNS {MIN_PIN, MAX_PIN, MAX_TRAVEL, FAST_RATE, SLOW_RATE, RETRACT, DIRECTION, MIN, MAX, LIMIT, NDEFNS}; |
33e4cc02 | 54 | |
718bfc9c JM |
55 | // global config settings |
56 | #define corexy_homing_checksum CHECKSUM("corexy_homing") | |
57 | #define delta_homing_checksum CHECKSUM("delta_homing") | |
58 | #define rdelta_homing_checksum CHECKSUM("rdelta_homing") | |
59 | #define scara_homing_checksum CHECKSUM("scara_homing") | |
33e4cc02 JM |
60 | |
61 | #define endstop_debounce_count_checksum CHECKSUM("endstop_debounce_count") | |
7778d1ce | 62 | #define endstop_debounce_ms_checksum CHECKSUM("endstop_debounce_ms") |
33e4cc02 | 63 | |
374d0777 | 64 | #define home_z_first_checksum CHECKSUM("home_z_first") |
80605954 | 65 | #define homing_order_checksum CHECKSUM("homing_order") |
2ddf75fd | 66 | #define move_to_origin_checksum CHECKSUM("move_to_origin_after_home") |
80605954 | 67 | |
718bfc9c JM |
68 | #define alpha_trim_checksum CHECKSUM("alpha_trim_mm") |
69 | #define beta_trim_checksum CHECKSUM("beta_trim_mm") | |
70 | #define gamma_trim_checksum CHECKSUM("gamma_trim_mm") | |
71 | ||
72 | // new config syntax | |
73 | // endstop.xmin.enable true | |
74 | // endstop.xmin.pin 1.29 | |
75 | // endstop.xmin.axis X | |
7d9e5765 | 76 | // endstop.xmin.homing_direction home_to_min |
718bfc9c JM |
77 | |
78 | #define endstop_checksum CHECKSUM("endstop") | |
79 | #define enable_checksum CHECKSUM("enable") | |
80 | #define pin_checksum CHECKSUM("pin") | |
81 | #define axis_checksum CHECKSUM("axis") | |
82 | #define direction_checksum CHECKSUM("homing_direction") | |
83 | #define position_checksum CHECKSUM("homing_position") | |
84 | #define fast_rate_checksum CHECKSUM("fast_rate") | |
85 | #define slow_rate_checksum CHECKSUM("slow_rate") | |
86 | #define max_travel_checksum CHECKSUM("max_travel") | |
87 | #define retract_checksum CHECKSUM("retract") | |
88 | #define limit_checksum CHECKSUM("limit_enable") | |
89 | ||
c8bac202 | 90 | #define STEPPER THEROBOT->actuators |
dd0a7cfa | 91 | #define STEPS_PER_MM(a) (STEPPER[a]->get_steps_per_mm()) |
56ce2b5a | 92 | |
2b3cedc7 | 93 | |
2b3cedc7 | 94 | |
3c947f85 | 95 | // Homing States |
eea58a01 | 96 | enum STATES { |
2ddf75fd | 97 | MOVING_TO_ENDSTOP_FAST, // homing move |
2ddf75fd | 98 | MOVING_TO_ENDSTOP_SLOW, // homing move |
98e30679 | 99 | MOVING_BACK, // homing move |
fafb45df | 100 | NOT_HOMING, |
3c947f85 | 101 | BACK_OFF_HOME, |
2ddf75fd | 102 | MOVE_TO_ORIGIN, |
3c947f85 JM |
103 | LIMIT_TRIGGERED |
104 | }; | |
105 | ||
33e4cc02 JM |
106 | Endstops::Endstops() |
107 | { | |
201bcb94 AW |
108 | this->status = NOT_HOMING; |
109 | } | |
110 | ||
33e4cc02 JM |
111 | void Endstops::on_module_loaded() |
112 | { | |
718bfc9c JM |
113 | // Do not do anything if not enabled or if no pins are defined |
114 | if (THEKERNEL->config->value( endstops_module_enable_checksum )->by_default(false)->as_bool()) { | |
115 | if(!load_old_config()) { | |
116 | delete this; | |
117 | return; | |
118 | } | |
119 | ||
120 | }else{ | |
121 | // check for new config syntax | |
122 | if(!load_config()) { | |
123 | delete this; | |
124 | return; | |
125 | } | |
33e4cc02 | 126 | } |
7d62445b | 127 | |
9f6f04a5 JM |
128 | register_for_event(ON_GCODE_RECEIVED); |
129 | register_for_event(ON_GET_PUBLIC_DATA); | |
7d6fe308 | 130 | register_for_event(ON_SET_PUBLIC_DATA); |
201bcb94 | 131 | |
c8bac202 JM |
132 | |
133 | THEKERNEL->slow_ticker->attach(1000, this, &Endstops::read_endstops); | |
201bcb94 AW |
134 | } |
135 | ||
718bfc9c JM |
136 | // Get config using old deprecated syntax Does not support ABC |
137 | bool Endstops::load_old_config() | |
33e4cc02 | 138 | { |
eea58a01 JM |
139 | uint16_t const checksums[][NDEFNS] = { |
140 | ENDSTOP_CHECKSUMS("alpha"), // X | |
141 | ENDSTOP_CHECKSUMS("beta"), // Y | |
718bfc9c | 142 | ENDSTOP_CHECKSUMS("gamma") // Z |
eea58a01 JM |
143 | }; |
144 | ||
718bfc9c JM |
145 | bool limit_enabled= false; |
146 | for (int i = X_AXIS; i <= Z_AXIS; ++i) { // X_AXIS to Z_AXIS | |
d2eef91b JM |
147 | homing_info_t hinfo; |
148 | ||
149 | // init homing struct | |
150 | hinfo.home_offset= 0; | |
151 | hinfo.homed= false; | |
152 | hinfo.axis= 'X'+i; | |
153 | hinfo.axis_index= i; | |
154 | hinfo.pin_info= nullptr; | |
155 | ||
156 | // rates in mm/sec | |
157 | hinfo.fast_rate= THEKERNEL->config->value(checksums[i][FAST_RATE])->by_default(100)->as_number(); | |
158 | hinfo.slow_rate= THEKERNEL->config->value(checksums[i][SLOW_RATE])->by_default(10)->as_number(); | |
159 | ||
160 | // retract in mm | |
161 | hinfo.retract= THEKERNEL->config->value(checksums[i][RETRACT])->by_default(5)->as_number(); | |
162 | ||
163 | // get homing direction and convert to boolean where true is home to min, and false is home to max | |
164 | hinfo.home_direction= THEKERNEL->config->value(checksums[i][DIRECTION])->by_default("home_to_min")->as_string() != "home_to_max"; | |
165 | ||
166 | // homing cartesian position | |
167 | hinfo.homing_position= hinfo.home_direction ? THEKERNEL->config->value(checksums[i][MIN])->by_default(0)->as_number() : THEKERNEL->config->value(checksums[i][MAX])->by_default(200)->as_number(); | |
168 | ||
169 | // used to set maximum movement on homing, set by alpha_max_travel if defined | |
170 | hinfo.max_travel= THEKERNEL->config->value(checksums[i][MAX_TRAVEL])->by_default(500)->as_number(); | |
171 | ||
172 | ||
173 | // pin definitions for endstop pins | |
eea58a01 | 174 | for (int j = MIN_PIN; j <= MAX_PIN; ++j) { |
d2eef91b | 175 | endstop_info_t *info= new endstop_info_t; |
718bfc9c JM |
176 | info->pin.from_string(THEKERNEL->config->value(checksums[i][j])->by_default("nc" )->as_string())->as_input(); |
177 | if(!info->pin.connected()){ | |
178 | // no pin defined try next | |
179 | delete info; | |
180 | continue; | |
eea58a01 | 181 | } |
718bfc9c | 182 | |
d2eef91b JM |
183 | // enter into endstop array |
184 | endstops.push_back(info); | |
185 | ||
186 | // add index to the homing struct if this is the one used for homing | |
187 | if((hinfo.home_direction && j == MIN_PIN) || (!hinfo.home_direction && j == MAX_PIN)) hinfo.pin_info= info; | |
718bfc9c JM |
188 | |
189 | // init struct | |
718bfc9c JM |
190 | info->debounce= 0; |
191 | info->axis= 'X'+i; | |
192 | info->axis_index= i; | |
193 | ||
718bfc9c JM |
194 | // limits enabled |
195 | info->limit_enable= THEKERNEL->config->value(checksums[i][LIMIT])->by_default(false)->as_bool(); | |
196 | limit_enabled |= info->limit_enable; | |
197 | } | |
d2eef91b JM |
198 | |
199 | homing_axis.push_back(hinfo); | |
718bfc9c JM |
200 | } |
201 | ||
202 | // if no pins defined then disable the module | |
203 | if(endstops.empty()) return false; | |
204 | ||
205 | get_global_configs(); | |
206 | ||
207 | if(limit_enabled) { | |
208 | register_for_event(ON_IDLE); | |
209 | } | |
210 | ||
211 | // sanity check for deltas | |
212 | /* | |
213 | if(this->is_delta || this->is_rdelta) { | |
214 | // some things must be the same or they will die, so force it here to avoid config errors | |
215 | this->fast_rates[1] = this->fast_rates[2] = this->fast_rates[0]; | |
216 | this->slow_rates[1] = this->slow_rates[2] = this->slow_rates[0]; | |
217 | this->retract_mm[1] = this->retract_mm[2] = this->retract_mm[0]; | |
218 | this->home_direction[1] = this->home_direction[2] = this->home_direction[0]; | |
219 | // NOTE homing_position for rdelta is the angle of the actuator not the cartesian position | |
220 | if(!this->is_rdelta) this->homing_position[0] = this->homing_position[1] = 0; | |
221 | } | |
222 | */ | |
223 | ||
224 | return true; | |
225 | } | |
226 | ||
227 | // Get config using new syntax supports ABC | |
228 | bool Endstops::load_config() | |
229 | { | |
230 | bool limit_enabled= false; | |
bab1e318 | 231 | |
7d9e5765 | 232 | std::array<homing_info_t, k_max_actuators> temp_axis_array; // needs to be at least XYZ, but allow for ABC |
bab1e318 JM |
233 | { |
234 | homing_info_t t; | |
235 | t.axis= 0; | |
236 | t.axis_index= 0; | |
237 | t.pin_info= nullptr; | |
238 | ||
239 | temp_axis_array.fill(t); | |
240 | } | |
241 | ||
718bfc9c | 242 | // iterate over all endstop.*.* |
bab1e318 | 243 | std::vector<uint16_t> modules; |
718bfc9c JM |
244 | THEKERNEL->config->get_module_list(&modules, endstop_checksum); |
245 | for(auto cs : modules ) { | |
246 | if(!THEKERNEL->config->value(endstop_checksum, cs, enable_checksum )->as_bool()) continue; | |
bab1e318 JM |
247 | |
248 | endstop_info_t *pin_info= new endstop_info_t; | |
249 | pin_info->pin.from_string(THEKERNEL->config->value(endstop_checksum, cs, pin_checksum)->by_default("nc" )->as_string())->as_input(); | |
250 | if(!pin_info->pin.connected()){ | |
718bfc9c | 251 | // no pin defined try next |
bab1e318 | 252 | delete pin_info; |
718bfc9c JM |
253 | continue; |
254 | } | |
255 | ||
bab1e318 | 256 | string axis= THEKERNEL->config->value(endstop_checksum, cs, axis_checksum)->by_default("")->as_string(); |
718bfc9c JM |
257 | if(axis.empty()){ |
258 | // axis is required | |
bab1e318 | 259 | delete pin_info; |
718bfc9c | 260 | continue; |
eea58a01 JM |
261 | } |
262 | ||
7d9e5765 | 263 | size_t i; |
718bfc9c JM |
264 | switch(toupper(axis[0])) { |
265 | case 'X': i= X_AXIS; break; | |
266 | case 'Y': i= Y_AXIS; break; | |
267 | case 'Z': i= Z_AXIS; break; | |
268 | case 'A': i= A_AXIS; break; | |
269 | case 'B': i= B_AXIS; break; | |
270 | case 'C': i= C_AXIS; break; | |
271 | default: // not a recognized axis | |
bab1e318 | 272 | delete pin_info; |
718bfc9c JM |
273 | continue; |
274 | } | |
275 | ||
bab1e318 JM |
276 | // init pin struct |
277 | pin_info->debounce= 0; | |
278 | pin_info->axis= toupper(axis[0]); | |
279 | pin_info->axis_index= i; | |
280 | ||
281 | // are limits enabled | |
282 | pin_info->limit_enable= THEKERNEL->config->value(endstop_checksum, cs, limit_checksum)->by_default(false)->as_bool(); | |
283 | limit_enabled |= pin_info->limit_enable; | |
284 | ||
285 | // enter into endstop array | |
286 | endstops.push_back(pin_info); | |
287 | ||
7d9e5765 JM |
288 | // check we are not going above the number of defined actuators/axis |
289 | if(i >= k_max_actuators) { | |
290 | // too many axis we only have configured k_max_actuators | |
291 | continue; | |
292 | } | |
293 | ||
bab1e318 | 294 | // if set to none it means not used for homing (maybe limit only) so do not add to the homing array |
718bfc9c | 295 | string direction= THEKERNEL->config->value(endstop_checksum, cs, direction_checksum)->by_default("none")->as_string(); |
bab1e318 JM |
296 | if(direction == "none") { |
297 | continue; | |
298 | } | |
718bfc9c | 299 | |
7b18a698 | 300 | // setup the homing array |
bab1e318 | 301 | homing_info_t hinfo; |
718bfc9c | 302 | |
bab1e318 JM |
303 | // init homing struct |
304 | hinfo.home_offset= 0; | |
305 | hinfo.homed= false; | |
306 | hinfo.axis= toupper(axis[0]); | |
307 | hinfo.axis_index= i; | |
308 | hinfo.pin_info= pin_info; | |
eea58a01 JM |
309 | |
310 | // rates in mm/sec | |
bab1e318 JM |
311 | hinfo.fast_rate= THEKERNEL->config->value(endstop_checksum, cs, fast_rate_checksum)->by_default(100)->as_number(); |
312 | hinfo.slow_rate= THEKERNEL->config->value(endstop_checksum, cs, slow_rate_checksum)->by_default(10)->as_number(); | |
eea58a01 | 313 | |
718bfc9c | 314 | // retract in mm |
bab1e318 | 315 | hinfo.retract= THEKERNEL->config->value(endstop_checksum, cs, retract_checksum)->by_default(5)->as_number(); |
eea58a01 | 316 | |
718bfc9c | 317 | // homing direction and convert to boolean where true is home to min, and false is home to max |
bab1e318 | 318 | hinfo.home_direction= direction == "home_to_min"; |
eea58a01 JM |
319 | |
320 | // homing cartesian position | |
bab1e318 | 321 | hinfo.homing_position= THEKERNEL->config->value(endstop_checksum, cs, position_checksum)->by_default(hinfo.home_direction ? 0 : 200)->as_number(); |
718bfc9c JM |
322 | |
323 | // used to set maximum movement on homing, set by max_travel if defined | |
bab1e318 | 324 | hinfo.max_travel= THEKERNEL->config->value(endstop_checksum, cs, max_travel_checksum)->by_default(500)->as_number(); |
718bfc9c | 325 | |
bab1e318 JM |
326 | // stick into array in correct place |
327 | temp_axis_array[hinfo.axis_index]= hinfo; | |
718bfc9c JM |
328 | } |
329 | ||
330 | // if no pins defined then disable the module | |
331 | if(endstops.empty()) return false; | |
332 | ||
bab1e318 JM |
333 | // copy to the homing_axis array |
334 | for (size_t i = 0; i < temp_axis_array.size(); ++i) { | |
335 | if(temp_axis_array[i].axis == 0) { | |
336 | // was not configured above, if it is XYZ then we need to force a dummy entry | |
337 | if(i <= Z_AXIS) { | |
338 | homing_info_t t; | |
339 | t.axis= 'X' + i; | |
340 | t.axis_index= i; | |
4ebf87af | 341 | t.pin_info= nullptr; // this tells it that it cannot be used for homing |
bab1e318 JM |
342 | homing_axis.push_back(t); |
343 | } | |
344 | ||
345 | }else{ | |
346 | homing_axis.push_back(temp_axis_array[i]); | |
347 | } | |
348 | } | |
349 | ||
718bfc9c JM |
350 | // sets some endstop global configs applicable to all endstops |
351 | get_global_configs(); | |
352 | ||
353 | if(limit_enabled) { | |
354 | register_for_event(ON_IDLE); | |
eea58a01 | 355 | } |
bab1e318 | 356 | |
718bfc9c JM |
357 | return true; |
358 | } | |
eea58a01 | 359 | |
718bfc9c | 360 | void Endstops::get_global_configs() |
33e4cc02 | 361 | { |
eea58a01 JM |
362 | // NOTE the debounce count is in milliseconds so probably does not need to beset anymore |
363 | this->debounce_ms= THEKERNEL->config->value(endstop_debounce_ms_checksum)->by_default(0)->as_number(); | |
364 | this->debounce_count= THEKERNEL->config->value(endstop_debounce_count_checksum)->by_default(100)->as_number(); | |
365 | ||
718bfc9c JM |
366 | this->is_corexy= THEKERNEL->config->value(corexy_homing_checksum)->by_default(false)->as_bool(); |
367 | this->is_delta= THEKERNEL->config->value(delta_homing_checksum)->by_default(false)->as_bool(); | |
368 | this->is_rdelta= THEKERNEL->config->value(rdelta_homing_checksum)->by_default(false)->as_bool(); | |
369 | this->is_scara= THEKERNEL->config->value(scara_homing_checksum)->by_default(false)->as_bool(); | |
f29b0272 | 370 | |
718bfc9c | 371 | this->home_z_first= THEKERNEL->config->value(home_z_first_checksum)->by_default(false)->as_bool(); |
abf706e6 | 372 | |
d2eef91b JM |
373 | this->trim_mm[0] = THEKERNEL->config->value(alpha_trim_checksum)->by_default(0)->as_number(); |
374 | this->trim_mm[1] = THEKERNEL->config->value(beta_trim_checksum)->by_default(0)->as_number(); | |
375 | this->trim_mm[2] = THEKERNEL->config->value(gamma_trim_checksum)->by_default(0)->as_number(); | |
374d0777 | 376 | |
5504a603 | 377 | // see if an order has been specified, must be three or more characters, XYZABC or ABYXZ etc |
e714bd32 JM |
378 | string order = THEKERNEL->config->value(homing_order_checksum)->by_default("")->as_string(); |
379 | this->homing_order = 0; | |
5504a603 | 380 | if(order.size() >= 3 && order.size() <= homing_axis.size() && !(this->is_delta || this->is_rdelta)) { |
e714bd32 | 381 | int shift = 0; |
80605954 | 382 | for(auto c : order) { |
b66cd8c9 JM |
383 | char n= toupper(c); |
384 | uint32_t i = n >= 'X' ? n - 'X' : n - 'A' + 3; | |
58c2948c | 385 | i += 1; // So X is 1 |
5504a603 | 386 | if(i > 6) { // bad value |
e714bd32 | 387 | this->homing_order = 0; |
80605954 JM |
388 | break; |
389 | } | |
390 | homing_order |= (i << shift); | |
5504a603 | 391 | shift += 3; |
80605954 JM |
392 | } |
393 | } | |
394 | ||
c8bac202 | 395 | // set to true by default for deltas due to trim, false on cartesians |
e714bd32 | 396 | this->move_to_origin_after_home = THEKERNEL->config->value(move_to_origin_checksum)->by_default(is_delta)->as_bool(); |
3c947f85 JM |
397 | } |
398 | ||
d2eef91b | 399 | bool Endstops::debounced_get(Pin *pin) |
28166daf | 400 | { |
d2eef91b | 401 | if(pin == nullptr) return false; |
e714bd32 | 402 | uint8_t debounce = 0; |
d2eef91b | 403 | while(pin->get()) { |
28166daf JM |
404 | if ( ++debounce >= this->debounce_count ) { |
405 | // pin triggered | |
406 | return true; | |
407 | } | |
408 | } | |
409 | return false; | |
410 | } | |
411 | ||
718bfc9c | 412 | // only called if limits are enabled |
3c947f85 JM |
413 | void Endstops::on_idle(void *argument) |
414 | { | |
c3d7feb6 JM |
415 | if(this->status == LIMIT_TRIGGERED) { |
416 | // if we were in limit triggered see if it has been cleared | |
718bfc9c | 417 | for(auto& i : endstops) { |
d2eef91b JM |
418 | if(i->limit_enable) { |
419 | if(i->pin.get()) { | |
718bfc9c | 420 | // still triggered, so exit |
d2eef91b | 421 | i->debounce = 0; |
718bfc9c JM |
422 | return; |
423 | } | |
424 | ||
7d9e5765 | 425 | if(i->debounce++ > debounce_count) { // can use less as it calls on_idle in between |
718bfc9c JM |
426 | // clear the state |
427 | this->status = NOT_HOMING; | |
c3d7feb6 JM |
428 | } |
429 | } | |
430 | } | |
4befe777 | 431 | return; |
c3d7feb6 | 432 | |
e714bd32 | 433 | } else if(this->status != NOT_HOMING) { |
c3d7feb6 JM |
434 | // don't check while homing |
435 | return; | |
436 | } | |
3c947f85 | 437 | |
718bfc9c | 438 | for(auto& i : endstops) { |
d2eef91b | 439 | if(i->limit_enable && STEPPER[i->axis_index]->is_moving()) { |
3c947f85 | 440 | // check min and max endstops |
d2eef91b | 441 | if(debounced_get(&i->pin)) { |
718bfc9c | 442 | // endstop triggered |
39c0196b JM |
443 | if(!THEKERNEL->is_grbl_mode()) { |
444 | THEKERNEL->streams->printf("Limit switch %c was hit - reset or M999 required\n", i->axis); | |
445 | }else{ | |
446 | THEKERNEL->streams->printf("ALARM: Hard limit %c\n", i->axis); | |
447 | } | |
718bfc9c | 448 | this->status = LIMIT_TRIGGERED; |
d2eef91b | 449 | i->debounce= 0; |
718bfc9c JM |
450 | // disables heaters and motors, ignores incoming Gcode and flushes block queue |
451 | THEKERNEL->call_event(ON_HALT, nullptr); | |
452 | return; | |
3c947f85 JM |
453 | } |
454 | } | |
455 | } | |
456 | } | |
457 | ||
458 | // if limit switches are enabled, then we must move off of the endstop otherwise we won't be able to move | |
5bfcd44a | 459 | // checks if triggered and only backs off if triggered |
718bfc9c | 460 | void Endstops::back_off_home(axis_bitmap_t axis) |
3c947f85 | 461 | { |
e714bd32 | 462 | std::vector<std::pair<char, float>> params; |
3c947f85 | 463 | this->status = BACK_OFF_HOME; |
7f3e6350 | 464 | |
718bfc9c JM |
465 | float slow_rate= NAN; // default mm/sec |
466 | ||
7f3e6350 | 467 | // these are handled differently |
1a6870b3 | 468 | if(is_delta) { |
7f3e6350 | 469 | // Move off of the endstop using a regular relative move in Z only |
d2eef91b JM |
470 | params.push_back({'Z', homing_axis[Z_AXIS].retract * (homing_axis[Z_AXIS].home_direction ? 1 : -1)}); |
471 | slow_rate= homing_axis[Z_AXIS].slow_rate; | |
44127aca | 472 | |
e714bd32 | 473 | } else { |
7f3e6350 | 474 | // cartesians, concatenate all the moves we need to do into one gcode |
d2eef91b JM |
475 | for( auto& e : homing_axis) { |
476 | if(!axis[e.axis_index]) continue; // only for axes we asked to move | |
7f3e6350 JM |
477 | |
478 | // if not triggered no need to move off | |
d2eef91b JM |
479 | if(e.pin_info != nullptr && e.pin_info->limit_enable && debounced_get(&e.pin_info->pin)) { |
480 | char ax= e.axis; | |
481 | params.push_back({ax, e.retract * (e.home_direction ? 1 : -1)}); | |
718bfc9c | 482 | // select slowest of them all |
d2eef91b | 483 | slow_rate= isnan(slow_rate) ? e.slow_rate : std::min(slow_rate, e.slow_rate); |
44127aca | 484 | } |
3c947f85 JM |
485 | } |
486 | } | |
7f3e6350 JM |
487 | |
488 | if(!params.empty()) { | |
489 | // Move off of the endstop using a regular relative move | |
7c801094 | 490 | params.insert(params.begin(), {'G', 0}); |
addf2490 | 491 | // use X slow rate to move, Z should have a max speed set anyway |
718bfc9c | 492 | params.push_back({'F', slow_rate * 60.0F}); |
7c801094 JM |
493 | char gcode_buf[64]; |
494 | append_parameters(gcode_buf, params, sizeof(gcode_buf)); | |
7f3e6350 | 495 | Gcode gc(gcode_buf, &(StreamOutput::NullStream)); |
1d323f9a JM |
496 | THEROBOT->push_state(); |
497 | THEROBOT->inch_mode = false; // needs to be in mm | |
498 | THEROBOT->absolute_mode = false; // needs to be relative mode | |
499 | THEROBOT->on_gcode_received(&gc); // send to robot directly | |
7f3e6350 | 500 | // Wait for above to finish |
04782655 | 501 | THECONVEYOR->wait_for_idle(); |
c8bac202 | 502 | THEROBOT->pop_state(); |
7f3e6350 JM |
503 | } |
504 | ||
3c947f85 | 505 | this->status = NOT_HOMING; |
a0e0d592 BG |
506 | } |
507 | ||
2b3cedc7 | 508 | // If enabled will move the head to 0,0 after homing, but only if X and Y were set to home |
718bfc9c | 509 | void Endstops::move_to_origin(axis_bitmap_t axis) |
2b3cedc7 | 510 | { |
1d323f9a | 511 | if(!is_delta && (!axis[X_AXIS] || !axis[Y_AXIS])) return; // ignore if X and Y not homing, unless delta |
2b3cedc7 | 512 | |
fafb45df | 513 | // Do we need to check if we are already at 0,0? probably not as the G0 will not do anything if we are |
c8bac202 | 514 | // float pos[3]; THEROBOT->get_axis_position(pos); if(pos[0] == 0 && pos[1] == 0) return; |
fafb45df | 515 | |
2ddf75fd | 516 | this->status = MOVE_TO_ORIGIN; |
2b3cedc7 | 517 | // Move to center using a regular move, use slower of X and Y fast rate |
718bfc9c | 518 | //float rate = std::min(this->fast_rates[0], this->fast_rates[1]) * 60.0F; |
2b3cedc7 | 519 | char buf[32]; |
1d323f9a JM |
520 | THEROBOT->push_state(); |
521 | THEROBOT->inch_mode = false; // needs to be in mm | |
5209dd4d | 522 | THEROBOT->absolute_mode = true; |
718bfc9c JM |
523 | //snprintf(buf, sizeof(buf), "G53 G0 X0 Y0 F%1.4f", rate); // must use machine coordinates in case G92 or WCS is in effect |
524 | snprintf(buf, sizeof(buf), "G53 G0 X0 Y0"); // must use machine coordinates in case G92 or WCS is in effect | |
e551657a JM |
525 | struct SerialMessage message; |
526 | message.message = buf; | |
527 | message.stream = &(StreamOutput::NullStream); | |
528 | THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message ); // as it is a multi G code command | |
2b3cedc7 | 529 | // Wait for above to finish |
04782655 | 530 | THECONVEYOR->wait_for_idle(); |
c8bac202 | 531 | THEROBOT->pop_state(); |
2b3cedc7 JM |
532 | this->status = NOT_HOMING; |
533 | } | |
534 | ||
c8bac202 JM |
535 | // Called every millisecond in an ISR |
536 | uint32_t Endstops::read_endstops(uint32_t dummy) | |
33e4cc02 | 537 | { |
98e30679 | 538 | if(this->status != MOVING_TO_ENDSTOP_SLOW && this->status != MOVING_TO_ENDSTOP_FAST) return 0; // not doing anything we need to monitor for |
798295c1 | 539 | |
55bb5165 JM |
540 | // check each homing endstop |
541 | for(auto& e : homing_axis) { // check all axis homing endstops | |
542 | if(e.pin_info == nullptr) continue; // ignore if not a homing endstop | |
543 | int m= e.axis_index; | |
c8bac202 | 544 | |
b84bd559 JM |
545 | // for corexy homing in X or Y we must only check the associated endstop, works as we only home one axis at a time for corexy |
546 | if(is_corexy && (m == X_AXIS || m == Y_AXIS) && !axis_to_home[m]) continue; | |
798295c1 | 547 | |
55bb5165 JM |
548 | if(STEPPER[m]->is_moving()) { |
549 | // if it is moving then we check the associated endstop, and debounce it | |
550 | if(e.pin_info->pin.get()) { | |
551 | if(e.pin_info->debounce < debounce_ms) { | |
552 | e.pin_info->debounce++; | |
c8bac202 | 553 | |
33e4cc02 | 554 | } else { |
55bb5165 | 555 | if(is_corexy && (m == X_AXIS || m == Y_AXIS)) { |
b84bd559 | 556 | // corexy when moving in X or Y we need to stop both the X and Y motors |
c8bac202 JM |
557 | STEPPER[X_AXIS]->stop_moving(); |
558 | STEPPER[Y_AXIS]->stop_moving(); | |
f29b0272 | 559 | |
55bb5165 JM |
560 | }else{ |
561 | // we signal the motor to stop, which will preempt any moves on that axis | |
562 | STEPPER[m]->stop_moving(); | |
563 | } | |
39c0196b | 564 | e.pin_info->triggered= true; |
c8bac202 | 565 | } |
55bb5165 JM |
566 | |
567 | } else { | |
568 | // The endstop was not hit yet | |
569 | e.pin_info->debounce= 0; | |
f29b0272 JM |
570 | } |
571 | } | |
572 | } | |
573 | ||
c8bac202 | 574 | return 0; |
3db88866 JM |
575 | } |
576 | ||
374d0777 | 577 | void Endstops::home_xy() |
33e4cc02 | 578 | { |
c8bac202 JM |
579 | if(axis_to_home[X_AXIS] && axis_to_home[Y_AXIS]) { |
580 | // Home XY first so as not to slow them down by homing Z at the same time | |
d2eef91b JM |
581 | float delta[3] {homing_axis[X_AXIS].max_travel, homing_axis[Y_AXIS].max_travel, 0}; |
582 | if(homing_axis[X_AXIS].home_direction) delta[X_AXIS]= -delta[X_AXIS]; | |
583 | if(homing_axis[Y_AXIS].home_direction) delta[Y_AXIS]= -delta[Y_AXIS]; | |
584 | float feed_rate = std::min(homing_axis[X_AXIS].fast_rate, homing_axis[Y_AXIS].fast_rate); | |
121094a5 | 585 | THEROBOT->delta_move(delta, feed_rate, 3); |
c8bac202 | 586 | |
c8bac202 JM |
587 | } else if(axis_to_home[X_AXIS]) { |
588 | // now home X only | |
d2eef91b JM |
589 | float delta[3] {homing_axis[X_AXIS].max_travel, 0, 0}; |
590 | if(homing_axis[X_AXIS].home_direction) delta[X_AXIS]= -delta[X_AXIS]; | |
591 | THEROBOT->delta_move(delta, homing_axis[X_AXIS].fast_rate, 3); | |
c8bac202 JM |
592 | |
593 | } else if(axis_to_home[Y_AXIS]) { | |
594 | // now home Y only | |
d2eef91b JM |
595 | float delta[3] {0, homing_axis[Y_AXIS].max_travel, 0}; |
596 | if(homing_axis[Y_AXIS].home_direction) delta[Y_AXIS]= -delta[Y_AXIS]; | |
597 | THEROBOT->delta_move(delta, homing_axis[Y_AXIS].fast_rate, 3); | |
c8bac202 | 598 | } |
2441ba94 JM |
599 | |
600 | // Wait for axis to have homed | |
601 | THECONVEYOR->wait_for_idle(); | |
374d0777 JM |
602 | } |
603 | ||
718bfc9c | 604 | void Endstops::home(axis_bitmap_t a) |
374d0777 | 605 | { |
718bfc9c JM |
606 | // reset debounce counts for all endstops |
607 | for(auto& e : endstops) { | |
d2eef91b | 608 | e->debounce= 0; |
39c0196b | 609 | e->triggered= false; |
718bfc9c | 610 | } |
c1ebb1fe | 611 | |
dab818ef | 612 | if (is_scara) { |
2ab02d2d | 613 | THEROBOT->disable_arm_solution = true; // Polar bots has to home in the actuator space. Arm solution disabled. |
dab818ef | 614 | } |
2ab02d2d | 615 | |
374d0777 JM |
616 | this->axis_to_home= a; |
617 | ||
618 | // Start moving the axes to the origin | |
619 | this->status = MOVING_TO_ENDSTOP_FAST; | |
620 | ||
621 | THEROBOT->disable_segmentation= true; // we must disable segmentation as this won't work with it enabled | |
622 | ||
623 | if(!home_z_first) home_xy(); | |
c8bac202 JM |
624 | |
625 | if(axis_to_home[Z_AXIS]) { | |
626 | // now home z | |
d2eef91b JM |
627 | float delta[3] {0, 0, homing_axis[Z_AXIS].max_travel}; // we go the max z |
628 | if(homing_axis[Z_AXIS].home_direction) delta[Z_AXIS]= -delta[Z_AXIS]; | |
629 | THEROBOT->delta_move(delta, homing_axis[Z_AXIS].fast_rate, 3); | |
c8bac202 | 630 | // wait for Z |
04782655 | 631 | THECONVEYOR->wait_for_idle(); |
c8bac202 JM |
632 | } |
633 | ||
374d0777 JM |
634 | if(home_z_first) home_xy(); |
635 | ||
4ebf87af JM |
636 | // potentially home A B and C individually |
637 | if(homing_axis.size() > 3){ | |
7b18a698 | 638 | for (size_t i = A_AXIS; i < homing_axis.size(); ++i) { |
4ebf87af JM |
639 | if(axis_to_home[i]) { |
640 | // now home A B or C | |
641 | float delta[i+1]; | |
642 | for (size_t j = 0; j <= i; ++j) delta[j]= 0; | |
643 | delta[i]= homing_axis[i].max_travel; // we go the max | |
644 | if(homing_axis[i].home_direction) delta[i]= -delta[i]; | |
645 | THEROBOT->delta_move(delta, homing_axis[i].fast_rate, i+1); | |
646 | // wait for it | |
647 | THECONVEYOR->wait_for_idle(); | |
648 | } | |
649 | } | |
eea58a01 JM |
650 | } |
651 | ||
39c0196b JM |
652 | // check that the endstops were hit and it did not stop short for some reason |
653 | // if the endstop is not triggered then enter ALARM state | |
654 | // with deltas we check all three axis were triggered | |
655 | for (size_t i = X_AXIS; i <= Z_AXIS; ++i) { | |
656 | if((axis_to_home[i] || this->is_delta || this->is_rdelta) && !homing_axis[i].pin_info->triggered) { | |
657 | this->status = NOT_HOMING; | |
658 | THEKERNEL->call_event(ON_HALT, nullptr); | |
659 | return; | |
660 | } | |
661 | } | |
662 | ||
663 | // also check ABC | |
664 | if(homing_axis.size() > 3){ | |
665 | for (size_t i = A_AXIS; i < homing_axis.size(); ++i) { | |
666 | if(axis_to_home[i] && !homing_axis[i].pin_info->triggered) { | |
667 | this->status = NOT_HOMING; | |
668 | THEKERNEL->call_event(ON_HALT, nullptr); | |
669 | return; | |
670 | } | |
671 | } | |
672 | } | |
eea58a01 | 673 | |
dab818ef | 674 | if (!is_scara) { |
39c0196b JM |
675 | // Only for non polar bots |
676 | // we did not complete movement the full distance if we hit the endstops | |
677 | // TODO Maybe only reset axis involved in the homing cycle | |
2ab02d2d | 678 | THEROBOT->reset_position_from_current_actuator_position(); |
dab818ef | 679 | } |
95bb7f04 | 680 | |
c8bac202 | 681 | // Move back a small distance for all homing axis |
33e4cc02 | 682 | this->status = MOVING_BACK; |
d2eef91b | 683 | float delta[homing_axis.size()]; |
4ebf87af JM |
684 | for (size_t i = 0; i < homing_axis.size(); ++i) delta[i]= 0; |
685 | ||
d2eef91b JM |
686 | // use minimum feed rate of all axes that are being homed (sub optimal, but necessary) |
687 | float feed_rate= homing_axis[X_AXIS].slow_rate; | |
688 | for (auto& i : homing_axis) { | |
689 | int c= i.axis_index; | |
c8bac202 | 690 | if(axis_to_home[c]) { |
d2eef91b JM |
691 | delta[c]= i.retract; |
692 | if(!i.home_direction) delta[c]= -delta[c]; | |
693 | feed_rate= std::min(i.slow_rate, feed_rate); | |
174d9961 JM |
694 | } |
695 | } | |
33e4cc02 | 696 | |
d2eef91b | 697 | THEROBOT->delta_move(delta, feed_rate, homing_axis.size()); |
c8bac202 | 698 | // wait until finished |
04782655 | 699 | THECONVEYOR->wait_for_idle(); |
f29b0272 | 700 | |
04782655 | 701 | // Start moving the axes towards the endstops slowly |
c8bac202 | 702 | this->status = MOVING_TO_ENDSTOP_SLOW; |
d2eef91b JM |
703 | for (auto& i : homing_axis) { |
704 | int c= i.axis_index; | |
98e30679 | 705 | if(axis_to_home[c]) { |
d2eef91b JM |
706 | delta[c]= i.retract*2; // move further than we moved off to make sure we hit it cleanly |
707 | if(i.home_direction) delta[c]= -delta[c]; | |
98e30679 JM |
708 | }else{ |
709 | delta[c]= 0; | |
710 | } | |
81f02e89 | 711 | } |
d2eef91b | 712 | THEROBOT->delta_move(delta, feed_rate, homing_axis.size()); |
c8bac202 | 713 | // wait until finished |
04782655 JM |
714 | THECONVEYOR->wait_for_idle(); |
715 | ||
04782655 | 716 | // we did not complete movement the full distance if we hit the endstops |
b6187406 | 717 | // TODO Maybe only reset axis involved in the homing cycle |
04782655 | 718 | THEROBOT->reset_position_from_current_actuator_position(); |
0058d8d4 | 719 | |
04782655 | 720 | THEROBOT->disable_segmentation= false; |
dab818ef | 721 | if (is_scara) { |
2ab02d2d | 722 | THEROBOT->disable_arm_solution = false; // Arm solution enabled again. |
dab818ef | 723 | } |
6d142b73 | 724 | |
ca287785 | 725 | this->status = NOT_HOMING; |
81f02e89 JM |
726 | } |
727 | ||
a2f1ce04 | 728 | void Endstops::process_home_command(Gcode* gcode) |
c339d634 | 729 | { |
a2f1ce04 | 730 | // First wait for the queue to be empty |
04782655 | 731 | THECONVEYOR->wait_for_idle(); |
7484e84a | 732 | |
b84bd559 JM |
733 | // turn off any compensation transform so Z does not move as XY home |
734 | auto savect= THEROBOT->compensationTransform; | |
735 | THEROBOT->compensationTransform= nullptr; | |
736 | ||
3811aa80 JM |
737 | // deltas always home Z axis only, which moves all three actuators |
738 | bool home_in_z = this->is_delta || this->is_rdelta; | |
6ded1a99 | 739 | |
95bb7f04 | 740 | // figure out which axis to home |
bab1e318 | 741 | axis_bitmap_t haxis; |
ceeb9155 JM |
742 | haxis.reset(); |
743 | ||
a35788ee | 744 | if(!home_in_z) { // ie not a delta |
eea58a01 JM |
745 | bool axis_speced = (gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z') || |
746 | gcode->has_letter('A') || gcode->has_letter('B') || gcode->has_letter('C')); | |
bab1e318 JM |
747 | |
748 | for (auto &p : homing_axis) { | |
749 | // only enable homing if the endstop is defined, | |
750 | if(p.pin_info == nullptr) continue; | |
751 | if(!axis_speced || gcode->has_letter(p.axis)) { | |
752 | haxis.set(p.axis_index); | |
8cc5173e | 753 | // now reset axis to 0 as we do not know what state we are in |
dab818ef | 754 | if (!is_scara) { |
2ab02d2d | 755 | THEROBOT->reset_axis_position(0, p.axis_index); |
19ae0137 | 756 | } else { |
2ab02d2d QH |
757 | // SCARA resets arms to plausable minimum angles |
758 | THEROBOT->reset_axis_position(-30,30,0); // angles set into axis space for homing. | |
759 | } | |
6ded1a99 | 760 | } |
e714bd32 | 761 | } |
6ded1a99 | 762 | |
c8bac202 | 763 | } else { |
a35788ee | 764 | // Only Z axis homes (even though all actuators move this is handled by arm solution) |
ceeb9155 | 765 | haxis.set(Z_AXIS); |
1c658603 JM |
766 | // we also set the kinematics to a known good position, this is necessary for a rotary delta, but doesn't hurt for linear delta |
767 | THEROBOT->reset_axis_position(0, 0, 0); | |
a2f1ce04 | 768 | } |
e714bd32 | 769 | |
a2f1ce04 | 770 | // do the actual homing |
6c9bf464 | 771 | if(homing_order != 0 && !is_scara) { |
a2f1ce04 | 772 | // if an order has been specified do it in the specified order |
58c2948c JM |
773 | // homing order is 0bfffeeedddcccbbbaaa where aaa is 1,2,3,4,5,6 to specify the first axis (XYZABC), bbb is the second and ccc is the third etc |
774 | // eg 0b0101011001010 would be Y X Z A, 011 010 001 100 101 would be B A X Y Z | |
58c9f7b3 | 775 | for (uint32_t m = homing_order; m != 0; m >>= 3) { |
58c2948c | 776 | uint32_t a= (m & 0x07)-1; // axis to home |
3c74260c | 777 | if(a < homing_axis.size() && haxis[a]) { // if axis is selected to home |
718bfc9c | 778 | axis_bitmap_t bs; |
ceeb9155 JM |
779 | bs.set(a); |
780 | home(bs); | |
07186543 | 781 | } |
a2f1ce04 JM |
782 | // check if on_halt (eg kill) |
783 | if(THEKERNEL->is_halted()) break; | |
e714bd32 | 784 | } |
c8bac202 JM |
785 | |
786 | } else if(is_corexy) { | |
787 | // corexy must home each axis individually | |
bab1e318 JM |
788 | for (auto &p : homing_axis) { |
789 | if(haxis[p.axis_index]) { | |
718bfc9c | 790 | axis_bitmap_t bs; |
bab1e318 | 791 | bs.set(p.axis_index); |
ceeb9155 | 792 | home(bs); |
c8bac202 | 793 | } |
b84bd559 JM |
794 | // check if on_halt (eg kill) |
795 | if(THEKERNEL->is_halted()) break; | |
c8bac202 | 796 | } |
798295c1 | 797 | |
a2f1ce04 | 798 | } else { |
c8bac202 | 799 | // they could all home at the same time |
ceeb9155 | 800 | home(haxis); |
a2f1ce04 | 801 | } |
e714bd32 | 802 | |
b84bd559 JM |
803 | // restore compensationTransform |
804 | THEROBOT->compensationTransform= savect; | |
805 | ||
39c0196b | 806 | // check if on_halt (eg kill or fail) |
a2f1ce04 JM |
807 | if(THEKERNEL->is_halted()) { |
808 | if(!THEKERNEL->is_grbl_mode()) { | |
39c0196b JM |
809 | THEKERNEL->streams->printf("ERROR: Homing cycle failed\n"); |
810 | }else{ | |
811 | THEKERNEL->streams->printf("ALARM: Homing fail\n"); | |
a2f1ce04 | 812 | } |
bab1e318 JM |
813 | // clear all the homed flags |
814 | for (auto &p : homing_axis) p.homed= false; | |
a2f1ce04 JM |
815 | return; |
816 | } | |
e714bd32 | 817 | |
2ab02d2d | 818 | if(home_in_z || is_scara) { // deltas and scaras only |
a2f1ce04 JM |
819 | // Here's where we would have been if the endstops were perfectly trimmed |
820 | // NOTE on a rotary delta home_offset is actuator position in degrees when homed and | |
821 | // home_offset is the theta offset for each actuator, so M206 is used to set theta offset for each actuator in degrees | |
8fe38353 | 822 | // FIXME not sure this will work with compensation transforms on. |
a2f1ce04 | 823 | float ideal_position[3] = { |
bab1e318 JM |
824 | homing_axis[X_AXIS].homing_position + homing_axis[X_AXIS].home_offset, |
825 | homing_axis[Y_AXIS].homing_position + homing_axis[Y_AXIS].home_offset, | |
826 | homing_axis[Z_AXIS].homing_position + homing_axis[Z_AXIS].home_offset | |
a2f1ce04 | 827 | }; |
7552475b | 828 | |
2ab02d2d | 829 | bool has_endstop_trim = this->is_delta || is_scara; |
a2f1ce04 JM |
830 | if (has_endstop_trim) { |
831 | ActuatorCoordinates ideal_actuator_position; | |
c8bac202 | 832 | THEROBOT->arm_solution->cartesian_to_actuator(ideal_position, ideal_actuator_position); |
42bbc035 | 833 | |
a2f1ce04 JM |
834 | // We are actually not at the ideal position, but a trim away |
835 | ActuatorCoordinates real_actuator_position = { | |
836 | ideal_actuator_position[X_AXIS] - this->trim_mm[X_AXIS], | |
837 | ideal_actuator_position[Y_AXIS] - this->trim_mm[Y_AXIS], | |
838 | ideal_actuator_position[Z_AXIS] - this->trim_mm[Z_AXIS] | |
839 | }; | |
840 | ||
841 | float real_position[3]; | |
c8bac202 | 842 | THEROBOT->arm_solution->actuator_to_cartesian(real_actuator_position, real_position); |
b6187406 | 843 | // Reset the actuator positions to correspond to our real position |
c8bac202 | 844 | THEROBOT->reset_axis_position(real_position[0], real_position[1], real_position[2]); |
42bbc035 | 845 | |
e714bd32 | 846 | } else { |
a2f1ce04 JM |
847 | // without endstop trim, real_position == ideal_position |
848 | if(is_rdelta) { | |
849 | // with a rotary delta we set the actuators angle then use the FK to calculate the resulting cartesian coordinates | |
93f20a8c | 850 | ActuatorCoordinates real_actuator_position = {ideal_position[0], ideal_position[1], ideal_position[2]}; |
c8bac202 | 851 | THEROBOT->reset_actuator_position(real_actuator_position); |
a2f1ce04 JM |
852 | |
853 | } else { | |
b6187406 | 854 | // Reset the actuator positions to correspond to our real position |
c8bac202 | 855 | THEROBOT->reset_axis_position(ideal_position[0], ideal_position[1], ideal_position[2]); |
3ffe27fb | 856 | } |
e714bd32 | 857 | } |
3c947f85 | 858 | |
bab1e318 JM |
859 | // for deltas we say all axis are homed even though it was only Z |
860 | for (auto &p : homing_axis) p.homed= true; | |
2b925443 | 861 | |
a2f1ce04 JM |
862 | } else { |
863 | // Zero the ax(i/e)s position, add in the home offset | |
fd2341bc JM |
864 | // NOTE that if compensation is active the Z will be set based on where XY are, so make sure XY are homed first then Z |
865 | // so XY are at a known consistent position. (especially true if using a proximity probe) | |
bab1e318 JM |
866 | for (auto &p : homing_axis) { |
867 | if (haxis[p.axis_index]) { // if we requested this axis to home | |
868 | THEROBOT->reset_axis_position(p.homing_position + p.home_offset, p.axis_index); | |
2b925443 | 869 | // set flag indicating axis was homed, it stays set once set until H/W reset or unhomed |
bab1e318 | 870 | p.homed= true; |
a2f1ce04 | 871 | } |
c339d634 | 872 | } |
e714bd32 | 873 | } |
81f02e89 | 874 | |
a2f1ce04 JM |
875 | // on some systems where 0,0 is bed center it is nice to have home goto 0,0 after homing |
876 | // default is off for cartesian on for deltas | |
877 | if(!is_delta) { | |
878 | // NOTE a rotary delta usually has optical or hall-effect endstops so it is safe to go past them a little bit | |
1d323f9a | 879 | if(this->move_to_origin_after_home) move_to_origin(haxis); |
a2f1ce04 | 880 | // if limit switches are enabled we must back off endstop after setting home |
98e30679 | 881 | back_off_home(haxis); |
a2f1ce04 | 882 | |
bab1e318 | 883 | } else if(this->move_to_origin_after_home || homing_axis[X_AXIS].pin_info->limit_enable) { |
a2f1ce04 JM |
884 | // deltas are not left at 0,0 because of the trim settings, so move to 0,0 if requested, but we need to back off endstops first |
885 | // also need to back off endstops if limits are enabled | |
98e30679 | 886 | back_off_home(haxis); |
1d323f9a | 887 | if(this->move_to_origin_after_home) move_to_origin(haxis); |
a2f1ce04 JM |
888 | } |
889 | } | |
890 | ||
078f76e0 JM |
891 | void Endstops::set_homing_offset(Gcode *gcode) |
892 | { | |
bab1e318 | 893 | // M306 Similar to M206 but sets Homing offsets based on current MCS position |
081889b9 JM |
894 | // Basically it finds the delta between the current MCS position and the requested position and adds it to the homing offset |
895 | // then will not let it be set again until that axis is homed. | |
896 | float pos[3]; | |
897 | THEROBOT->get_axis_position(pos); | |
cb082011 | 898 | |
7492a02e | 899 | if (gcode->has_letter('X')) { |
bab1e318 | 900 | if(!homing_axis[X_AXIS].homed) { |
fdfa00d2 JM |
901 | gcode->stream->printf("error: Axis X must be homed before setting Homing offset\n"); |
902 | return; | |
903 | } | |
bab1e318 JM |
904 | homing_axis[X_AXIS].home_offset += (THEROBOT->to_millimeters(gcode->get_value('X')) - pos[X_AXIS]); |
905 | homing_axis[X_AXIS].homed= false; // force it to be homed | |
078f76e0 | 906 | } |
7492a02e | 907 | if (gcode->has_letter('Y')) { |
bab1e318 | 908 | if(!homing_axis[Y_AXIS].homed) { |
fdfa00d2 JM |
909 | gcode->stream->printf("error: Axis Y must be homed before setting Homing offset\n"); |
910 | return; | |
911 | } | |
bab1e318 JM |
912 | homing_axis[Y_AXIS].home_offset += (THEROBOT->to_millimeters(gcode->get_value('Y')) - pos[Y_AXIS]); |
913 | homing_axis[Y_AXIS].homed= false; // force it to be homed | |
7492a02e JM |
914 | } |
915 | if (gcode->has_letter('Z')) { | |
bab1e318 | 916 | if(!homing_axis[Z_AXIS].homed) { |
fdfa00d2 JM |
917 | gcode->stream->printf("error: Axis Z must be homed before setting Homing offset\n"); |
918 | return; | |
919 | } | |
bab1e318 JM |
920 | homing_axis[Z_AXIS].home_offset += (THEROBOT->to_millimeters(gcode->get_value('Z')) - pos[Z_AXIS]); |
921 | homing_axis[Z_AXIS].homed= false; // force it to be homed | |
7492a02e JM |
922 | } |
923 | ||
bab1e318 JM |
924 | gcode->stream->printf("Homing Offset: X %5.3f Y %5.3f Z %5.3f will take effect next home\n", homing_axis[X_AXIS].home_offset, homing_axis[Y_AXIS].home_offset, homing_axis[Z_AXIS].home_offset); |
925 | } | |
926 | ||
927 | void Endstops::handle_park(Gcode * gcode) | |
928 | { | |
7b18a698 | 929 | // TODO: spec says if XYZ specified move to them first then move to MCS of specifed axis |
bab1e318 JM |
930 | THEROBOT->push_state(); |
931 | THEROBOT->inch_mode = false; // needs to be in mm | |
932 | THEROBOT->absolute_mode = true; | |
933 | char buf[32]; | |
934 | snprintf(buf, sizeof(buf), "G53 G0 X%f Y%f", saved_position[X_AXIS], saved_position[Y_AXIS]); // must use machine coordinates in case G92 or WCS is in effect | |
935 | struct SerialMessage message; | |
936 | message.message = buf; | |
937 | message.stream = &(StreamOutput::NullStream); | |
938 | THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message ); // as it is a multi G code command | |
939 | // Wait for above to finish | |
940 | THECONVEYOR->wait_for_idle(); | |
941 | THEROBOT->pop_state(); | |
078f76e0 JM |
942 | } |
943 | ||
bab1e318 | 944 | // parse gcodes |
a2f1ce04 JM |
945 | void Endstops::on_gcode_received(void *argument) |
946 | { | |
947 | Gcode *gcode = static_cast<Gcode *>(argument); | |
bab1e318 | 948 | |
a2f1ce04 | 949 | if ( gcode->has_g && gcode->g == 28) { |
bab1e318 JM |
950 | switch(gcode->subcode) { |
951 | case 0: // G28 in grbl mode will do a rapid to the predefined position otherwise it is home command | |
952 | if(THEKERNEL->is_grbl_mode()){ | |
953 | handle_park(gcode); | |
954 | }else{ | |
955 | process_home_command(gcode); | |
956 | } | |
957 | break; | |
958 | ||
959 | case 1: // G28.1 set pre defined park position | |
960 | // saves current position in absolute machine coordinates | |
961 | THEROBOT->get_axis_position(saved_position); // Only XY are used | |
962 | // Note the following is only meant to be used for recovering a saved position from config-override | |
963 | // Not a standard Gcode and not to be relied on | |
964 | if (gcode->has_letter('X')) saved_position[X_AXIS] = gcode->get_value('X'); | |
965 | if (gcode->has_letter('Y')) saved_position[Y_AXIS] = gcode->get_value('Y'); | |
966 | break; | |
967 | ||
968 | case 2: // G28.2 in grbl mode does homing (triggered by $H), otherwise it moves to the park position | |
969 | if(THEKERNEL->is_grbl_mode()) { | |
970 | process_home_command(gcode); | |
971 | }else{ | |
972 | handle_park(gcode); | |
973 | } | |
974 | break; | |
975 | ||
976 | case 3: // G28.3 is a smoothie special it sets manual homing | |
977 | if(gcode->get_num_args() == 0) { | |
7d9e5765 JM |
978 | for (auto &p : homing_axis) { |
979 | p.homed= true; | |
980 | THEROBOT->reset_axis_position(0, p.axis_index); | |
981 | } | |
bab1e318 JM |
982 | } else { |
983 | // do a manual homing based on given coordinates, no endstops required | |
984 | if(gcode->has_letter('X')){ THEROBOT->reset_axis_position(gcode->get_value('X'), X_AXIS); homing_axis[X_AXIS].homed= true; } | |
985 | if(gcode->has_letter('Y')){ THEROBOT->reset_axis_position(gcode->get_value('Y'), Y_AXIS); homing_axis[Y_AXIS].homed= true; } | |
986 | if(gcode->has_letter('Z')){ THEROBOT->reset_axis_position(gcode->get_value('Z'), Z_AXIS); homing_axis[Z_AXIS].homed= true; } | |
7d9e5765 JM |
987 | if(homing_axis.size() > A_AXIS && gcode->has_letter('A')){ THEROBOT->reset_axis_position(gcode->get_value('A'), A_AXIS); homing_axis[A_AXIS].homed= true; } |
988 | if(homing_axis.size() > B_AXIS && gcode->has_letter('B')){ THEROBOT->reset_axis_position(gcode->get_value('B'), B_AXIS); homing_axis[B_AXIS].homed= true; } | |
989 | if(homing_axis.size() > C_AXIS && gcode->has_letter('C')){ THEROBOT->reset_axis_position(gcode->get_value('C'), C_AXIS); homing_axis[C_AXIS].homed= true; } | |
bab1e318 JM |
990 | } |
991 | break; | |
992 | ||
993 | case 4: { // G28.4 is a smoothie special it sets manual homing based on the actuator position (used for rotary delta) | |
994 | // do a manual homing based on given coordinates, no endstops required | |
995 | ActuatorCoordinates ac{NAN, NAN, NAN}; | |
996 | if(gcode->has_letter('X')){ ac[0] = gcode->get_value('X'); homing_axis[X_AXIS].homed= true; } | |
997 | if(gcode->has_letter('Y')){ ac[1] = gcode->get_value('Y'); homing_axis[Y_AXIS].homed= true; } | |
998 | if(gcode->has_letter('Z')){ ac[2] = gcode->get_value('Z'); homing_axis[Z_AXIS].homed= true; } | |
999 | THEROBOT->reset_actuator_position(ac); | |
1000 | } | |
1001 | break; | |
1002 | ||
1003 | case 5: // G28.5 is a smoothie special it clears the homed flag for the specified axis, or all if not specifed | |
1004 | if(gcode->get_num_args() == 0) { | |
1005 | for (auto &p : homing_axis) p.homed= false; | |
1006 | } else { | |
1007 | if(gcode->has_letter('X')) homing_axis[X_AXIS].homed= false; | |
1008 | if(gcode->has_letter('Y')) homing_axis[Y_AXIS].homed= false; | |
1009 | if(gcode->has_letter('Z')) homing_axis[Z_AXIS].homed= false; | |
7d9e5765 JM |
1010 | if(homing_axis.size() > A_AXIS && gcode->has_letter('A')) homing_axis[A_AXIS].homed= false; |
1011 | if(homing_axis.size() > B_AXIS && gcode->has_letter('B')) homing_axis[B_AXIS].homed= false; | |
1012 | if(homing_axis.size() > C_AXIS && gcode->has_letter('C')) homing_axis[C_AXIS].homed= false; | |
bab1e318 JM |
1013 | } |
1014 | break; | |
1015 | ||
1016 | case 6: // G28.6 is a smoothie special it shows the homing status of each axis | |
1017 | for (auto &p : homing_axis) { | |
1018 | gcode->stream->printf("%c:%d ", p.axis, p.homed); | |
1019 | } | |
1020 | gcode->add_nl= true; | |
1021 | break; | |
1022 | ||
1023 | default: | |
1024 | if(THEKERNEL->is_grbl_mode()) { | |
1025 | gcode->stream->printf("error:Unsupported command\n"); | |
1026 | } | |
1027 | break; | |
1028 | } | |
a2f1ce04 | 1029 | |
8b261cdc | 1030 | } else if (gcode->has_m) { |
a2f1ce04 | 1031 | |
33e4cc02 JM |
1032 | switch (gcode->m) { |
1033 | case 119: { | |
7d9e5765 JM |
1034 | for(auto& h : homing_axis) { |
1035 | string name; | |
1036 | name.append(1, h.axis).append(h.home_direction ? "_min" : "_max"); | |
1037 | gcode->stream->printf("%s:%d ", name.c_str(), h.pin_info->pin.get()); | |
1038 | } | |
1039 | gcode->stream->printf("pins- "); | |
bab1e318 | 1040 | for(auto& p : endstops) { |
49feaeb0 JM |
1041 | string str(1, p->axis); |
1042 | if(p->limit_enable) str.append("L"); | |
1043 | gcode->stream->printf("(%s)P%d.%d:%d ", str.c_str(), p->pin.port_number, p->pin.pin, p->pin.get()); | |
ef7bd372 | 1044 | } |
e714bd32 | 1045 | gcode->add_nl = true; |
33e4cc02 JM |
1046 | } |
1047 | break; | |
1048 | ||
1049 | case 206: // M206 - set homing offset | |
0c18b666 | 1050 | if(is_rdelta) return; // RotaryDeltaCalibration module will handle this |
bab1e318 JM |
1051 | for (auto &p : homing_axis) { |
1052 | if (gcode->has_letter(p.axis)) p.home_offset= gcode->get_value(p.axis); | |
1053 | } | |
932a3995 | 1054 | |
bab1e318 JM |
1055 | for (auto &p : homing_axis) { |
1056 | gcode->stream->printf("%c: %5.3f ", p.axis, p.home_offset); | |
1057 | } | |
932a3995 | 1058 | |
bab1e318 | 1059 | gcode->stream->printf(" will take effect next home\n"); |
504f0e3e | 1060 | break; |
0e4bf280 | 1061 | |
078f76e0 | 1062 | case 306: // set homing offset based on current position |
0c18b666 JM |
1063 | if(is_rdelta) return; // RotaryDeltaCalibration module will handle this |
1064 | ||
078f76e0 | 1065 | set_homing_offset(gcode); |
42bbc035 | 1066 | break; |
33e4cc02 JM |
1067 | |
1068 | case 500: // save settings | |
1069 | case 503: // print settings | |
bab1e318 JM |
1070 | if(!is_rdelta) { |
1071 | gcode->stream->printf(";Home offset (mm):\nM206 "); | |
1072 | for (auto &p : homing_axis) { | |
1073 | gcode->stream->printf("%c%1.2f ", p.axis, p.home_offset); | |
1074 | } | |
1075 | gcode->stream->printf("\n"); | |
1076 | ||
1077 | }else{ | |
1078 | gcode->stream->printf(";Theta offset (degrees):\nM206 A%1.5f B%1.5f C%1.5f\n", | |
1079 | homing_axis[X_AXIS].home_offset, homing_axis[Y_AXIS].home_offset, homing_axis[Z_AXIS].home_offset); | |
1080 | } | |
932a3995 | 1081 | |
d0280b9d | 1082 | if (this->is_delta || this->is_scara) { |
42bbc035 | 1083 | gcode->stream->printf(";Trim (mm):\nM666 X%1.3f Y%1.3f Z%1.3f\n", trim_mm[0], trim_mm[1], trim_mm[2]); |
bab1e318 | 1084 | gcode->stream->printf(";Max Z\nM665 Z%1.3f\n", homing_axis[Z_AXIS].homing_position); |
7a8fe6e0 | 1085 | } |
e714bd32 | 1086 | if(saved_position[X_AXIS] != 0 || saved_position[Y_AXIS] != 0) { |
5af383e2 | 1087 | gcode->stream->printf(";predefined position:\nG28.1 X%1.4f Y%1.4f\n", saved_position[X_AXIS], saved_position[Y_AXIS]); |
e714bd32 | 1088 | } |
c339d634 | 1089 | break; |
47bbe224 | 1090 | |
42bbc035 JM |
1091 | case 665: |
1092 | if (this->is_delta || this->is_scara) { // M665 - set max gamma/z height | |
bab1e318 | 1093 | float gamma_max = homing_axis[Z_AXIS].homing_position; |
42bbc035 | 1094 | if (gcode->has_letter('Z')) { |
bab1e318 | 1095 | homing_axis[Z_AXIS].homing_position= gamma_max = gcode->get_value('Z'); |
42bbc035 JM |
1096 | } |
1097 | gcode->stream->printf("Max Z %8.3f ", gamma_max); | |
1098 | gcode->add_nl = true; | |
ec4773e5 | 1099 | } |
42bbc035 | 1100 | break; |
47bbe224 | 1101 | |
56ce2b5a | 1102 | case 666: |
3e1f5b74 | 1103 | if(this->is_delta || this->is_scara) { // M666 - set trim for each axis in mm, NB negative mm trim is down |
56ce2b5a JM |
1104 | if (gcode->has_letter('X')) trim_mm[0] = gcode->get_value('X'); |
1105 | if (gcode->has_letter('Y')) trim_mm[1] = gcode->get_value('Y'); | |
1106 | if (gcode->has_letter('Z')) trim_mm[2] = gcode->get_value('Z'); | |
47bbe224 | 1107 | |
56ce2b5a JM |
1108 | // print the current trim values in mm |
1109 | gcode->stream->printf("X: %5.3f Y: %5.3f Z: %5.3f\n", trim_mm[0], trim_mm[1], trim_mm[2]); | |
6e92ab91 | 1110 | |
56ce2b5a | 1111 | } |
e714bd32 | 1112 | break; |
47bbe224 | 1113 | |
64eaf21e | 1114 | } |
64eaf21e | 1115 | } |
64eaf21e | 1116 | } |
9f6f04a5 | 1117 | |
e714bd32 JM |
1118 | void Endstops::on_get_public_data(void* argument) |
1119 | { | |
9f6f04a5 JM |
1120 | PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument); |
1121 | ||
1122 | if(!pdr->starts_with(endstops_checksum)) return; | |
1123 | ||
1124 | if(pdr->second_element_is(trim_checksum)) { | |
86fa0b93 | 1125 | pdr->set_data_ptr(&this->trim_mm); |
ea5c6d92 JM |
1126 | pdr->set_taken(); |
1127 | ||
e714bd32 | 1128 | } else if(pdr->second_element_is(home_offset_checksum)) { |
bab1e318 JM |
1129 | // provided by caller |
1130 | float *data = static_cast<float *>(pdr->get_data_ptr()); | |
1131 | for (int i = 0; i < 3; ++i) { | |
1132 | data[i]= homing_axis[i].home_offset; | |
1133 | } | |
9f6f04a5 | 1134 | pdr->set_taken(); |
e714bd32 JM |
1135 | |
1136 | } else if(pdr->second_element_is(saved_position_checksum)) { | |
1137 | pdr->set_data_ptr(&this->saved_position); | |
1138 | pdr->set_taken(); | |
07186543 JM |
1139 | |
1140 | } else if(pdr->second_element_is(get_homing_status_checksum)) { | |
a2f1ce04 JM |
1141 | bool *homing = static_cast<bool *>(pdr->get_data_ptr()); |
1142 | *homing = this->status != NOT_HOMING; | |
07186543 | 1143 | pdr->set_taken(); |
9f6f04a5 JM |
1144 | } |
1145 | } | |
7d6fe308 | 1146 | |
e714bd32 JM |
1147 | void Endstops::on_set_public_data(void* argument) |
1148 | { | |
7d6fe308 JM |
1149 | PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument); |
1150 | ||
1151 | if(!pdr->starts_with(endstops_checksum)) return; | |
1152 | ||
1153 | if(pdr->second_element_is(trim_checksum)) { | |
e714bd32 JM |
1154 | float *t = static_cast<float*>(pdr->get_data_ptr()); |
1155 | this->trim_mm[0] = t[0]; | |
1156 | this->trim_mm[1] = t[1]; | |
1157 | this->trim_mm[2] = t[2]; | |
7d6fe308 | 1158 | pdr->set_taken(); |
ea5c6d92 | 1159 | |
e714bd32 JM |
1160 | } else if(pdr->second_element_is(home_offset_checksum)) { |
1161 | float *t = static_cast<float*>(pdr->get_data_ptr()); | |
bab1e318 JM |
1162 | if(!isnan(t[0])) homing_axis[0].home_offset= t[0]; |
1163 | if(!isnan(t[1])) homing_axis[1].home_offset= t[1]; | |
1164 | if(!isnan(t[2])) homing_axis[2].home_offset= t[2]; | |
7d6fe308 JM |
1165 | } |
1166 | } |