forked from MarlinFirmware/Marlin
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmotion.cpp
2517 lines (2160 loc) · 87.2 KB
/
motion.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
/**
* Marlin 3D Printer Firmware
* Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
*
* This program 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.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
*/
/**
* motion.cpp
*/
#include "motion.h"
#include "endstops.h"
#include "stepper.h"
#include "planner.h"
#include "temperature.h"
#include "../gcode/gcode.h"
#include "../lcd/marlinui.h"
#include "../inc/MarlinConfig.h"
#if ENABLED(FT_MOTION)
#include "ft_motion.h"
#endif
#if IS_SCARA
#include "../libs/buzzer.h"
#include "../lcd/marlinui.h"
#endif
#if ENABLED(POLAR)
#include "polar.h"
#endif
#if HAS_BED_PROBE
#include "probe.h"
#endif
#if HAS_LEVELING
#include "../feature/bedlevel/bedlevel.h"
#endif
#if ENABLED(BLTOUCH)
#include "../feature/bltouch.h"
#endif
#if HAS_FILAMENT_SENSOR
#include "../feature/runout.h"
#endif
#if ENABLED(SENSORLESS_HOMING)
#include "../feature/tmc_util.h"
#endif
#if ENABLED(FWRETRACT)
#include "../feature/fwretract.h"
#endif
#if ENABLED(BABYSTEP_DISPLAY_TOTAL)
#include "../feature/babystep.h"
#endif
#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
#include "../core/debug_out.h"
#if ENABLED(BD_SENSOR)
#include "../feature/bedlevel/bdl/bdl.h"
#endif
// Relative Mode. Enable with G91, disable with G90.
bool relative_mode; // = false;
/**
* Cartesian Current Position
* Used to track the native machine position as moves are queued.
* Used by 'line_to_current_position' to do a move after changing it.
* Used by 'sync_plan_position' to update 'planner.position'.
*/
#ifdef Z_IDLE_HEIGHT
#define Z_INIT_POS Z_IDLE_HEIGHT
#else
#define Z_INIT_POS Z_HOME_POS
#endif
xyze_pos_t current_position = LOGICAL_AXIS_ARRAY(0, X_HOME_POS, Y_HOME_POS, Z_INIT_POS, I_HOME_POS, J_HOME_POS, K_HOME_POS, U_HOME_POS, V_HOME_POS, W_HOME_POS);
/**
* Cartesian Destination
* The destination for a move, filled in by G-code movement commands,
* and expected by functions like 'prepare_line_to_destination'.
* G-codes can set destination using 'get_destination_from_command'
*/
xyze_pos_t destination; // {0}
// G60/G61 Position Save and Return
#if SAVED_POSITIONS
uint8_t saved_slots[(SAVED_POSITIONS + 7) >> 3];
xyze_pos_t stored_position[SAVED_POSITIONS];
#endif
// The active extruder (tool). Set with T<extruder> command.
#if HAS_MULTI_EXTRUDER
uint8_t active_extruder = 0; // = 0
#endif
#if ENABLED(LCD_SHOW_E_TOTAL)
float e_move_accumulator; // = 0
#endif
// Extruder offsets
#if HAS_HOTEND_OFFSET
xyz_pos_t hotend_offset[HOTENDS]; // Initialized by settings.load()
void reset_hotend_offsets() {
constexpr float tmp[XYZ][HOTENDS] = { HOTEND_OFFSET_X, HOTEND_OFFSET_Y, HOTEND_OFFSET_Z };
static_assert(
!tmp[X_AXIS][0] && !tmp[Y_AXIS][0] && !tmp[Z_AXIS][0],
"Offsets for the first hotend must be 0.0."
);
// Transpose from [XYZ][HOTENDS] to [HOTENDS][XYZ]
HOTEND_LOOP() LOOP_ABC(a) hotend_offset[e][a] = tmp[a][e];
TERN_(DUAL_X_CARRIAGE, hotend_offset[1].x = _MAX(X2_HOME_POS, X2_MAX_POS));
}
#endif
// The feedrate for the current move, often used as the default if
// no other feedrate is specified. Overridden for special moves.
// Set by the last G0 through G5 command's "F" parameter.
// Functions that override this for custom moves *must always* restore it!
#ifndef DEFAULT_FEEDRATE_MM_M
#define DEFAULT_FEEDRATE_MM_M 4000
#endif
feedRate_t feedrate_mm_s = MMM_TO_MMS(DEFAULT_FEEDRATE_MM_M);
int16_t feedrate_percentage = 100;
// Cartesian conversion result goes here:
xyz_pos_t cartes;
#if IS_KINEMATIC
abce_pos_t delta;
#if HAS_SCARA_OFFSET
abc_pos_t scara_home_offset;
#endif
#if HAS_SOFTWARE_ENDSTOPS
float delta_max_radius, delta_max_radius_2;
#elif IS_SCARA
constexpr float delta_max_radius = PRINTABLE_RADIUS,
delta_max_radius_2 = sq(PRINTABLE_RADIUS);
#elif ENABLED(POLAR)
constexpr float delta_max_radius = PRINTABLE_RADIUS,
delta_max_radius_2 = sq(PRINTABLE_RADIUS);
#else // DELTA
constexpr float delta_max_radius = PRINTABLE_RADIUS,
delta_max_radius_2 = sq(PRINTABLE_RADIUS);
#endif
#endif
/**
* The workspace can be offset by some commands, or
* these offsets may be omitted to save on computation.
*/
#if HAS_HOME_OFFSET
// This offset is added to the configured home position.
// Set by M206, M428, or menu item. Saved to EEPROM.
xyz_pos_t home_offset{0};
#endif
#if HAS_WORKSPACE_OFFSET
// The above two are combined to save on computes
xyz_pos_t workspace_offset{0};
#endif
#if HAS_ABL_NOT_UBL
feedRate_t xy_probe_feedrate_mm_s = MMM_TO_MMS(XY_PROBE_FEEDRATE);
#endif
/**
* Output the current position to serial
*/
inline void report_more_positions() {
stepper.report_positions();
TERN_(IS_SCARA, scara_report_positions());
TERN_(POLAR, polar_report_positions());
}
// Report the logical position for a given machine position
inline void report_logical_position(const xyze_pos_t &rpos) {
const xyze_pos_t lpos = rpos.asLogical();
#if NUM_AXES
SERIAL_ECHOPGM_P(
LIST_N(DOUBLE(NUM_AXES),
X_LBL, lpos.x,
SP_Y_LBL, lpos.y,
SP_Z_LBL, lpos.z,
SP_I_LBL, lpos.i,
SP_J_LBL, lpos.j,
SP_K_LBL, lpos.k,
SP_U_LBL, lpos.u,
SP_V_LBL, lpos.v,
SP_W_LBL, lpos.w
)
);
#endif
#if HAS_EXTRUDERS
SERIAL_ECHOPGM_P(SP_E_LBL, lpos.e);
#endif
}
// Report the real current position according to the steppers.
// Forward kinematics and un-leveling are applied.
void report_real_position() {
get_cartesian_from_steppers();
xyze_pos_t npos = LOGICAL_AXIS_ARRAY(
planner.get_axis_position_mm(E_AXIS),
cartes.x, cartes.y, cartes.z,
cartes.i, cartes.j, cartes.k,
cartes.u, cartes.v, cartes.w
);
TERN_(HAS_POSITION_MODIFIERS, planner.unapply_modifiers(npos, true));
report_logical_position(npos);
report_more_positions();
}
// Report the logical current position according to the most recent G-code command
void report_current_position() {
report_logical_position(current_position);
report_more_positions();
}
/**
* Report the logical current position according to the most recent G-code command.
* The planner.position always corresponds to the last G-code too. This makes M114
* suitable for debugging kinematics and leveling while avoiding planner sync that
* definitively interrupts the printing flow.
*/
void report_current_position_projected() {
report_logical_position(current_position);
stepper.report_a_position(planner.position);
}
#if ENABLED(AUTO_REPORT_POSITION)
AutoReporter<PositionReport> position_auto_reporter;
#endif
#if ANY(FULL_REPORT_TO_HOST_FEATURE, REALTIME_REPORTING_COMMANDS)
M_StateEnum M_State_grbl = M_INIT;
/**
* Output the current grbl compatible state to serial while moving
*/
void report_current_grblstate_moving() { SERIAL_ECHOLNPGM("S_XYZ:", int(M_State_grbl)); }
/**
* Output the current position (processed) to serial while moving
*/
void report_current_position_moving() {
get_cartesian_from_steppers();
const xyz_pos_t lpos = cartes.asLogical();
SERIAL_ECHOPGM_P(
LIST_N(DOUBLE(NUM_AXES),
X_LBL, lpos.x,
SP_Y_LBL, lpos.y,
SP_Z_LBL, lpos.z,
SP_I_LBL, lpos.i,
SP_J_LBL, lpos.j,
SP_K_LBL, lpos.k,
SP_U_LBL, lpos.u,
SP_V_LBL, lpos.v,
SP_W_LBL, lpos.w
)
#if HAS_EXTRUDERS
, SP_E_LBL, current_position.e
#endif
);
report_more_positions();
report_current_grblstate_moving();
}
/**
* Set a Grbl-compatible state from the current marlin_state
*/
M_StateEnum grbl_state_for_marlin_state() {
switch (marlin_state) {
case MF_INITIALIZING: return M_INIT;
case MF_SD_COMPLETE: return M_ALARM;
case MF_WAITING: return M_IDLE;
case MF_STOPPED: return M_END;
case MF_RUNNING: return M_RUNNING;
case MF_PAUSED: return M_HOLD;
case MF_KILLED: return M_ERROR;
default: return M_IDLE;
}
}
#endif
#if IS_KINEMATIC
bool position_is_reachable(const_float_t rx, const_float_t ry, const float inset/*=0*/) {
bool can_reach;
#if ENABLED(DELTA)
can_reach = HYPOT2(rx, ry) <= sq(PRINTABLE_RADIUS - inset + fslop);
#elif ENABLED(AXEL_TPARA)
const float R2 = HYPOT2(rx - TPARA_OFFSET_X, ry - TPARA_OFFSET_Y);
can_reach = (
R2 <= sq(L1 + L2) - inset
#if MIDDLE_DEAD_ZONE_R > 0
&& R2 >= sq(float(MIDDLE_DEAD_ZONE_R))
#endif
);
#elif IS_SCARA
const float R2 = HYPOT2(rx - SCARA_OFFSET_X, ry - SCARA_OFFSET_Y);
can_reach = (
R2 <= sq(L1 + L2) - inset
#if MIDDLE_DEAD_ZONE_R > 0
&& R2 >= sq(float(MIDDLE_DEAD_ZONE_R))
#endif
);
#elif ENABLED(POLARGRAPH)
const float d1 = rx - (draw_area_min.x),
d2 = (draw_area_max.x) - rx,
y = ry - (draw_area_max.y),
a = HYPOT(d1, y),
b = HYPOT(d2, y);
can_reach = (
a < polargraph_max_belt_len + 1
&& b < polargraph_max_belt_len + 1
);
#elif ENABLED(POLAR)
can_reach = HYPOT(rx, ry) <= PRINTABLE_RADIUS;
#endif
return can_reach;
}
#else // CARTESIAN
// Return true if the given position is within the machine bounds.
bool position_is_reachable(TERN_(HAS_X_AXIS, const_float_t rx) OPTARG(HAS_Y_AXIS, const_float_t ry)) {
if (TERN0(HAS_Y_AXIS, !COORDINATE_OKAY(ry, Y_MIN_POS - fslop, Y_MAX_POS + fslop))) return false;
#if ENABLED(DUAL_X_CARRIAGE)
if (active_extruder)
return COORDINATE_OKAY(rx, X2_MIN_POS - fslop, X2_MAX_POS + fslop);
else
return COORDINATE_OKAY(rx, X1_MIN_POS - fslop, X1_MAX_POS + fslop);
#else
if (TERN0(HAS_X_AXIS, !COORDINATE_OKAY(rx, X_MIN_POS - fslop, X_MAX_POS + fslop))) return false;
return true;
#endif
}
#endif // CARTESIAN
void home_if_needed(const bool keeplev/*=false*/) {
if (!all_axes_trusted()) gcode.home_all_axes(keeplev);
}
/**
* Run out the planner buffer and re-sync the current
* position from the last-updated stepper positions.
*/
void quickstop_stepper() {
planner.quick_stop();
planner.synchronize();
set_current_from_steppers_for_axis(ALL_AXES_ENUM);
sync_plan_position();
}
#if ENABLED(REALTIME_REPORTING_COMMANDS)
void quickpause_stepper() {
planner.quick_pause();
//planner.synchronize();
}
void quickresume_stepper() {
planner.quick_resume();
//planner.synchronize();
}
#endif
/**
* Set the planner/stepper positions directly from current_position with
* no kinematic translation. Used for homing axes and cartesian/core syncing.
*/
void sync_plan_position() {
if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position", current_position);
planner.set_position_mm(current_position);
}
#if HAS_EXTRUDERS
void sync_plan_position_e() { planner.set_e_position_mm(current_position.e); }
#endif
/**
* Get the stepper positions in the cartes[] array.
* Forward kinematics are applied for DELTA and SCARA.
*
* The result is in the current coordinate space with
* leveling applied. The coordinates need to be run through
* unapply_leveling to obtain the "ideal" coordinates
* suitable for current_position, etc.
*/
void get_cartesian_from_steppers() {
#if ENABLED(DELTA)
forward_kinematics(planner.get_axis_positions_mm());
#elif IS_SCARA
forward_kinematics(
planner.get_axis_position_degrees(A_AXIS), planner.get_axis_position_degrees(B_AXIS)
OPTARG(AXEL_TPARA, planner.get_axis_position_degrees(C_AXIS))
);
cartes.z = planner.get_axis_position_mm(Z_AXIS);
#elif ENABLED(POLAR)
forward_kinematics(planner.get_axis_position_mm(X_AXIS), planner.get_axis_position_degrees(B_AXIS));
cartes.z = planner.get_axis_position_mm(Z_AXIS);
#else
NUM_AXIS_CODE(
cartes.x = planner.get_axis_position_mm(X_AXIS),
cartes.y = planner.get_axis_position_mm(Y_AXIS),
cartes.z = planner.get_axis_position_mm(Z_AXIS),
cartes.i = planner.get_axis_position_mm(I_AXIS),
cartes.j = planner.get_axis_position_mm(J_AXIS),
cartes.k = planner.get_axis_position_mm(K_AXIS),
cartes.u = planner.get_axis_position_mm(U_AXIS),
cartes.v = planner.get_axis_position_mm(V_AXIS),
cartes.w = planner.get_axis_position_mm(W_AXIS)
);
#endif
}
/**
* Set the current_position for an axis based on
* the stepper positions, removing any leveling that
* may have been applied.
*
* To prevent small shifts in axis position always call
* sync_plan_position after updating axes with this.
*
* To keep hosts in sync, always call report_current_position
* after updating the current_position.
*/
void set_current_from_steppers_for_axis(const AxisEnum axis) {
get_cartesian_from_steppers();
xyze_pos_t pos = cartes;
TERN_(HAS_EXTRUDERS, pos.e = planner.get_axis_position_mm(E_AXIS));
TERN_(HAS_POSITION_MODIFIERS, planner.unapply_modifiers(pos, true));
if (axis == ALL_AXES_ENUM)
current_position = pos;
else
current_position[axis] = pos[axis];
}
/**
* Move the planner to the current position from wherever it last moved
* (or from wherever it has been told it is located).
*/
void line_to_current_position(const_feedRate_t fr_mm_s/*=feedrate_mm_s*/) {
planner.buffer_line(current_position, fr_mm_s);
}
#if HAS_EXTRUDERS
void unscaled_e_move(const_float_t length, const_feedRate_t fr_mm_s) {
TERN_(HAS_FILAMENT_SENSOR, runout.reset());
current_position.e += length / planner.e_factor[active_extruder];
line_to_current_position(fr_mm_s);
planner.synchronize();
}
#endif
#if IS_KINEMATIC
/**
* Buffer a fast move without interpolation. Set current_position to destination
*/
void prepare_fast_move_to_destination(const_feedRate_t scaled_fr_mm_s/*=MMS_SCALED(feedrate_mm_s)*/) {
if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_fast_move_to_destination", destination);
#if UBL_SEGMENTED
// UBL segmented line will do Z-only moves in single segment
bedlevel.line_to_destination_segmented(scaled_fr_mm_s);
#else
if (current_position == destination) return;
planner.buffer_line(destination, scaled_fr_mm_s);
#endif
current_position = destination;
}
#endif // IS_KINEMATIC
/**
* Do a fast or normal move to 'destination' with an optional FR.
* - Move at normal speed regardless of feedrate percentage.
* - Extrude the specified length regardless of flow percentage.
*/
void _internal_move_to_destination(const_feedRate_t fr_mm_s/*=0.0f*/
OPTARG(IS_KINEMATIC, const bool is_fast/*=false*/)
) {
REMEMBER(fr, feedrate_mm_s);
REMEMBER(pct, feedrate_percentage, 100);
TERN_(HAS_EXTRUDERS, REMEMBER(fac, planner.e_factor[active_extruder], 1.0f));
if (fr_mm_s) feedrate_mm_s = fr_mm_s;
if (TERN0(IS_KINEMATIC, is_fast))
TERN(IS_KINEMATIC, prepare_fast_move_to_destination(), NOOP);
else
prepare_line_to_destination();
}
#if SECONDARY_AXES
void secondary_axis_moves(SECONDARY_AXIS_ARGS(const_float_t), const_feedRate_t fr_mm_s) {
auto move_one = [&](const AxisEnum a, const_float_t p) {
const feedRate_t fr = fr_mm_s ?: homing_feedrate(a);
current_position[a] = p; line_to_current_position(fr);
};
SECONDARY_AXIS_CODE(
move_one(I_AXIS, i), move_one(J_AXIS, j), move_one(K_AXIS, k),
move_one(U_AXIS, u), move_one(V_AXIS, v), move_one(W_AXIS, w)
);
}
#endif
/**
* Plan a move to (X, Y, Z, [I, [J, [K...]]]) and set the current_position
* Plan a move to (X, Y, Z, [I, [J, [K...]]]) with separation of Z from other components.
*
* - If Z is moving up, the Z move is done before XY, etc.
* - If Z is moving down, the Z move is done after XY, etc.
* - Delta may lower Z first to get into the free motion zone.
* - Before returning, wait for the planner buffer to empty.
*/
void do_blocking_move_to(NUM_AXIS_ARGS_(const_float_t) const_feedRate_t fr_mm_s/*=0.0f*/) {
DEBUG_SECTION(log_move, "do_blocking_move_to", DEBUGGING(LEVELING));
#if NUM_AXES
if (DEBUGGING(LEVELING)) DEBUG_XYZ("> ", NUM_AXIS_ARGS());
#endif
const feedRate_t xy_feedrate = fr_mm_s ?: feedRate_t(XY_PROBE_FEEDRATE_MM_S);
#if HAS_Z_AXIS
const feedRate_t z_feedrate = fr_mm_s ?: homing_feedrate(Z_AXIS);
#endif
#if IS_KINEMATIC && DISABLED(POLARGRAPH)
// kinematic machines are expected to home to a point 1.5x their range? never reachable.
if (!position_is_reachable(x, y)) return;
destination = current_position; // sync destination at the start
#endif
#if ENABLED(DELTA)
REMEMBER(fr, feedrate_mm_s, xy_feedrate);
if (DEBUGGING(LEVELING)) DEBUG_POS("destination = current_position", destination);
// when in the danger zone
if (current_position.z > delta_clip_start_height) {
if (z > delta_clip_start_height) { // staying in the danger zone
destination.set(x, y, z); // move directly (uninterpolated)
prepare_internal_fast_move_to_destination(); // set current_position from destination
if (DEBUGGING(LEVELING)) DEBUG_POS("danger zone move", current_position);
return;
}
destination.z = delta_clip_start_height;
prepare_internal_fast_move_to_destination(); // set current_position from destination
if (DEBUGGING(LEVELING)) DEBUG_POS("zone border move", current_position);
}
if (z > current_position.z) { // raising?
destination.z = z;
prepare_internal_fast_move_to_destination(z_feedrate); // set current_position from destination
if (DEBUGGING(LEVELING)) DEBUG_POS("z raise move", current_position);
}
destination.set(x, y);
prepare_internal_move_to_destination(); // set current_position from destination
if (DEBUGGING(LEVELING)) DEBUG_POS("xy move", current_position);
if (z < current_position.z) { // lowering?
destination.z = z;
prepare_internal_fast_move_to_destination(z_feedrate); // set current_position from destination
if (DEBUGGING(LEVELING)) DEBUG_POS("z lower move", current_position);
}
#if SECONDARY_AXES
secondary_axis_moves(SECONDARY_AXIS_LIST(i, j, k, u, v, w), fr_mm_s);
#endif
#elif IS_SCARA
// If Z needs to raise, do it before moving XY
if (destination.z < z) { destination.z = z; prepare_internal_fast_move_to_destination(z_feedrate); }
destination.set(x, y); prepare_internal_fast_move_to_destination(xy_feedrate);
#if SECONDARY_AXES
secondary_axis_moves(SECONDARY_AXIS_LIST(i, j, k, u, v, w), fr_mm_s);
#endif
// If Z needs to lower, do it after moving XY
if (destination.z > z) { destination.z = z; prepare_internal_fast_move_to_destination(z_feedrate); }
#else
#if HAS_Z_AXIS // If Z needs to raise, do it before moving XY
if (current_position.z < z) { current_position.z = z; line_to_current_position(z_feedrate); }
#endif
current_position.set(TERN_(HAS_X_AXIS, x) OPTARG(HAS_Y_AXIS, y)); line_to_current_position(xy_feedrate);
#if SECONDARY_AXES
secondary_axis_moves(SECONDARY_AXIS_LIST(i, j, k, u, v, w), fr_mm_s);
#endif
#if HAS_Z_AXIS
// If Z needs to lower, do it after moving XY
if (current_position.z > z) { current_position.z = z; line_to_current_position(z_feedrate); }
#endif
#endif
planner.synchronize();
}
void do_blocking_move_to(const xy_pos_t &raw, const_feedRate_t fr_mm_s/*=0.0f*/) {
do_blocking_move_to(NUM_AXIS_LIST_(raw.x, raw.y, current_position.z, current_position.i, current_position.j, current_position.k,
current_position.u, current_position.v, current_position.w) fr_mm_s);
}
void do_blocking_move_to(const xyz_pos_t &raw, const_feedRate_t fr_mm_s/*=0.0f*/) {
do_blocking_move_to(NUM_AXIS_ELEM_(raw) fr_mm_s);
}
void do_blocking_move_to(const xyze_pos_t &raw, const_feedRate_t fr_mm_s/*=0.0f*/) {
do_blocking_move_to(NUM_AXIS_ELEM_(raw) fr_mm_s);
}
#if HAS_X_AXIS
void do_blocking_move_to_x(const_float_t rx, const_feedRate_t fr_mm_s/*=0.0*/) {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("do_blocking_move_to_x(", rx, ", ", fr_mm_s, ")");
do_blocking_move_to(
NUM_AXIS_LIST_(rx, current_position.y, current_position.z, current_position.i, current_position.j, current_position.k,
current_position.u, current_position.v, current_position.w)
fr_mm_s
);
}
#endif
#if HAS_Y_AXIS
void do_blocking_move_to_y(const_float_t ry, const_feedRate_t fr_mm_s/*=0.0*/) {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("do_blocking_move_to_y(", ry, ", ", fr_mm_s, ")");
do_blocking_move_to(
NUM_AXIS_LIST_(current_position.x, ry, current_position.z, current_position.i, current_position.j, current_position.k,
current_position.u, current_position.v, current_position.w)
fr_mm_s
);
}
#endif
#if HAS_Z_AXIS
void do_blocking_move_to_z(const_float_t rz, const_feedRate_t fr_mm_s/*=0.0*/) {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("do_blocking_move_to_z(", rz, ", ", fr_mm_s, ")");
do_blocking_move_to_xy_z(current_position, rz, fr_mm_s);
}
#endif
#if HAS_I_AXIS
void do_blocking_move_to_i(const_float_t ri, const_feedRate_t fr_mm_s/*=0.0*/) {
do_blocking_move_to_xyz_i(current_position, ri, fr_mm_s);
}
void do_blocking_move_to_xyz_i(const xyze_pos_t &raw, const_float_t i, const_feedRate_t fr_mm_s/*=0.0f*/) {
do_blocking_move_to(
NUM_AXIS_LIST_(raw.x, raw.y, raw.z, i, raw.j, raw.k, raw.u, raw.v, raw.w)
fr_mm_s
);
}
#endif
#if HAS_J_AXIS
void do_blocking_move_to_j(const_float_t rj, const_feedRate_t fr_mm_s/*=0.0*/) {
do_blocking_move_to_xyzi_j(current_position, rj, fr_mm_s);
}
void do_blocking_move_to_xyzi_j(const xyze_pos_t &raw, const_float_t j, const_feedRate_t fr_mm_s/*=0.0f*/) {
do_blocking_move_to(
NUM_AXIS_LIST_(raw.x, raw.y, raw.z, raw.i, j, raw.k, raw.u, raw.v, raw.w)
fr_mm_s
);
}
#endif
#if HAS_K_AXIS
void do_blocking_move_to_k(const_float_t rk, const_feedRate_t fr_mm_s/*=0.0*/) {
do_blocking_move_to_xyzij_k(current_position, rk, fr_mm_s);
}
void do_blocking_move_to_xyzij_k(const xyze_pos_t &raw, const_float_t k, const_feedRate_t fr_mm_s/*=0.0f*/) {
do_blocking_move_to(
NUM_AXIS_LIST_(raw.x, raw.y, raw.z, raw.i, raw.j, k, raw.u, raw.v, raw.w)
fr_mm_s
);
}
#endif
#if HAS_U_AXIS
void do_blocking_move_to_u(const_float_t ru, const_feedRate_t fr_mm_s/*=0.0*/) {
do_blocking_move_to_xyzijk_u(current_position, ru, fr_mm_s);
}
void do_blocking_move_to_xyzijk_u(const xyze_pos_t &raw, const_float_t u, const_feedRate_t fr_mm_s/*=0.0f*/) {
do_blocking_move_to(
NUM_AXIS_LIST_(raw.x, raw.y, raw.z, raw.i, raw.j, raw.k, u, raw.v, raw.w)
fr_mm_s
);
}
#endif
#if HAS_V_AXIS
void do_blocking_move_to_v(const_float_t rv, const_feedRate_t fr_mm_s/*=0.0*/) {
do_blocking_move_to_xyzijku_v(current_position, rv, fr_mm_s);
}
void do_blocking_move_to_xyzijku_v(const xyze_pos_t &raw, const_float_t v, const_feedRate_t fr_mm_s/*=0.0f*/) {
do_blocking_move_to(
NUM_AXIS_LIST_(raw.x, raw.y, raw.z, raw.i, raw.j, raw.k, raw.u, v, raw.w)
fr_mm_s
);
}
#endif
#if HAS_W_AXIS
void do_blocking_move_to_w(const_float_t rw, const_feedRate_t fr_mm_s/*=0.0*/) {
do_blocking_move_to_xyzijkuv_w(current_position, rw, fr_mm_s);
}
void do_blocking_move_to_xyzijkuv_w(const xyze_pos_t &raw, const_float_t w, const_feedRate_t fr_mm_s/*=0.0f*/) {
do_blocking_move_to(
NUM_AXIS_LIST_(raw.x, raw.y, raw.z, raw.i, raw.j, raw.k, raw.u, raw.v, w)
fr_mm_s
);
}
#endif
#if HAS_Y_AXIS
void do_blocking_move_to_xy(const_float_t rx, const_float_t ry, const_feedRate_t fr_mm_s/*=0.0*/) {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("do_blocking_move_to_xy(", rx, ", ", ry, ", ", fr_mm_s, ")");
do_blocking_move_to(
NUM_AXIS_LIST_(rx, ry, current_position.z, current_position.i, current_position.j, current_position.k,
current_position.u, current_position.v, current_position.w)
fr_mm_s
);
}
void do_blocking_move_to_xy(const xy_pos_t &raw, const_feedRate_t fr_mm_s/*=0.0f*/) {
do_blocking_move_to_xy(raw.x, raw.y, fr_mm_s);
}
#endif
#if HAS_Z_AXIS
void do_blocking_move_to_xy_z(const xy_pos_t &raw, const_float_t z, const_feedRate_t fr_mm_s/*=0.0f*/) {
do_blocking_move_to(
NUM_AXIS_LIST_(raw.x, raw.y, z, current_position.i, current_position.j, current_position.k,
current_position.u, current_position.v, current_position.w)
fr_mm_s
);
}
void do_z_clearance(const_float_t zclear, const bool with_probe/*=true*/, const bool lower_allowed/*=false*/) {
UNUSED(with_probe);
float zdest = zclear;
TERN_(HAS_BED_PROBE, if (with_probe && probe.offset.z < 0) zdest -= probe.offset.z);
NOMORE(zdest, Z_MAX_POS);
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("do_z_clearance(", zclear, " [", current_position.z, " to ", zdest, "], ", lower_allowed, ")");
if ((!lower_allowed && zdest < current_position.z) || zdest == current_position.z) return;
do_blocking_move_to_z(zdest, TERN(HAS_BED_PROBE, z_probe_fast_mm_s, homing_feedrate(Z_AXIS)));
}
void do_z_clearance_by(const_float_t zclear) {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("do_z_clearance_by(", zclear, ")");
do_z_clearance(current_position.z + zclear, false);
}
void do_move_after_z_homing() {
DEBUG_SECTION(mzah, "do_move_after_z_homing", DEBUGGING(LEVELING));
#if defined(Z_AFTER_HOMING) || ALL(DWIN_LCD_PROUI, INDIVIDUAL_AXIS_HOMING_SUBMENU, MESH_BED_LEVELING)
do_z_clearance(Z_POST_CLEARANCE, true, true);
#elif ENABLED(USE_PROBE_FOR_Z_HOMING)
probe.move_z_after_probing();
#endif
}
#endif
//
// Prepare to do endstop or probe moves with custom feedrates.
// - Save / restore current feedrate and multiplier
//
static float saved_feedrate_mm_s;
static int16_t saved_feedrate_percentage;
void remember_feedrate_scaling_off() {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("remember_feedrate_scaling_off: fr=", feedrate_mm_s, " ", feedrate_percentage, "%");
saved_feedrate_mm_s = feedrate_mm_s;
saved_feedrate_percentage = feedrate_percentage;
feedrate_percentage = 100;
}
void restore_feedrate_and_scaling() {
feedrate_mm_s = saved_feedrate_mm_s;
feedrate_percentage = saved_feedrate_percentage;
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("restore_feedrate_and_scaling: fr=", feedrate_mm_s, " ", feedrate_percentage, "%");
}
#if HAS_SOFTWARE_ENDSTOPS
// Software Endstops are based on the configured limits.
#define _AMIN(A) A##_MIN_POS
#define _AMAX(A) A##_MAX_POS
soft_endstops_t soft_endstop = {
true, false,
{ MAPLIST(_AMIN, MAIN_AXIS_NAMES) },
{ MAPLIST(_AMAX, MAIN_AXIS_NAMES) },
};
/**
* Software endstops can be used to monitor the open end of
* an axis that has a hardware endstop on the other end. Or
* they can prevent axes from moving past endstops and grinding.
*
* To keep doing their job as the coordinate system changes,
* the software endstop positions must be refreshed to remain
* at the same positions relative to the machine.
*/
void update_software_endstops(const AxisEnum axis
OPTARG(HAS_HOTEND_OFFSET, const uint8_t old_tool_index/*=0*/, const uint8_t new_tool_index/*=0*/)
) {
#if ENABLED(DUAL_X_CARRIAGE)
if (axis == X_AXIS) {
// In Dual X mode hotend_offset[X] is T1's home position
const float dual_max_x = _MAX(hotend_offset[1].x, X2_MAX_POS);
if (new_tool_index != 0) {
// T1 can move from X2_MIN_POS to X2_MAX_POS or X2 home position (whichever is larger)
soft_endstop.min.x = X2_MIN_POS;
soft_endstop.max.x = dual_max_x;
}
else if (idex_is_duplicating()) {
// In Duplication Mode, T0 can move as far left as X1_MIN_POS
// but not so far to the right that T1 would move past the end
soft_endstop.min.x = X1_MIN_POS;
soft_endstop.max.x = _MIN(X1_MAX_POS, dual_max_x - duplicate_extruder_x_offset);
}
else {
// In other modes, T0 can move from X1_MIN_POS to X1_MAX_POS
soft_endstop.min.x = X1_MIN_POS;
soft_endstop.max.x = X1_MAX_POS;
}
}
#elif ENABLED(DELTA)
soft_endstop.min[axis] = base_min_pos(axis);
soft_endstop.max[axis] = (axis == Z_AXIS) ? DIFF_TERN(USE_PROBE_FOR_Z_HOMING, delta_height, probe.offset.z) : base_home_pos(axis);
switch (axis) {
case X_AXIS:
case Y_AXIS:
// Get a minimum radius for clamping
delta_max_radius = _MIN(ABS(_MAX(soft_endstop.min.x, soft_endstop.min.y)), soft_endstop.max.x, soft_endstop.max.y);
delta_max_radius_2 = sq(delta_max_radius);
break;
case Z_AXIS:
refresh_delta_clip_start_height();
default: break;
}
#elif HAS_HOTEND_OFFSET
// Software endstops are relative to the tool 0 workspace, so
// the movement limits must be shifted by the tool offset to
// retain the same physical limit when other tools are selected.
if (new_tool_index == old_tool_index || axis == Z_AXIS) { // The Z axis is "special" and shouldn't be modified
const float offs = (axis == Z_AXIS) ? 0 : hotend_offset[active_extruder][axis];
soft_endstop.min[axis] = base_min_pos(axis) + offs;
soft_endstop.max[axis] = base_max_pos(axis) + offs;
}
else {
const float diff = hotend_offset[new_tool_index][axis] - hotend_offset[old_tool_index][axis];
soft_endstop.min[axis] += diff;
soft_endstop.max[axis] += diff;
}
#else
soft_endstop.min[axis] = base_min_pos(axis);
soft_endstop.max[axis] = base_max_pos(axis);
#endif
if (DEBUGGING(LEVELING))
SERIAL_ECHOLNPGM("Axis ", AS_CHAR(AXIS_CHAR(axis)), " min:", soft_endstop.min[axis], " max:", soft_endstop.max[axis]);
}
/**
* Constrain the given coordinates to the software endstops.
*
* For DELTA/SCARA the XY constraint is based on the smallest
* radius within the set software endstops.
*/
void apply_motion_limits(xyz_pos_t &target) {
if (!soft_endstop._enabled) return;
#if IS_KINEMATIC
if (TERN0(DELTA, !all_axes_homed())) return;
#if ALL(HAS_HOTEND_OFFSET, DELTA)
// The effector center position will be the target minus the hotend offset.
const xy_pos_t offs = hotend_offset[active_extruder];
#elif ENABLED(POLARGRAPH)
// POLARGRAPH uses draw_area_* below...
#elif ENABLED(POLAR)
// For now, we don't limit POLAR
#else
// SCARA needs to consider the angle of the arm through the entire move, so for now use no tool offset.
constexpr xy_pos_t offs{0};
#endif
#if ENABLED(POLARGRAPH)
LIMIT(target.x, draw_area_min.x, draw_area_max.x);
LIMIT(target.y, draw_area_min.y, draw_area_max.y);
#elif ENABLED(POLAR)
// Motion limits are as same as cartesian limits.
#else
if (TERN1(IS_SCARA, axis_was_homed(X_AXIS) && axis_was_homed(Y_AXIS))) {
const float dist_2 = HYPOT2(target.x - offs.x, target.y - offs.y);
if (dist_2 > delta_max_radius_2)
target *= float(delta_max_radius / SQRT(dist_2)); // 200 / 300 = 0.66
}
#endif
#else
#if HAS_X_AXIS
if (axis_was_homed(X_AXIS)) {
#if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MIN_SOFTWARE_ENDSTOP_X)
NOLESS(target.x, soft_endstop.min.x);
#endif
#if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MAX_SOFTWARE_ENDSTOP_X)
NOMORE(target.x, soft_endstop.max.x);
#endif
}
#endif
#if HAS_Y_AXIS
if (axis_was_homed(Y_AXIS)) {
#if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MIN_SOFTWARE_ENDSTOP_Y)
NOLESS(target.y, soft_endstop.min.y);
#endif
#if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MAX_SOFTWARE_ENDSTOP_Y)
NOMORE(target.y, soft_endstop.max.y);
#endif
}
#endif
#endif