extruder: Implement non-linear extrusion

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,67 @@ 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;
+    // Total amount of extra movement added by nonlinear extrusion
+    double added_travel;
+    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);
+
+    double a = es->nonlinear_a, b = es->nonlinear_b;
+    if (a || b)
+        pos += nonlinear_extrusion_integrate(m, move_time, a, 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);
+
+    double a = es->nonlinear_a, b = es->nonlinear_b;
+    if (a || b)
+        es->added_travel += nonlinear_extrusion_integrate(m, m->move_t, a, b);
 }
 
 void __visible
@@ -128,12 +172,21 @@ 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,20 @@ 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)
+
+        toolhead = self.printer.lookup_object('toolhead')
+        toolhead.flush_step_generation()
+        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')