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