extruder: Implement non-linear extrusion

This allows klipper to compensate for slippage in the extruder at high back-pressures.
When enabled through SET_NONLINEAR_EXTRUSION, or nonlinear_a/b in the configuration,
the actual extrusion velocity will be modified to v(t) + A*v(t)^2 + B*v(t)^3
where v(t) is the requested velocity, and A/B the provided parameters.

This is similar to RepRap's M592 command, except correctly applied to accelerating moves
rather than simply scaling the distance of the entire move based on its average velocity.

Signed-off-by: Tobias Haegermarck <tobias.haegermarck@gmail.com>

klippy/chelper/__init__.py

@@ -113,6 +113,8 @@
     struct stepper_kinematics *extruder_stepper_alloc(void);
     void extruder_set_smooth_time(struct stepper_kinematics *sk
         , double smooth_time);
+    void extruder_set_nonlinear(struct stepper_kinematics *sk, double a
+        , double b);
 """
 
 defs_kin_shaper = """

klippy/chelper/itersolve.c

@@ -174,6 +174,9 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
                                                             , flush_time);
                     if (ret)
                         return ret;
+
+                    if (sk->post_move_cb)
+                        sk->post_move_cb(sk, pm);
                     pm = list_next_entry(pm, node);
                 } while (pm != m);
             }
@@ -207,6 +210,9 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
             if (flush_time + sk->gen_steps_pre_active <= move_end)
                 return 0;
         }
+
+        if (sk->post_move_cb)
+            sk->post_move_cb(sk, m);
         m = list_next_entry(m, node);
     }
 }

klippy/chelper/itersolve.h

@@ -11,6 +11,7 @@ struct stepper_kinematics;
 struct move;
 typedef double (*sk_calc_callback)(struct stepper_kinematics *sk, struct move *m
                                    , double move_time);
+typedef void (*sk_post_move)(struct stepper_kinematics *sk, struct move *m);
 typedef void (*sk_post_callback)(struct stepper_kinematics *sk);
 struct stepper_kinematics {
     double step_dist, commanded_pos;
@@ -22,6 +23,7 @@ struct stepper_kinematics {
     double gen_steps_pre_active, gen_steps_post_active;
 
     sk_calc_callback calc_position_cb;
+    sk_post_move post_move_cb;
     sk_post_callback post_cb;
 };
 

klippy/chelper/kin_extruder.c

@@ -97,23 +97,62 @@ pa_range_integrate(struct move *m, double move_time, double hst)
     return res;
 }
 
+// Calculate the additional non-linear extrusion movement over a move
+static double
+nonlinear_extrusion_integrate(struct move *m, double move_time, double a, double b)
+{
+    double t2 = move_time * move_time;
+    double t3 = t2 * move_time;
+    double t4 = t3 * move_time;
+    double sv2 = m->start_v * m->start_v;
+    double sv3 = sv2 * m->start_v;
+
+    // Integral of the intended velocity squared
+    double v2 = sv2 * move_time
+        + m->start_v * m->half_accel * t2
+        + m->half_accel * m->half_accel * t3 * (4.0 / 3.0);
+
+    // Integral of the intended velocity cubed
+    double v3 = sv3 * move_time
+        + sv2 * m->half_accel * t2 * 3.0
+        + m->half_accel * m->half_accel * m->start_v  * t3 * 4.0
+        + m->half_accel * m->half_accel * m->half_accel * t4 * 2.0;
+
+    return a * v2 + b * v3;
+}
+
 struct extruder_stepper {
     struct stepper_kinematics sk;
     double half_smooth_time, inv_half_smooth_time2;
+    double added_travel; // Total amount of extra movement added by nonlinear extrusion
+    double nonlinear_a, nonlinear_b;
 };
 
 static double
 extruder_calc_position(struct stepper_kinematics *sk, struct move *m
                        , double move_time)
 {
     struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk);
+    double pos = m->start_pos.x + es->added_travel;
     double hst = es->half_smooth_time;
-    if (!hst)
-        // Pressure advance not enabled
-        return m->start_pos.x + move_get_distance(m, move_time);
-    // Apply pressure advance and average over smooth_time
-    double area = pa_range_integrate(m, move_time, hst);
-    return m->start_pos.x + area * es->inv_half_smooth_time2;
+    if (hst) {
+        // Apply pressure advance and average over smooth_time
+        double area = pa_range_integrate(m, move_time, hst);
+        pos += area * es->inv_half_smooth_time2;
+    } else // Pressure advance not enabled
+        pos += move_get_distance(m, move_time);
+
+    if (es->nonlinear_a || es->nonlinear_b)
+        pos += nonlinear_extrusion_integrate(m, move_time, es->nonlinear_a, es->nonlinear_b);
+    return pos;
+}
+
+static void
+extruder_post_move(struct stepper_kinematics *sk, struct move *m)
+{
+    struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk);
+    if (es->nonlinear_a || es->nonlinear_b)
+        es->added_travel += nonlinear_extrusion_integrate(m, m->move_t, es->nonlinear_a, es->nonlinear_b);
 }
 
 void __visible
@@ -128,12 +167,20 @@ extruder_set_smooth_time(struct stepper_kinematics *sk, double smooth_time)
     es->inv_half_smooth_time2 = 1. / (hst * hst);
 }
 
+void __visible
+extruder_set_nonlinear(struct stepper_kinematics *sk, double a, double b)
+{
+    struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk);
+    es->nonlinear_a = a;
+    es->nonlinear_b = b;
+}
 struct stepper_kinematics * __visible
 extruder_stepper_alloc(void)
 {
     struct extruder_stepper *es = malloc(sizeof(*es));
     memset(es, 0, sizeof(*es));
     es->sk.calc_position_cb = extruder_calc_position;
+    es->sk.post_move_cb = extruder_post_move;
     es->sk.active_flags = AF_X;
     return &es->sk;
 }

klippy/kinematics/extruder.py

@@ -45,6 +45,11 @@ def __init__(self, config, extruder_num):
         pressure_advance = config.getfloat('pressure_advance', 0., minval=0.)
         smooth_time = config.getfloat('pressure_advance_smooth_time',
                                       0.040, above=0., maxval=.200)
+
+        self.nonlinear_a = self.nonlinear_b = 0.
+        nonlinear_a = config.getfloat('nonlinear_a', 0.)
+        nonlinear_b = config.getfloat('nonlinear_b', 0.)
+
         # Setup iterative solver
         ffi_main, ffi_lib = chelper.get_ffi()
         self.trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free)
@@ -56,7 +61,9 @@ def __init__(self, config, extruder_num):
         self.stepper.set_trapq(self.trapq)
         toolhead.register_step_generator(self.stepper.generate_steps)
         self.extruder_set_smooth_time = ffi_lib.extruder_set_smooth_time
+        self.extruder_set_nonlinear = ffi_lib.extruder_set_nonlinear
         self._set_pressure_advance(pressure_advance, smooth_time)
+        self._set_nonlinear_extrusion(nonlinear_a, nonlinear_b)
         # Register commands
         gcode = self.printer.lookup_object('gcode')
         if self.name == 'extruder':
@@ -66,6 +73,9 @@ def __init__(self, config, extruder_num):
             gcode.register_mux_command("SET_PRESSURE_ADVANCE", "EXTRUDER", None,
                                        self.cmd_default_SET_PRESSURE_ADVANCE,
                                        desc=self.cmd_SET_PRESSURE_ADVANCE_help)
+        gcode.register_mux_command("SET_NONLINEAR_EXTRUSION", "EXTRUDER",
+                                   self.name, self.cmd_SET_NONLINEAR_EXTRUSION,
+                                   desc=self.cmd_SET_NONLINEAR_EXTRUSION_help)
         gcode.register_mux_command("SET_PRESSURE_ADVANCE", "EXTRUDER",
                                    self.name, self.cmd_SET_PRESSURE_ADVANCE,
                                    desc=self.cmd_SET_PRESSURE_ADVANCE_help)
@@ -90,6 +100,10 @@ def _set_pressure_advance(self, pressure_advance, smooth_time):
         self.extruder_set_smooth_time(self.sk_extruder, new_smooth_time)
         self.pressure_advance = pressure_advance
         self.pressure_advance_smooth_time = smooth_time
+    def _set_nonlinear_extrusion(self, a, b):
+        self.extruder_set_nonlinear(self.sk_extruder, a, b)
+        self.nonlinear_a = a
+        self.nonlinear_b = b
     def get_status(self, eventtime):
         return dict(self.heater.get_status(eventtime),
                     pressure_advance=self.pressure_advance,
@@ -192,6 +206,16 @@ def cmd_SET_PRESSURE_ADVANCE(self, gcmd):
                % (pressure_advance, smooth_time))
         self.printer.set_rollover_info(self.name, "%s: %s" % (self.name, msg))
         gcmd.respond_info(msg, log=False)
+    cmd_SET_NONLINEAR_EXTRUSION_help = "Set nonlinear extrusion parameters"
+    def cmd_SET_NONLINEAR_EXTRUSION(self, gcmd):
+        a = gcmd.get_float('A', self.nonlinear_a)
+        b = gcmd.get_float('B', self.nonlinear_b)
+        self._set_nonlinear_extrusion(a, b)
+        msg = ("a: %.6f\n"
+               "b: %.6f"
+               % (a, b))
+        self.printer.set_rollover_info(self.name, "%s: %s" % (self.name, msg))
+        gcmd.respond_info(msg, log=False)
     cmd_SET_E_STEP_DISTANCE_help = "Set extruder step distance"
     def cmd_SET_E_STEP_DISTANCE(self, gcmd):
         toolhead = self.printer.lookup_object('toolhead')