12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400 |
- /**
- * Marlin 3D Printer Firmware
- * Copyright (C) 2016 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 <http://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 "../inc/MarlinConfig.h"
-
- #if IS_SCARA
- #include "../libs/buzzer.h"
- #include "../lcd/ultralcd.h"
- #endif
-
- #if HAS_BED_PROBE
- #include "probe.h"
- #endif
-
- #if HAS_LEVELING
- #include "../feature/bedlevel/bedlevel.h"
- #endif
-
- #if HAS_AXIS_UNHOMED_ERR && ENABLED(ULTRA_LCD)
- #include "../lcd/ultralcd.h"
- #endif
-
- #if ENABLED(SENSORLESS_HOMING)
- #include "../feature/tmc_util.h"
- #endif
-
- #if ENABLED(FWRETRACT)
- #include "../feature/fwretract.h"
- #endif
-
- #define XYZ_CONSTS(type, array, CONFIG) const PROGMEM type array##_P[XYZ] = { X_##CONFIG, Y_##CONFIG, Z_##CONFIG }
-
- XYZ_CONSTS(float, base_min_pos, MIN_POS);
- XYZ_CONSTS(float, base_max_pos, MAX_POS);
- XYZ_CONSTS(float, base_home_pos, HOME_POS);
- XYZ_CONSTS(float, max_length, MAX_LENGTH);
- XYZ_CONSTS(float, home_bump_mm, HOME_BUMP_MM);
- XYZ_CONSTS(signed char, home_dir, HOME_DIR);
-
- // 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 'buffer_line_to_current_position' to do a move after changing it.
- * Used by 'SYNC_PLAN_POSITION_KINEMATIC' to update 'planner.position'.
- */
- float current_position[XYZE] = { 0.0 };
-
- /**
- * Cartesian Destination
- * The destination for a move, filled in by G-code movement commands,
- * and expected by functions like 'prepare_move_to_destination'.
- * Set with 'gcode_get_destination' or 'set_destination_from_current'.
- */
- float destination[XYZE] = { 0.0 };
-
-
- // The active extruder (tool). Set with T<extruder> command.
- uint8_t active_extruder = 0;
-
- // Extruder offsets
- #if HOTENDS > 1
- float hotend_offset[XYZ][HOTENDS]; // Initialized by settings.load()
- #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!
- float feedrate_mm_s = MMM_TO_MMS(1500.0);
-
- int16_t feedrate_percentage = 100;
-
- // Homing feedrate is const progmem - compare to constexpr in the header
- const float homing_feedrate_mm_s[4] PROGMEM = {
- #if ENABLED(DELTA)
- MMM_TO_MMS(HOMING_FEEDRATE_Z), MMM_TO_MMS(HOMING_FEEDRATE_Z),
- #else
- MMM_TO_MMS(HOMING_FEEDRATE_XY), MMM_TO_MMS(HOMING_FEEDRATE_XY),
- #endif
- MMM_TO_MMS(HOMING_FEEDRATE_Z), 0
- };
-
- // Cartesian conversion result goes here:
- float cartes[XYZ];
-
- // Until kinematics.cpp is created, create this here
- #if IS_KINEMATIC
- float delta[ABC];
- #endif
-
- /**
- * The workspace can be offset by some commands, or
- * these offsets may be omitted to save on computation.
- */
- #if HAS_WORKSPACE_OFFSET
- #if HAS_POSITION_SHIFT
- // The distance that XYZ has been offset by G92. Reset by G28.
- float position_shift[XYZ] = { 0 };
- #endif
- #if HAS_HOME_OFFSET
- // This offset is added to the configured home position.
- // Set by M206, M428, or menu item. Saved to EEPROM.
- float home_offset[XYZ] = { 0 };
- #endif
- #if HAS_HOME_OFFSET && HAS_POSITION_SHIFT
- // The above two are combined to save on computes
- float workspace_offset[XYZ] = { 0 };
- #endif
- #endif
-
- #if OLDSCHOOL_ABL
- float xy_probe_feedrate_mm_s = MMM_TO_MMS(XY_PROBE_SPEED);
- #endif
-
- /**
- * Output the current position to serial
- */
- void report_current_position() {
- SERIAL_PROTOCOLPGM("X:");
- SERIAL_PROTOCOL(LOGICAL_X_POSITION(current_position[X_AXIS]));
- SERIAL_PROTOCOLPGM(" Y:");
- SERIAL_PROTOCOL(LOGICAL_Y_POSITION(current_position[Y_AXIS]));
- SERIAL_PROTOCOLPGM(" Z:");
- SERIAL_PROTOCOL(LOGICAL_Z_POSITION(current_position[Z_AXIS]));
- SERIAL_PROTOCOLPGM(" E:");
- SERIAL_PROTOCOL(current_position[E_AXIS]);
-
- stepper.report_positions();
-
- #if IS_SCARA
- scara_report_positions();
- #endif
- }
-
- /**
- * sync_plan_position
- *
- * 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 ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position", current_position);
- #endif
- planner.set_position_mm(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
- }
-
- void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); }
-
- /**
- * 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_DELTA(
- stepper.get_axis_position_mm(A_AXIS),
- stepper.get_axis_position_mm(B_AXIS),
- stepper.get_axis_position_mm(C_AXIS)
- );
- #else
- #if IS_SCARA
- forward_kinematics_SCARA(
- stepper.get_axis_position_degrees(A_AXIS),
- stepper.get_axis_position_degrees(B_AXIS)
- );
- #else
- cartes[X_AXIS] = stepper.get_axis_position_mm(X_AXIS);
- cartes[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
- #endif
- cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_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_KINEMATIC 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();
- #if PLANNER_LEVELING
- planner.unapply_leveling(cartes);
- #endif
- if (axis == ALL_AXES)
- COPY(current_position, cartes);
- else
- current_position[axis] = cartes[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() {
- planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate_mm_s, active_extruder);
- }
-
- /**
- * Move the planner to the position stored in the destination array, which is
- * used by G0/G1/G2/G3/G5 and many other functions to set a destination.
- */
- void buffer_line_to_destination(const float fr_mm_s) {
- planner.buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], fr_mm_s, active_extruder);
- }
-
- #if IS_KINEMATIC
-
- void sync_plan_position_kinematic() {
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_kinematic", current_position);
- #endif
- planner.set_position_mm_kinematic(current_position);
- }
-
- /**
- * Calculate delta, start a line, and set current_position to destination
- */
- void prepare_uninterpolated_move_to_destination(const float fr_mm_s/*=0.0*/) {
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_uninterpolated_move_to_destination", destination);
- #endif
-
- gcode.refresh_cmd_timeout();
-
- #if UBL_SEGMENTED
- // ubl segmented line will do z-only moves in single segment
- ubl.prepare_segmented_line_to(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s));
- #else
- if ( current_position[X_AXIS] == destination[X_AXIS]
- && current_position[Y_AXIS] == destination[Y_AXIS]
- && current_position[Z_AXIS] == destination[Z_AXIS]
- && current_position[E_AXIS] == destination[E_AXIS]
- ) return;
-
- planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
- #endif
-
- set_current_from_destination();
- }
-
- #endif // IS_KINEMATIC
-
- /**
- * Plan a move to (X, Y, Z) and set the current_position
- * The final current_position may not be the one that was requested
- */
- void do_blocking_move_to(const float &rx, const float &ry, const float &rz, const float &fr_mm_s/*=0.0*/) {
- const float old_feedrate_mm_s = feedrate_mm_s;
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) print_xyz(PSTR(">>> do_blocking_move_to"), NULL, rx, ry, rz);
- #endif
-
- const float z_feedrate = fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS);
-
- #if ENABLED(DELTA)
-
- if (!position_is_reachable(rx, ry)) return;
-
- feedrate_mm_s = fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S;
-
- set_destination_from_current(); // sync destination at the start
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) DEBUG_POS("set_destination_from_current", destination);
- #endif
-
- // when in the danger zone
- if (current_position[Z_AXIS] > delta_clip_start_height) {
- if (rz > delta_clip_start_height) { // staying in the danger zone
- destination[X_AXIS] = rx; // move directly (uninterpolated)
- destination[Y_AXIS] = ry;
- destination[Z_AXIS] = rz;
- prepare_uninterpolated_move_to_destination(); // set_current_from_destination()
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) DEBUG_POS("danger zone move", current_position);
- #endif
- return;
- }
- destination[Z_AXIS] = delta_clip_start_height;
- prepare_uninterpolated_move_to_destination(); // set_current_from_destination()
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) DEBUG_POS("zone border move", current_position);
- #endif
- }
-
- if (rz > current_position[Z_AXIS]) { // raising?
- destination[Z_AXIS] = rz;
- prepare_uninterpolated_move_to_destination(z_feedrate); // set_current_from_destination()
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) DEBUG_POS("z raise move", current_position);
- #endif
- }
-
- destination[X_AXIS] = rx;
- destination[Y_AXIS] = ry;
- prepare_move_to_destination(); // set_current_from_destination()
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) DEBUG_POS("xy move", current_position);
- #endif
-
- if (rz < current_position[Z_AXIS]) { // lowering?
- destination[Z_AXIS] = rz;
- prepare_uninterpolated_move_to_destination(z_feedrate); // set_current_from_destination()
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) DEBUG_POS("z lower move", current_position);
- #endif
- }
-
- #elif IS_SCARA
-
- if (!position_is_reachable(rx, ry)) return;
-
- set_destination_from_current();
-
- // If Z needs to raise, do it before moving XY
- if (destination[Z_AXIS] < rz) {
- destination[Z_AXIS] = rz;
- prepare_uninterpolated_move_to_destination(z_feedrate);
- }
-
- destination[X_AXIS] = rx;
- destination[Y_AXIS] = ry;
- prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S);
-
- // If Z needs to lower, do it after moving XY
- if (destination[Z_AXIS] > rz) {
- destination[Z_AXIS] = rz;
- prepare_uninterpolated_move_to_destination(z_feedrate);
- }
-
- #else
-
- // If Z needs to raise, do it before moving XY
- if (current_position[Z_AXIS] < rz) {
- feedrate_mm_s = z_feedrate;
- current_position[Z_AXIS] = rz;
- line_to_current_position();
- }
-
- feedrate_mm_s = fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S;
- current_position[X_AXIS] = rx;
- current_position[Y_AXIS] = ry;
- line_to_current_position();
-
- // If Z needs to lower, do it after moving XY
- if (current_position[Z_AXIS] > rz) {
- feedrate_mm_s = z_feedrate;
- current_position[Z_AXIS] = rz;
- line_to_current_position();
- }
-
- #endif
-
- stepper.synchronize();
-
- feedrate_mm_s = old_feedrate_mm_s;
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< do_blocking_move_to");
- #endif
- }
- void do_blocking_move_to_x(const float &rx, const float &fr_mm_s/*=0.0*/) {
- do_blocking_move_to(rx, current_position[Y_AXIS], current_position[Z_AXIS], fr_mm_s);
- }
- void do_blocking_move_to_z(const float &rz, const float &fr_mm_s/*=0.0*/) {
- do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], rz, fr_mm_s);
- }
- void do_blocking_move_to_xy(const float &rx, const float &ry, const float &fr_mm_s/*=0.0*/) {
- do_blocking_move_to(rx, ry, current_position[Z_AXIS], fr_mm_s);
- }
-
- //
- // Prepare to do endstop or probe moves
- // with custom feedrates.
- //
- // - Save current feedrates
- // - Reset the rate multiplier
- // - Reset the command timeout
- // - Enable the endstops (for endstop moves)
- //
- void bracket_probe_move(const bool before) {
- static float saved_feedrate_mm_s;
- static int16_t saved_feedrate_percentage;
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) DEBUG_POS("bracket_probe_move", current_position);
- #endif
- if (before) {
- saved_feedrate_mm_s = feedrate_mm_s;
- saved_feedrate_percentage = feedrate_percentage;
- feedrate_percentage = 100;
- gcode.refresh_cmd_timeout();
- }
- else {
- feedrate_mm_s = saved_feedrate_mm_s;
- feedrate_percentage = saved_feedrate_percentage;
- gcode.refresh_cmd_timeout();
- }
- }
-
- void setup_for_endstop_or_probe_move() { bracket_probe_move(true); }
- void clean_up_after_endstop_or_probe_move() { bracket_probe_move(false); }
-
- // Software Endstops are based on the configured limits.
- float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
- soft_endstop_max[XYZ] = { X_MAX_BED, Y_MAX_BED, Z_MAX_POS };
-
- #if HAS_SOFTWARE_ENDSTOPS
-
- // Software Endstops are based on the configured limits.
- bool soft_endstops_enabled = true;
-
- #if IS_KINEMATIC
- float soft_endstop_radius, soft_endstop_radius_2;
- #endif
-
- /**
- * 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 clamp_to_software_endstops(float target[XYZ]) {
- if (!soft_endstops_enabled) return;
- #if IS_KINEMATIC
- const float dist_2 = HYPOT2(target[X_AXIS], target[Y_AXIS]);
- if (dist_2 > soft_endstop_radius_2) {
- const float ratio = soft_endstop_radius / SQRT(dist_2); // 200 / 300 = 0.66
- target[X_AXIS] *= ratio;
- target[Y_AXIS] *= ratio;
- }
- #else
- #if ENABLED(MIN_SOFTWARE_ENDSTOP_X)
- NOLESS(target[X_AXIS], soft_endstop_min[X_AXIS]);
- #endif
- #if ENABLED(MIN_SOFTWARE_ENDSTOP_Y)
- NOLESS(target[Y_AXIS], soft_endstop_min[Y_AXIS]);
- #endif
- #if ENABLED(MAX_SOFTWARE_ENDSTOP_X)
- NOMORE(target[X_AXIS], soft_endstop_max[X_AXIS]);
- #endif
- #if ENABLED(MAX_SOFTWARE_ENDSTOP_Y)
- NOMORE(target[Y_AXIS], soft_endstop_max[Y_AXIS]);
- #endif
- #endif
- #if ENABLED(MIN_SOFTWARE_ENDSTOP_Z)
- NOLESS(target[Z_AXIS], soft_endstop_min[Z_AXIS]);
- #endif
- #if ENABLED(MAX_SOFTWARE_ENDSTOP_Z)
- NOMORE(target[Z_AXIS], soft_endstop_max[Z_AXIS]);
- #endif
- }
-
- #endif
-
- #if !UBL_SEGMENTED
- #if IS_KINEMATIC
-
- #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
- #if ENABLED(DELTA)
- #define ADJUST_DELTA(V) \
- if (planner.leveling_active) { \
- const float zadj = bilinear_z_offset(V); \
- delta[A_AXIS] += zadj; \
- delta[B_AXIS] += zadj; \
- delta[C_AXIS] += zadj; \
- }
- #else
- #define ADJUST_DELTA(V) if (planner.leveling_active) { delta[Z_AXIS] += bilinear_z_offset(V); }
- #endif
- #else
- #define ADJUST_DELTA(V) NOOP
- #endif
-
- /**
- * Prepare a linear move in a DELTA or SCARA setup.
- *
- * Called from prepare_move_to_destination as the
- * default Delta/SCARA segmenter.
- *
- * This calls planner.buffer_line several times, adding
- * small incremental moves for DELTA or SCARA.
- *
- * For Unified Bed Leveling (Delta or Segmented Cartesian)
- * the ubl.prepare_segmented_line_to method replaces this.
- *
- * For Auto Bed Leveling (Bilinear) with SEGMENT_LEVELED_MOVES
- * this is replaced by segmented_line_to_destination below.
- */
- inline bool prepare_kinematic_move_to(const float (&rtarget)[XYZE]) {
-
- // Get the top feedrate of the move in the XY plane
- const float _feedrate_mm_s = MMS_SCALED(feedrate_mm_s);
-
- const float xdiff = rtarget[X_AXIS] - current_position[X_AXIS],
- ydiff = rtarget[Y_AXIS] - current_position[Y_AXIS];
-
- // If the move is only in Z/E don't split up the move
- if (!xdiff && !ydiff) {
- planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder);
- return false; // caller will update current_position
- }
-
- // Fail if attempting move outside printable radius
- if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) return true;
-
- // Remaining cartesian distances
- const float zdiff = rtarget[Z_AXIS] - current_position[Z_AXIS],
- ediff = rtarget[E_AXIS] - current_position[E_AXIS];
-
- // Get the linear distance in XYZ
- float cartesian_mm = SQRT(sq(xdiff) + sq(ydiff) + sq(zdiff));
-
- // If the move is very short, check the E move distance
- if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = FABS(ediff);
-
- // No E move either? Game over.
- if (UNEAR_ZERO(cartesian_mm)) return true;
-
- // Minimum number of seconds to move the given distance
- const float seconds = cartesian_mm / _feedrate_mm_s;
-
- // The number of segments-per-second times the duration
- // gives the number of segments
- uint16_t segments = delta_segments_per_second * seconds;
-
- // For SCARA minimum segment size is 0.25mm
- #if IS_SCARA
- NOMORE(segments, cartesian_mm * 4);
- #endif
-
- // At least one segment is required
- NOLESS(segments, 1);
-
- // The approximate length of each segment
- const float inv_segments = 1.0 / float(segments),
- cartesian_segment_mm = cartesian_mm * inv_segments,
- segment_distance[XYZE] = {
- xdiff * inv_segments,
- ydiff * inv_segments,
- zdiff * inv_segments,
- ediff * inv_segments
- };
-
- // SERIAL_ECHOPAIR("mm=", cartesian_mm);
- // SERIAL_ECHOPAIR(" seconds=", seconds);
- // SERIAL_ECHOLNPAIR(" segments=", segments);
- // SERIAL_ECHOLNPAIR(" segment_mm=", cartesian_segment_mm);
-
- #if ENABLED(SCARA_FEEDRATE_SCALING)
- // SCARA needs to scale the feed rate from mm/s to degrees/s
- const float inv_segment_length = min(10.0, float(segments) / cartesian_mm), // 1/mm/segs
- inverse_secs = inv_segment_length * _feedrate_mm_s;
- float oldA = stepper.get_axis_position_degrees(A_AXIS),
- oldB = stepper.get_axis_position_degrees(B_AXIS);
- #endif
-
- // Get the current position as starting point
- float raw[XYZE];
- COPY(raw, current_position);
-
-
- // Calculate and execute the segments
- while (--segments) {
-
- static millis_t next_idle_ms = millis() + 200UL;
- thermalManager.manage_heater(); // This returns immediately if not really needed.
- if (ELAPSED(millis(), next_idle_ms)) {
- next_idle_ms = millis() + 200UL;
- idle();
- }
-
- LOOP_XYZE(i) raw[i] += segment_distance[i];
-
- #if ENABLED(DELTA)
- DELTA_IK(raw); // Delta can inline its kinematics
- #else
- inverse_kinematics(raw);
- #endif
- ADJUST_DELTA(raw); // Adjust Z if bed leveling is enabled
-
- #if ENABLED(SCARA_FEEDRATE_SCALING)
- // For SCARA scale the feed rate from mm/s to degrees/s
- // i.e., Complete the angular vector in the given time.
- planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder, cartesian_segment_mm);
- oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
- #else
- planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder, cartesian_segment_mm);
- #endif
- }
-
- // Ensure last segment arrives at target location.
- #if ENABLED(SCARA_FEEDRATE_SCALING)
- inverse_kinematics(rtarget);
- ADJUST_DELTA(rtarget);
- planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], rtarget[Z_AXIS], rtarget[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder, cartesian_segment_mm);
- #else
- planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder, cartesian_segment_mm);
- #endif
-
- return false; // caller will update current_position
- }
-
- #else // !IS_KINEMATIC
-
- #if ENABLED(SEGMENT_LEVELED_MOVES)
-
- /**
- * Prepare a segmented move on a CARTESIAN setup.
- *
- * This calls planner.buffer_line several times, adding
- * small incremental moves. This allows the planner to
- * apply more detailed bed leveling to the full move.
- */
- inline void segmented_line_to_destination(const float &fr_mm_s, const float segment_size=LEVELED_SEGMENT_LENGTH) {
-
- const float xdiff = destination[X_AXIS] - current_position[X_AXIS],
- ydiff = destination[Y_AXIS] - current_position[Y_AXIS];
-
- // If the move is only in Z/E don't split up the move
- if (!xdiff && !ydiff) {
- planner.buffer_line_kinematic(destination, fr_mm_s, active_extruder);
- return;
- }
-
- // Remaining cartesian distances
- const float zdiff = destination[Z_AXIS] - current_position[Z_AXIS],
- ediff = destination[E_AXIS] - current_position[E_AXIS];
-
- // Get the linear distance in XYZ
- // If the move is very short, check the E move distance
- // No E move either? Game over.
- float cartesian_mm = SQRT(sq(xdiff) + sq(ydiff) + sq(zdiff));
- if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = FABS(ediff);
- if (UNEAR_ZERO(cartesian_mm)) return;
-
- // The length divided by the segment size
- // At least one segment is required
- uint16_t segments = cartesian_mm / segment_size;
- NOLESS(segments, 1);
-
- // The approximate length of each segment
- const float inv_segments = 1.0 / float(segments),
- cartesian_segment_mm = cartesian_mm * inv_segments,
- segment_distance[XYZE] = {
- xdiff * inv_segments,
- ydiff * inv_segments,
- zdiff * inv_segments,
- ediff * inv_segments
- };
-
- // SERIAL_ECHOPAIR("mm=", cartesian_mm);
- // SERIAL_ECHOLNPAIR(" segments=", segments);
- // SERIAL_ECHOLNPAIR(" segment_mm=", cartesian_segment_mm);
-
- // Get the raw current position as starting point
- float raw[XYZE];
- COPY(raw, current_position);
-
- // Calculate and execute the segments
- while (--segments) {
- static millis_t next_idle_ms = millis() + 200UL;
- thermalManager.manage_heater(); // This returns immediately if not really needed.
- if (ELAPSED(millis(), next_idle_ms)) {
- next_idle_ms = millis() + 200UL;
- idle();
- }
- LOOP_XYZE(i) raw[i] += segment_distance[i];
- planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder, cartesian_segment_mm);
- }
-
- // Since segment_distance is only approximate,
- // the final move must be to the exact destination.
- planner.buffer_line_kinematic(destination, fr_mm_s, active_extruder, cartesian_segment_mm);
- }
-
- #endif // SEGMENT_LEVELED_MOVES
-
- /**
- * Prepare a linear move in a Cartesian setup.
- *
- * When a mesh-based leveling system is active, moves are segmented
- * according to the configuration of the leveling system.
- *
- * Returns true if current_position[] was set to destination[]
- */
- inline bool prepare_move_to_destination_cartesian() {
- #if HAS_MESH
- if (planner.leveling_active && planner.leveling_active_at_z(destination[Z_AXIS])) {
- #if ENABLED(AUTO_BED_LEVELING_UBL)
- ubl.line_to_destination_cartesian(MMS_SCALED(feedrate_mm_s), active_extruder); // UBL's motion routine needs to know about
- return true; // all moves, including Z-only moves.
- #elif ENABLED(SEGMENT_LEVELED_MOVES)
- segmented_line_to_destination(MMS_SCALED(feedrate_mm_s));
- return false; // caller will update current_position
- #else
- /**
- * For MBL and ABL-BILINEAR only segment moves when X or Y are involved.
- * Otherwise fall through to do a direct single move.
- */
- if (current_position[X_AXIS] != destination[X_AXIS] || current_position[Y_AXIS] != destination[Y_AXIS]) {
- #if ENABLED(MESH_BED_LEVELING)
- mesh_line_to_destination(MMS_SCALED(feedrate_mm_s));
- #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
- bilinear_line_to_destination(MMS_SCALED(feedrate_mm_s));
- #endif
- return true;
- }
- #endif
- }
- #endif // HAS_MESH
-
- buffer_line_to_destination(MMS_SCALED(feedrate_mm_s));
- return false; // caller will update current_position
- }
-
- #endif // !IS_KINEMATIC
- #endif // !UBL_SEGMENTED
-
- #if ENABLED(DUAL_X_CARRIAGE) || ENABLED(DUAL_NOZZLE_DUPLICATION_MODE)
- bool extruder_duplication_enabled = false; // Used in Dual X mode 2
- #endif
-
- #if ENABLED(DUAL_X_CARRIAGE)
-
- DualXMode dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE;
- float inactive_extruder_x_pos = X2_MAX_POS, // used in mode 0 & 1
- raised_parked_position[XYZE], // used in mode 1
- duplicate_extruder_x_offset = DEFAULT_DUPLICATION_X_OFFSET; // used in mode 2
- bool active_extruder_parked = false; // used in mode 1 & 2
- millis_t delayed_move_time = 0; // used in mode 1
- int16_t duplicate_extruder_temp_offset = 0; // used in mode 2
-
- float x_home_pos(const int extruder) {
- if (extruder == 0)
- return base_home_pos(X_AXIS);
- else
- /**
- * In dual carriage mode the extruder offset provides an override of the
- * second X-carriage position when homed - otherwise X2_HOME_POS is used.
- * This allows soft recalibration of the second extruder home position
- * without firmware reflash (through the M218 command).
- */
- return hotend_offset[X_AXIS][1] > 0 ? hotend_offset[X_AXIS][1] : X2_HOME_POS;
- }
-
- /**
- * Prepare a linear move in a dual X axis setup
- *
- * Return true if current_position[] was set to destination[]
- */
- inline bool dual_x_carriage_unpark() {
- if (active_extruder_parked) {
- switch (dual_x_carriage_mode) {
- case DXC_FULL_CONTROL_MODE:
- break;
- case DXC_AUTO_PARK_MODE:
- if (current_position[E_AXIS] == destination[E_AXIS]) {
- // This is a travel move (with no extrusion)
- // Skip it, but keep track of the current position
- // (so it can be used as the start of the next non-travel move)
- if (delayed_move_time != 0xFFFFFFFFUL) {
- set_current_from_destination();
- NOLESS(raised_parked_position[Z_AXIS], destination[Z_AXIS]);
- delayed_move_time = millis();
- return true;
- }
- }
- // unpark extruder: 1) raise, 2) move into starting XY position, 3) lower
- for (uint8_t i = 0; i < 3; i++)
- planner.buffer_line(
- i == 0 ? raised_parked_position[X_AXIS] : current_position[X_AXIS],
- i == 0 ? raised_parked_position[Y_AXIS] : current_position[Y_AXIS],
- i == 2 ? current_position[Z_AXIS] : raised_parked_position[Z_AXIS],
- current_position[E_AXIS],
- i == 1 ? PLANNER_XY_FEEDRATE() : planner.max_feedrate_mm_s[Z_AXIS],
- active_extruder
- );
- delayed_move_time = 0;
- active_extruder_parked = false;
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Clear active_extruder_parked");
- #endif
- break;
- case DXC_DUPLICATION_MODE:
- if (active_extruder == 0) {
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- SERIAL_ECHOPAIR("Set planner X", inactive_extruder_x_pos);
- SERIAL_ECHOLNPAIR(" ... Line to X", current_position[X_AXIS] + duplicate_extruder_x_offset);
- }
- #endif
- // move duplicate extruder into correct duplication position.
- planner.set_position_mm(
- inactive_extruder_x_pos,
- current_position[Y_AXIS],
- current_position[Z_AXIS],
- current_position[E_AXIS]
- );
- planner.buffer_line(
- current_position[X_AXIS] + duplicate_extruder_x_offset,
- current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS],
- planner.max_feedrate_mm_s[X_AXIS], 1
- );
- SYNC_PLAN_POSITION_KINEMATIC();
- stepper.synchronize();
- extruder_duplication_enabled = true;
- active_extruder_parked = false;
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Set extruder_duplication_enabled\nClear active_extruder_parked");
- #endif
- }
- else {
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Active extruder not 0");
- #endif
- }
- break;
- }
- }
- return false;
- }
-
- #endif // DUAL_X_CARRIAGE
-
- /**
- * Prepare a single move and get ready for the next one
- *
- * This may result in several calls to planner.buffer_line to
- * do smaller moves for DELTA, SCARA, mesh moves, etc.
- *
- * Make sure current_position[E] and destination[E] are good
- * before calling or cold/lengthy extrusion may get missed.
- */
- void prepare_move_to_destination() {
- clamp_to_software_endstops(destination);
- gcode.refresh_cmd_timeout();
-
- #if ENABLED(PREVENT_COLD_EXTRUSION) || ENABLED(PREVENT_LENGTHY_EXTRUDE)
-
- if (!DEBUGGING(DRYRUN)) {
- if (destination[E_AXIS] != current_position[E_AXIS]) {
- #if ENABLED(PREVENT_COLD_EXTRUSION)
- if (thermalManager.tooColdToExtrude(active_extruder)) {
- current_position[E_AXIS] = destination[E_AXIS]; // Behave as if the move really took place, but ignore E part
- SERIAL_ECHO_START();
- SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
- }
- #endif // PREVENT_COLD_EXTRUSION
- #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
- if (FABS(destination[E_AXIS] - current_position[E_AXIS]) * planner.e_factor[active_extruder] > (EXTRUDE_MAXLENGTH)) {
- current_position[E_AXIS] = destination[E_AXIS]; // Behave as if the move really took place, but ignore E part
- SERIAL_ECHO_START();
- SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
- }
- #endif // PREVENT_LENGTHY_EXTRUDE
- }
- }
-
- #endif // PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE
-
- #if ENABLED(DUAL_X_CARRIAGE)
- if (dual_x_carriage_unpark()) return;
- #endif
-
- if (
- #if UBL_SEGMENTED
- ubl.prepare_segmented_line_to(destination, MMS_SCALED(feedrate_mm_s))
- #elif IS_KINEMATIC
- prepare_kinematic_move_to(destination)
- #else
- prepare_move_to_destination_cartesian()
- #endif
- ) return;
-
- set_current_from_destination();
- }
-
- #if HAS_AXIS_UNHOMED_ERR
-
- bool axis_unhomed_error(const bool x/*=true*/, const bool y/*=true*/, const bool z/*=true*/) {
- #if ENABLED(HOME_AFTER_DEACTIVATE)
- const bool xx = x && !axis_known_position[X_AXIS],
- yy = y && !axis_known_position[Y_AXIS],
- zz = z && !axis_known_position[Z_AXIS];
- #else
- const bool xx = x && !axis_homed[X_AXIS],
- yy = y && !axis_homed[Y_AXIS],
- zz = z && !axis_homed[Z_AXIS];
- #endif
- if (xx || yy || zz) {
- SERIAL_ECHO_START();
- SERIAL_ECHOPGM(MSG_HOME " ");
- if (xx) SERIAL_ECHOPGM(MSG_X);
- if (yy) SERIAL_ECHOPGM(MSG_Y);
- if (zz) SERIAL_ECHOPGM(MSG_Z);
- SERIAL_ECHOLNPGM(" " MSG_FIRST);
-
- #if ENABLED(ULTRA_LCD)
- lcd_status_printf_P(0, PSTR(MSG_HOME " %s%s%s " MSG_FIRST), xx ? MSG_X : "", yy ? MSG_Y : "", zz ? MSG_Z : "");
- #endif
- return true;
- }
- return false;
- }
-
- #endif // HAS_AXIS_UNHOMED_ERR
-
- /**
- * The homing feedrate may vary
- */
- inline float get_homing_bump_feedrate(const AxisEnum axis) {
- static const uint8_t homing_bump_divisor[] PROGMEM = HOMING_BUMP_DIVISOR;
- uint8_t hbd = pgm_read_byte(&homing_bump_divisor[axis]);
- if (hbd < 1) {
- hbd = 10;
- SERIAL_ECHO_START();
- SERIAL_ECHOLNPGM("Warning: Homing Bump Divisor < 1");
- }
- return homing_feedrate(axis) / hbd;
- }
-
- /**
- * Home an individual linear axis
- */
- static void do_homing_move(const AxisEnum axis, const float distance, const float fr_mm_s=0.0) {
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- SERIAL_ECHOPAIR(">>> do_homing_move(", axis_codes[axis]);
- SERIAL_ECHOPAIR(", ", distance);
- SERIAL_ECHOPAIR(", ", fr_mm_s);
- SERIAL_CHAR(')');
- SERIAL_EOL();
- }
- #endif
-
- #if HOMING_Z_WITH_PROBE && ENABLED(BLTOUCH)
- const bool deploy_bltouch = (axis == Z_AXIS && distance < 0);
- if (deploy_bltouch) set_bltouch_deployed(true);
- #endif
-
- #if QUIET_PROBING
- if (axis == Z_AXIS) probing_pause(true);
- #endif
-
- // Tell the planner the axis is at 0
- current_position[axis] = 0;
-
- #if IS_SCARA
- SYNC_PLAN_POSITION_KINEMATIC();
- current_position[axis] = distance;
- inverse_kinematics(current_position);
- planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder);
- #else
- sync_plan_position();
- current_position[axis] = distance;
- planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder);
- #endif
-
- stepper.synchronize();
-
- #if QUIET_PROBING
- if (axis == Z_AXIS) probing_pause(false);
- #endif
-
- #if HOMING_Z_WITH_PROBE && ENABLED(BLTOUCH)
- if (deploy_bltouch) set_bltouch_deployed(false);
- #endif
-
- endstops.hit_on_purpose();
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- SERIAL_ECHOPAIR("<<< do_homing_move(", axis_codes[axis]);
- SERIAL_CHAR(')');
- SERIAL_EOL();
- }
- #endif
- }
-
- /**
- * Set an axis' current position to its home position (after homing).
- *
- * For Core and Cartesian robots this applies one-to-one when an
- * individual axis has been homed.
- *
- * DELTA should wait until all homing is done before setting the XYZ
- * current_position to home, because homing is a single operation.
- * In the case where the axis positions are already known and previously
- * homed, DELTA could home to X or Y individually by moving either one
- * to the center. However, homing Z always homes XY and Z.
- *
- * SCARA should wait until all XY homing is done before setting the XY
- * current_position to home, because neither X nor Y is at home until
- * both are at home. Z can however be homed individually.
- *
- * Callers must sync the planner position after calling this!
- */
- void set_axis_is_at_home(const AxisEnum axis) {
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- SERIAL_ECHOPAIR(">>> set_axis_is_at_home(", axis_codes[axis]);
- SERIAL_CHAR(')');
- SERIAL_EOL();
- }
- #endif
-
- axis_known_position[axis] = axis_homed[axis] = true;
-
- #if HAS_POSITION_SHIFT
- position_shift[axis] = 0;
- update_software_endstops(axis);
- #endif
-
- #if ENABLED(DUAL_X_CARRIAGE)
- if (axis == X_AXIS && (active_extruder == 1 || dual_x_carriage_mode == DXC_DUPLICATION_MODE)) {
- current_position[X_AXIS] = x_home_pos(active_extruder);
- return;
- }
- #endif
-
- #if ENABLED(MORGAN_SCARA)
- scara_set_axis_is_at_home(axis);
- #elif ENABLED(DELTA)
- current_position[axis] = (axis == Z_AXIS ? delta_height : base_home_pos(axis));
- #else
- current_position[axis] = base_home_pos(axis);
- #endif
-
- /**
- * Z Probe Z Homing? Account for the probe's Z offset.
- */
- #if HAS_BED_PROBE && Z_HOME_DIR < 0
- if (axis == Z_AXIS) {
- #if HOMING_Z_WITH_PROBE
-
- current_position[Z_AXIS] -= zprobe_zoffset;
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- SERIAL_ECHOLNPGM("*** Z HOMED WITH PROBE (Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN) ***");
- SERIAL_ECHOLNPAIR("> zprobe_zoffset = ", zprobe_zoffset);
- }
- #endif
-
- #elif ENABLED(DEBUG_LEVELING_FEATURE)
-
- if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("*** Z HOMED TO ENDSTOP (Z_MIN_PROBE_ENDSTOP) ***");
-
- #endif
- }
- #endif
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- #if HAS_HOME_OFFSET
- SERIAL_ECHOPAIR("> home_offset[", axis_codes[axis]);
- SERIAL_ECHOLNPAIR("] = ", home_offset[axis]);
- #endif
- DEBUG_POS("", current_position);
- SERIAL_ECHOPAIR("<<< set_axis_is_at_home(", axis_codes[axis]);
- SERIAL_CHAR(')');
- SERIAL_EOL();
- }
- #endif
-
- #if ENABLED(I2C_POSITION_ENCODERS)
- I2CPEM.homed(axis);
- #endif
- }
-
- /**
- * Home an individual "raw axis" to its endstop.
- * This applies to XYZ on Cartesian and Core robots, and
- * to the individual ABC steppers on DELTA and SCARA.
- *
- * At the end of the procedure the axis is marked as
- * homed and the current position of that axis is updated.
- * Kinematic robots should wait till all axes are homed
- * before updating the current position.
- */
-
- void homeaxis(const AxisEnum axis) {
-
- #if IS_SCARA
- // Only Z homing (with probe) is permitted
- if (axis != Z_AXIS) { BUZZ(100, 880); return; }
- #else
- #define CAN_HOME(A) \
- (axis == A##_AXIS && ((A##_MIN_PIN > -1 && A##_HOME_DIR < 0) || (A##_MAX_PIN > -1 && A##_HOME_DIR > 0)))
- if (!CAN_HOME(X) && !CAN_HOME(Y) && !CAN_HOME(Z)) return;
- #endif
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- SERIAL_ECHOPAIR(">>> homeaxis(", axis_codes[axis]);
- SERIAL_CHAR(')');
- SERIAL_EOL();
- }
- #endif
-
- const int axis_home_dir =
- #if ENABLED(DUAL_X_CARRIAGE)
- (axis == X_AXIS) ? x_home_dir(active_extruder) :
- #endif
- home_dir(axis);
-
- // Homing Z towards the bed? Deploy the Z probe or endstop.
- #if HOMING_Z_WITH_PROBE
- if (axis == Z_AXIS && DEPLOY_PROBE()) return;
- #endif
-
- // Set flags for X, Y, Z motor locking
- #if ENABLED(X_DUAL_ENDSTOPS)
- if (axis == X_AXIS) stepper.set_homing_flag_x(true);
- #endif
- #if ENABLED(Y_DUAL_ENDSTOPS)
- if (axis == Y_AXIS) stepper.set_homing_flag_y(true);
- #endif
- #if ENABLED(Z_DUAL_ENDSTOPS)
- if (axis == Z_AXIS) stepper.set_homing_flag_z(true);
- #endif
-
- // Disable stealthChop if used. Enable diag1 pin on driver.
- #if ENABLED(SENSORLESS_HOMING)
- #if ENABLED(X_IS_TMC2130)
- if (axis == X_AXIS) tmc_sensorless_homing(stepperX);
- #endif
- #if ENABLED(Y_IS_TMC2130)
- if (axis == Y_AXIS) tmc_sensorless_homing(stepperY);
- #endif
- #endif
-
- // Fast move towards endstop until triggered
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Home 1 Fast:");
- #endif
- do_homing_move(axis, 1.5 * max_length(axis) * axis_home_dir);
-
- // When homing Z with probe respect probe clearance
- const float bump = axis_home_dir * (
- #if HOMING_Z_WITH_PROBE
- (axis == Z_AXIS) ? max(Z_CLEARANCE_BETWEEN_PROBES, home_bump_mm(Z_AXIS)) :
- #endif
- home_bump_mm(axis)
- );
-
- // If a second homing move is configured...
- if (bump) {
- // Move away from the endstop by the axis HOME_BUMP_MM
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Move Away:");
- #endif
- do_homing_move(axis, -bump);
-
- // Slow move towards endstop until triggered
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Home 2 Slow:");
- #endif
- do_homing_move(axis, 2 * bump, get_homing_bump_feedrate(axis));
- }
-
- #if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
- const bool pos_dir = axis_home_dir > 0;
- #if ENABLED(X_DUAL_ENDSTOPS)
- if (axis == X_AXIS) {
- const bool lock_x1 = pos_dir ? (endstops.x_endstop_adj > 0) : (endstops.x_endstop_adj < 0);
- float adj = FABS(endstops.x_endstop_adj);
- if (pos_dir) adj = -adj;
- if (lock_x1) stepper.set_x_lock(true); else stepper.set_x2_lock(true);
- do_homing_move(axis, adj);
- if (lock_x1) stepper.set_x_lock(false); else stepper.set_x2_lock(false);
- stepper.set_homing_flag_x(false);
- }
- #endif
- #if ENABLED(Y_DUAL_ENDSTOPS)
- if (axis == Y_AXIS) {
- const bool lock_y1 = pos_dir ? (endstops.y_endstop_adj > 0) : (endstops.y_endstop_adj < 0);
- float adj = FABS(endstops.y_endstop_adj);
- if (pos_dir) adj = -adj;
- if (lock_y1) stepper.set_y_lock(true); else stepper.set_y2_lock(true);
- do_homing_move(axis, adj);
- if (lock_y1) stepper.set_y_lock(false); else stepper.set_y2_lock(false);
- stepper.set_homing_flag_y(false);
- }
- #endif
- #if ENABLED(Z_DUAL_ENDSTOPS)
- if (axis == Z_AXIS) {
- const bool lock_z1 = pos_dir ? (endstops.z_endstop_adj > 0) : (endstops.z_endstop_adj < 0);
- float adj = FABS(endstops.z_endstop_adj);
- if (pos_dir) adj = -adj;
- if (lock_z1) stepper.set_z_lock(true); else stepper.set_z2_lock(true);
- do_homing_move(axis, adj);
- if (lock_z1) stepper.set_z_lock(false); else stepper.set_z2_lock(false);
- stepper.set_homing_flag_z(false);
- }
- #endif
- #endif
-
- #if IS_SCARA
-
- set_axis_is_at_home(axis);
- SYNC_PLAN_POSITION_KINEMATIC();
-
- #elif ENABLED(DELTA)
-
- // Delta has already moved all three towers up in G28
- // so here it re-homes each tower in turn.
- // Delta homing treats the axes as normal linear axes.
-
- // retrace by the amount specified in delta_endstop_adj + additional 0.1mm in order to have minimum steps
- if (delta_endstop_adj[axis] * Z_HOME_DIR <= 0) {
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("delta_endstop_adj:");
- #endif
- do_homing_move(axis, delta_endstop_adj[axis] - 0.1 * Z_HOME_DIR);
- }
-
- #else
-
- // For cartesian/core machines,
- // set the axis to its home position
- set_axis_is_at_home(axis);
- sync_plan_position();
-
- destination[axis] = current_position[axis];
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) DEBUG_POS("> AFTER set_axis_is_at_home", current_position);
- #endif
-
- #endif
-
- // Re-enable stealthChop if used. Disable diag1 pin on driver.
- #if ENABLED(SENSORLESS_HOMING)
- #if ENABLED(X_IS_TMC2130)
- if (axis == X_AXIS) tmc_sensorless_homing(stepperX, false);
- #endif
- #if ENABLED(Y_IS_TMC2130)
- if (axis == Y_AXIS) tmc_sensorless_homing(stepperY, false);
- #endif
- #endif
-
- // Put away the Z probe
- #if HOMING_Z_WITH_PROBE
- if (axis == Z_AXIS && STOW_PROBE()) return;
- #endif
-
- // Clear retracted status if homing the Z axis
- #if ENABLED(FWRETRACT)
- if (axis == Z_AXIS)
- for (uint8_t i = 0; i < EXTRUDERS; i++) fwretract.retracted[i] = false;
- #endif
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- SERIAL_ECHOPAIR("<<< homeaxis(", axis_codes[axis]);
- SERIAL_CHAR(')');
- SERIAL_EOL();
- }
- #endif
- } // homeaxis()
-
- #if HAS_WORKSPACE_OFFSET || ENABLED(DUAL_X_CARRIAGE) || ENABLED(DELTA)
-
- /**
- * 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) {
- #if HAS_HOME_OFFSET && HAS_POSITION_SHIFT
- workspace_offset[axis] = home_offset[axis] + position_shift[axis];
- #endif
-
- #if ENABLED(DUAL_X_CARRIAGE)
- if (axis == X_AXIS) {
-
- // In Dual X mode hotend_offset[X] is T1's home position
- float dual_max_x = max(hotend_offset[X_AXIS][1], X2_MAX_POS);
-
- if (active_extruder != 0) {
- // T1 can move from X2_MIN_POS to X2_MAX_POS or X2 home position (whichever is larger)
- soft_endstop_min[X_AXIS] = X2_MIN_POS;
- soft_endstop_max[X_AXIS] = dual_max_x;
- }
- else if (dual_x_carriage_mode == DXC_DUPLICATION_MODE) {
- // In Duplication Mode, T0 can move as far left as X_MIN_POS
- // but not so far to the right that T1 would move past the end
- soft_endstop_min[X_AXIS] = base_min_pos(X_AXIS);
- soft_endstop_max[X_AXIS] = min(base_max_pos(X_AXIS), dual_max_x - duplicate_extruder_x_offset);
- }
- else {
- // In other modes, T0 can move from X_MIN_POS to X_MAX_POS
- soft_endstop_min[axis] = base_min_pos(axis);
- soft_endstop_max[axis] = base_max_pos(axis);
- }
- }
- #elif ENABLED(DELTA)
- soft_endstop_min[axis] = base_min_pos(axis);
- soft_endstop_max[axis] = (axis == Z_AXIS ? delta_height : base_max_pos(axis));
- #else
- soft_endstop_min[axis] = base_min_pos(axis);
- soft_endstop_max[axis] = base_max_pos(axis);
- #endif
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- SERIAL_ECHOPAIR("For ", axis_codes[axis]);
- #if HAS_HOME_OFFSET
- SERIAL_ECHOPAIR(" axis:\n home_offset = ", home_offset[axis]);
- #endif
- #if HAS_POSITION_SHIFT
- SERIAL_ECHOPAIR("\n position_shift = ", position_shift[axis]);
- #endif
- SERIAL_ECHOPAIR("\n soft_endstop_min = ", soft_endstop_min[axis]);
- SERIAL_ECHOLNPAIR("\n soft_endstop_max = ", soft_endstop_max[axis]);
- }
- #endif
-
- #if ENABLED(DELTA)
- switch(axis) {
- #if HAS_SOFTWARE_ENDSTOPS
- case X_AXIS:
- case Y_AXIS:
- // Get a minimum radius for clamping
- soft_endstop_radius = MIN3(FABS(max(soft_endstop_min[X_AXIS], soft_endstop_min[Y_AXIS])), soft_endstop_max[X_AXIS], soft_endstop_max[Y_AXIS]);
- soft_endstop_radius_2 = sq(soft_endstop_radius);
- break;
- #endif
- case Z_AXIS:
- delta_clip_start_height = soft_endstop_max[axis] - delta_safe_distance_from_top();
- default: break;
- }
- #endif
- }
-
- #endif // HAS_WORKSPACE_OFFSET || DUAL_X_CARRIAGE || DELTA
-
- #if HAS_M206_COMMAND
- /**
- * Change the home offset for an axis.
- * Also refreshes the workspace offset.
- */
- void set_home_offset(const AxisEnum axis, const float v) {
- home_offset[axis] = v;
- update_software_endstops(axis);
- }
- #endif // HAS_M206_COMMAND
|