extruder: Convert to using iterative solver

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

4 files changed, 79 insertions, 16 deletions

diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py
index 7366cc81..51f59e59 100644
--- a/klippy/chelper/__init__.py
+++ b/klippy/chelper/__init__.py
@@ -16,7 +16,7 @@ COMPILE_CMD = ("gcc -Wall -g -O2 -shared -fPIC"
                " -o %s %s")
 SOURCE_FILES = [
     'pyhelper.c', 'serialqueue.c', 'stepcompress.c', 'itersolve.c',
-    'kin_cartesian.c', 'kin_corexy.c', 'kin_delta.c',
+    'kin_cartesian.c', 'kin_corexy.c', 'kin_delta.c', 'kin_extruder.c'
 ]
 DEST_LIB = "c_helper.so"
 OTHER_FILES = [
@@ -71,6 +71,14 @@ defs_kin_delta = """
         , double tower_x, double tower_y);
 """
 
+defs_kin_extruder = """
+    struct stepper_kinematics *extruder_stepper_alloc(void);
+    void extruder_move_fill(struct move *m, double print_time
+        , double accel_t, double cruise_t, double decel_t, double start_pos
+        , double start_v, double cruise_v, double accel
+        , double extra_accel_v, double extra_decel_v);
+"""
+
 defs_serialqueue = """
     #define MESSAGE_MAX 64
     struct pull_queue_message {
@@ -109,7 +117,7 @@ defs_std = """
 
 defs_all = [
     defs_pyhelper, defs_serialqueue, defs_std, defs_stepcompress, defs_itersolve,
-    defs_kin_cartesian, defs_kin_corexy, defs_kin_delta
+    defs_kin_cartesian, defs_kin_corexy, defs_kin_delta, defs_kin_extruder
 ]
 
 # Return the list of file modification times
diff --git a/klippy/chelper/itersolve.c b/klippy/chelper/itersolve.c
index e79d5f5f..72e6ad89 100644
--- a/klippy/chelper/itersolve.c
+++ b/klippy/chelper/itersolve.c
@@ -17,19 +17,6 @@
  * Kinematic moves
  ****************************************************************/
 
-struct move_accel {
-    double c1, c2;
-};
-
-struct move {
-    double print_time, move_t;
-    double accel_t, cruise_t;
-    double cruise_start_d, decel_start_d;
-    double cruise_v;
-    struct move_accel accel, decel;
-    struct coord start_pos, axes_r;
-};
-
 struct move * __visible
 move_alloc(void)
 {
@@ -80,7 +67,7 @@ move_eval_accel(struct move_accel *ma, double move_time)
 }
 
 // Return the distance moved given a time in a move
-static double
+inline double
 move_get_distance(struct move *m, double move_time)
 {
     if (unlikely(move_time < m->accel_t))
diff --git a/klippy/chelper/itersolve.h b/klippy/chelper/itersolve.h
index bd8fba0e..23b3201d 100644
--- a/klippy/chelper/itersolve.h
+++ b/klippy/chelper/itersolve.h
@@ -7,12 +7,26 @@ struct coord {
     double x, y, z;
 };
 
+struct move_accel {
+    double c1, c2;
+};
+
+struct move {
+    double print_time, move_t;
+    double accel_t, cruise_t;
+    double cruise_start_d, decel_start_d;
+    double cruise_v;
+    struct move_accel accel, decel;
+    struct coord start_pos, axes_r;
+};
+
 struct move *move_alloc(void);
 void move_fill(struct move *m, double print_time
                , double accel_t, double cruise_t, double decel_t
                , double start_pos_x, double start_pos_y, double start_pos_z
                , double axes_d_x, double axes_d_y, double axes_d_z
                , double start_v, double cruise_v, double accel);
+double move_get_distance(struct move *m, double move_time);
 struct coord move_get_coord(struct move *m, double move_time);
 
 struct stepper_kinematics;
diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c
new file mode 100644
index 00000000..6b34980f
--- /dev/null
+++ b/klippy/chelper/kin_extruder.c
@@ -0,0 +1,54 @@
+// Extruder stepper pulse time generation
+//
+// Copyright (C) 2018  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include "compiler.h" // __visible
+#include "itersolve.h" // struct stepper_kinematics
+#include "pyhelper.h" // errorf
+
+static double
+extruder_calc_position(struct stepper_kinematics *sk, struct move *m
+                       , double move_time)
+{
+    return m->start_pos.x + move_get_distance(m, move_time);
+}
+
+struct stepper_kinematics * __visible
+extruder_stepper_alloc(void)
+{
+    struct stepper_kinematics *sk = malloc(sizeof(*sk));
+    memset(sk, 0, sizeof(*sk));
+    sk->calc_position = extruder_calc_position;
+    return sk;
+}
+
+// Populate a 'struct move' with an extruder velocity trapezoid
+void __visible
+extruder_move_fill(struct move *m, double print_time
+                   , double accel_t, double cruise_t, double decel_t
+                   , double start_pos
+                   , double start_v, double cruise_v, double accel
+                   , double extra_accel_v, double extra_decel_v)
+{
+    // Setup velocity trapezoid
+    m->print_time = print_time;
+    m->move_t = accel_t + cruise_t + decel_t;
+    m->accel_t = accel_t;
+    m->cruise_t = cruise_t;
+    m->cruise_start_d = accel_t * (.5 * (cruise_v + start_v) + extra_accel_v);
+    m->decel_start_d = m->cruise_start_d + cruise_t * cruise_v;
+
+    // Setup for accel/cruise/decel phases
+    m->cruise_v = cruise_v;
+    m->accel.c1 = start_v + extra_accel_v;
+    m->accel.c2 = .5 * accel;
+    m->decel.c1 = cruise_v + extra_decel_v;
+    m->decel.c2 = -m->accel.c2;
+
+    // Setup start distance
+    m->start_pos.x = start_pos;
+}