Merge pull request #8847 from thinkyhead/bf1_restore_position_float

[1.1.x] Restore position_float to LIN_ADVANCE
6 years ago · 2aaf66b789
parent 4f375cd17d 477e36afab
commit 2aaf66b789
3 changed files with 133 additions and 24 deletions
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@ -491,6 +491,10 @@ static_assert(X_MAX_LENGTH >= X_BED_SIZE && Y_MAX_LENGTH >= Y_BED_SIZE,
  #endif
 #endif

+#if ENABLED(LIN_ADVANCE) && !IS_CARTESIAN
+  #error "Sorry! LIN_ADVANCE is only compatible with Cartesian."
+#endif
+
 /**
 * Parking Extruder requirements
 */
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@ -171,7 +171,10 @@ float Planner::previous_speed[NUM_AXIS],

 #if ENABLED(LIN_ADVANCE)
  float Planner::extruder_advance_k, // Initialized by settings.load()
-        Planner::advance_ed_ratio;   // Initialized by settings.load()
+        Planner::advance_ed_ratio,   // Initialized by settings.load()
+        Planner::position_float[XYZE], // Needed for accurate maths. Steps cannot be used!
+        Planner::lin_dist_xy,
+        Planner::lin_dist_e;
 #endif

 #if ENABLED(ULTRA_LCD)
@ -187,6 +190,9 @@ Planner::Planner() { init(); }
 void Planner::init() {
  block_buffer_head = block_buffer_tail = 0;
  ZERO(position);
+  #if ENABLED(LIN_ADVANCE)
+    ZERO(position_float);
+  #endif
  ZERO(previous_speed);
  previous_nominal_speed = 0.0;
  #if ABL_PLANAR
@ -731,7 +737,9 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
    SERIAL_ECHOLNPGM(" steps)");
  //*/

-  #if ENABLED(PREVENT_COLD_EXTRUSION) || ENABLED(PREVENT_LENGTHY_EXTRUDE)
+  // If LIN_ADVANCE is disabled then do E move prevention with integers
+  // Otherwise it's done in _buffer_segment.
+  #if DISABLED(LIN_ADVANCE) && (ENABLED(PREVENT_COLD_EXTRUSION) || ENABLED(PREVENT_LENGTHY_EXTRUDE))
    if (de) {
      #if ENABLED(PREVENT_COLD_EXTRUSION)
        if (thermalManager.tooColdToExtrude(extruder)) {
@ -750,7 +758,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
        }
      #endif // PREVENT_LENGTHY_EXTRUDE
    }
-  #endif // PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE
+  #endif // !LIN_ADVANCE && (PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE)

  // Compute direction bit-mask for this block
  uint8_t dm = 0;
@ -1057,6 +1065,9 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
    CRITICAL_SECTION_END
  #endif

+  block->nominal_speed = block->millimeters * inverse_secs;           //   (mm/sec) Always > 0
+  block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0
+
  #if ENABLED(FILAMENT_WIDTH_SENSOR)
    static float filwidth_e_count = 0, filwidth_delay_dist = 0;

@ -1091,14 +1102,10 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
    }
  #endif

-  // Calculate and limit speed in mm/sec for each axis, calculate minimum acceleration ratio
+  // Calculate and limit speed in mm/sec for each axis
  float current_speed[NUM_AXIS], speed_factor = 1.0; // factor <1 decreases speed
-  float max_stepper_speed = 0, min_axis_accel_ratio = 1; // ratio < 1 means acceleration ramp needed
  LOOP_XYZE(i) {
    const float cs = FABS((current_speed[i] = delta_mm[i] * inverse_secs));
-    if (cs > max_jerk[i])
-      NOMORE(min_axis_accel_ratio, max_jerk[i] / cs);
-    NOLESS(max_stepper_speed, cs);
    #if ENABLED(DISTINCT_E_FACTORS)
      if (i == E_AXIS) i += extruder;
    #endif
@ -1143,9 +1150,6 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
    }
  #endif // XY_FREQUENCY_LIMIT

-  block->nominal_speed = max_stepper_speed; // (mm/sec) Always > 0
-  block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0
-
  // Correct the speed
  if (speed_factor < 1.0) {
    LOOP_XYZE(i) current_speed[i] *= speed_factor;
@ -1153,9 +1157,6 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
    block->nominal_rate *= speed_factor;
  }

-  float safe_speed = block->nominal_speed * min_axis_accel_ratio;
-  static float previous_safe_speed;
-
  // Compute and limit the acceleration rate for the trapezoid generator.
  const float steps_per_mm = block->step_event_count * inverse_millimeters;
  uint32_t accel;
@ -1257,6 +1258,32 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
    }
  #endif

+  /**
+   * Adapted from Průša MKS firmware
+   * https://github.com/prusa3d/Prusa-Firmware
+   *
+   * Start with a safe speed (from which the machine may halt to stop immediately).
+   */
+
+  // Exit speed limited by a jerk to full halt of a previous last segment
+  static float previous_safe_speed;
+
+  float safe_speed = block->nominal_speed;
+  uint8_t limited = 0;
+  LOOP_XYZE(i) {
+    const float jerk = FABS(current_speed[i]), maxj = max_jerk[i];
+    if (jerk > maxj) {
+      if (limited) {
+        const float mjerk = maxj * block->nominal_speed;
+        if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk;
+      }
+      else {
+        ++limited;
+        safe_speed = maxj;
+      }
+    }
+  }
+
  if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) {
    // Estimate a maximum velocity allowed at a joint of two successive segments.
    // If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities,
@ -1268,7 +1295,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const

    // Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
    float v_factor = 1;
-    uint8_t limited = 0;
+    limited = 0;

    // Now limit the jerk in all axes.
    const float smaller_speed_factor = vmax_junction / previous_nominal_speed;
@ -1344,16 +1371,16 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
     *                                      In that case, the retract and move will be executed together.
     *                                      This leads to too many advance steps due to a huge e_acceleration.
     *                                      The math is good, but we must avoid retract moves with advance!
-     * de > 0                             : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves)
+     * lin_dist_e > 0                       : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves)
     */
    block->use_advance_lead =  esteps && (block->steps[X_AXIS] || block->steps[Y_AXIS])
                            && extruder_advance_k
                            && (uint32_t)esteps != block->step_event_count
-                            && de > 0;
+                            && lin_dist_e > 0;
    if (block->use_advance_lead)
      block->abs_adv_steps_multiplier8 = LROUND(
        extruder_advance_k
-        * (UNEAR_ZERO(advance_ed_ratio) ? de * steps_to_mm[E_AXIS_N] / HYPOT(da * steps_to_mm[X_AXIS], db * steps_to_mm[Y_AXIS]) : advance_ed_ratio) // Use the fixed ratio, if set
+        * (UNEAR_ZERO(advance_ed_ratio) ? lin_dist_e / lin_dist_xy : advance_ed_ratio) // Use the fixed ratio, if set
        * (block->nominal_speed / (float)block->nominal_rate)
        * axis_steps_per_mm[E_AXIS_N] * 256.0
      );
@ -1403,6 +1430,48 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con
    LROUND(e * axis_steps_per_mm[E_AXIS_N])
  };

+  // DRYRUN prevents E moves from taking place
+  if (DEBUGGING(DRYRUN)) {
+    position[E_AXIS] = target[E_AXIS];
+    #if ENABLED(LIN_ADVANCE)
+      position_float[E_AXIS] = e;
+    #endif
+  }
+
+  #if ENABLED(LIN_ADVANCE)
+    lin_dist_e = e - position_float[E_AXIS];
+  #endif
+
+  // If LIN_ADVANCE is enabled then do E move prevention with floats
+  // Otherwise it's done in _buffer_steps.
+  #if ENABLED(LIN_ADVANCE) && (ENABLED(PREVENT_COLD_EXTRUSION) || ENABLED(PREVENT_LENGTHY_EXTRUDE))
+    if (lin_dist_e) {
+      #if ENABLED(PREVENT_COLD_EXTRUSION)
+        if (thermalManager.tooColdToExtrude(extruder)) {
+          position_float[E_AXIS] = e; // Behave as if the move really took place, but ignore E part
+          position[E_AXIS] = target[E_AXIS];
+          lin_dist_e = 0;
+          SERIAL_ECHO_START();
+          SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
+        }
+      #endif // PREVENT_COLD_EXTRUSION
+      #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
+        if (lin_dist_e * e_factor[extruder] > (EXTRUDE_MAXLENGTH)) {
+          position_float[E_AXIS] = e; // Behave as if the move really took place, but ignore E part
+          position[E_AXIS] = target[E_AXIS];
+          lin_dist_e = 0;
+          SERIAL_ECHO_START();
+          SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
+        }
+      #endif // PREVENT_LENGTHY_EXTRUDE
+    }
+  #endif // LIN_ADVANCE && (PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE)
+
+  #if ENABLED(LIN_ADVANCE)
+    if (lin_dist_e > 0)
+      lin_dist_xy = HYPOT(a - position_float[X_AXIS], b - position_float[Y_AXIS]);
+  #endif
+
  /* <-- add a slash to enable
    SERIAL_ECHOPAIR("  buffer_segment FR:", fr_mm_s);
    #if IS_KINEMATIC
@ -1431,16 +1500,27 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con
    SERIAL_ECHOLNPGM(")");
  //*/

-  // DRYRUN ignores all temperature constraints and assures that the extruder is instantly satisfied
-  if (DEBUGGING(DRYRUN))
-    position[E_AXIS] = target[E_AXIS];
-
  // Always split the first move into two (if not homing or probing)
  if (!blocks_queued()) {
+
    #define _BETWEEN(A) (position[A##_AXIS] + target[A##_AXIS]) >> 1
    const int32_t between[XYZE] = { _BETWEEN(X), _BETWEEN(Y), _BETWEEN(Z), _BETWEEN(E) };
    DISABLE_STEPPER_DRIVER_INTERRUPT();
+
+    #if ENABLED(LIN_ADVANCE)
+      lin_dist_xy *= 0.5;
+      lin_dist_e *= 0.5;
+    #endif
+
    _buffer_steps(between, fr_mm_s, extruder);
+
+    #if ENABLED(LIN_ADVANCE)
+      position_float[X_AXIS] = (position_float[X_AXIS] + a) * 0.5;
+      position_float[Y_AXIS] = (position_float[Y_AXIS] + b) * 0.5;
+      //position_float[Z_AXIS] = (position_float[Z_AXIS] + c) * 0.5;
+      position_float[E_AXIS] = (position_float[E_AXIS] + e) * 0.5;
+    #endif
+
    const uint8_t next = block_buffer_head;
    _buffer_steps(target, fr_mm_s, extruder);
    SBI(block_buffer[next].flag, BLOCK_BIT_CONTINUED);
@ -1451,6 +1531,12 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con

  stepper.wake_up();

+  #if ENABLED(LIN_ADVANCE)
+    position_float[X_AXIS] = a;
+    position_float[Y_AXIS] = b;
+    //position_float[Z_AXIS] = c;
+    position_float[E_AXIS] = e;
+  #endif
 } // buffer_segment()

 /**
@ -1471,6 +1557,12 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
                nb = position[Y_AXIS] = LROUND(b * axis_steps_per_mm[Y_AXIS]),
                nc = position[Z_AXIS] = LROUND(c * axis_steps_per_mm[Z_AXIS]),
                ne = position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]);
+  #if ENABLED(LIN_ADVANCE)
+    position_float[X_AXIS] = a;
+    position_float[Y_AXIS] = b;
+    //position_float[Z_AXIS] = c;
+    position_float[E_AXIS] = e;
+  #endif
  stepper.set_position(na, nb, nc, ne);
  previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
  ZERO(previous_speed);
@ -1495,8 +1587,16 @@ void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) {
 * Sync from the stepper positions. (e.g., after an interrupted move)
 */
 void Planner::sync_from_steppers() {
-  LOOP_XYZE(i)
+  LOOP_XYZE(i) {
    position[i] = stepper.position((AxisEnum)i);
+    #if ENABLED(LIN_ADVANCE)
+      position_float[i] = position[i] * steps_to_mm[i
+        #if ENABLED(DISTINCT_E_FACTORS)
+          + (i == E_AXIS ? active_extruder : 0)
+        #endif
+      ];
+    #endif
+  }
 }

 /**
@ -1510,6 +1610,9 @@ void Planner::set_position_mm(const AxisEnum axis, const float &v) {
    const uint8_t axis_index = axis;
  #endif
  position[axis] = LROUND(v * axis_steps_per_mm[axis_index]);
+  #if ENABLED(LIN_ADVANCE)
+    position_float[axis] = v;
+  #endif
  stepper.set_position(axis, v);
  previous_speed[axis] = 0.0;
 }
--- a/Marlin/planner.h
+++ b/Marlin/planner.h
@ -191,7 +191,9 @@ class Planner {
    #endif

    #if ENABLED(LIN_ADVANCE)
-      static float extruder_advance_k, advance_ed_ratio;
+      static float extruder_advance_k, advance_ed_ratio,
+                   position_float[XYZE],
+                   lin_dist_xy, lin_dist_e;
    #endif

    #if ENABLED(SKEW_CORRECTION)