itersolve: Add kinematic iterative solver code

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

5 files changed, 315 insertions, 4 deletions

diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py
index 32b065d3..da31ec38 100644
--- a/klippy/chelper/__init__.py
+++ b/klippy/chelper/__init__.py
@@ -15,11 +15,13 @@ COMPILE_CMD = ("gcc -Wall -g -O2 -shared -fPIC"
                " -flto -fwhole-program -fno-use-linker-plugin"
                " -o %s %s")
 SOURCE_FILES = [
-    'stepcompress.c', 'kin_cartesian.c', 'kin_delta.c',
+    'stepcompress.c', 'kin_cartesian.c', 'kin_delta.c', 'itersolve.c',
     'serialqueue.c', 'pyhelper.c'
 ]
 DEST_LIB = "c_helper.so"
-OTHER_FILES = ['list.h', 'serialqueue.h', 'stepcompress.h', 'pyhelper.h']
+OTHER_FILES = [
+    'list.h', 'serialqueue.h', 'stepcompress.h', 'itersolve.h', 'pyhelper.h'
+]
 
 defs_stepcompress = """
     struct stepcompress *stepcompress_alloc(uint32_t oid);
@@ -39,6 +41,19 @@ defs_stepcompress = """
     int steppersync_flush(struct steppersync *ss, uint64_t move_clock);
 """
 
+defs_itersolve = """
+    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);
+    int32_t itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m);
+    void itersolve_set_commanded_pos(struct stepper_kinematics *sk, double pos);
+    void itersolve_set_stepcompress(struct stepper_kinematics *sk
+        , struct stepcompress *sc, double step_dist);
+"""
+
 defs_kin_cartesian = """
     int32_t stepcompress_push(struct stepcompress *sc, double step_clock
         , int32_t sdir);
@@ -84,9 +99,13 @@ defs_pyhelper = """
     double get_monotonic(void);
 """
 
+defs_std = """
+    void free(void*);
+"""
+
 defs_all = [
-    defs_stepcompress, defs_kin_cartesian, defs_kin_delta,
-    defs_serialqueue, defs_pyhelper
+    defs_stepcompress, defs_itersolve, defs_kin_cartesian, defs_kin_delta,
+    defs_serialqueue, defs_pyhelper, defs_std
 ]
 
 # Return the list of file modification times
diff --git a/klippy/chelper/itersolve.c b/klippy/chelper/itersolve.c
new file mode 100644
index 00000000..e79d5f5f
--- /dev/null
+++ b/klippy/chelper/itersolve.c
@@ -0,0 +1,236 @@
+// Iterative solver for kinematic moves
+//
+// Copyright (C) 2018  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <math.h> // sqrt
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include "compiler.h" // __visible
+#include "itersolve.h" // struct coord
+#include "pyhelper.h" // errorf
+#include "stepcompress.h" // queue_append_start
+
+
+/****************************************************************
+ * 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)
+{
+    struct move *m = malloc(sizeof(*m));
+    memset(m, 0, sizeof(*m));
+    return m;
+}
+
+// Populate a 'struct move' with a velocity trapezoid
+void __visible
+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)
+{
+    // 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);
+    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;
+    m->accel.c2 = .5 * accel;
+    m->decel.c1 = cruise_v;
+    m->decel.c2 = -m->accel.c2;
+
+    // Setup for move_get_coord()
+    m->start_pos.x = start_pos_x;
+    m->start_pos.y = start_pos_y;
+    m->start_pos.z = start_pos_z;
+    double inv_move_d = 1. / sqrt(axes_d_x*axes_d_x + axes_d_y*axes_d_y
+                                  + axes_d_z*axes_d_z);
+    m->axes_r.x = axes_d_x * inv_move_d;
+    m->axes_r.y = axes_d_y * inv_move_d;
+    m->axes_r.z = axes_d_z * inv_move_d;
+}
+
+// Find the distance travel during acceleration/deceleration
+static inline double
+move_eval_accel(struct move_accel *ma, double move_time)
+{
+    return (ma->c1 + ma->c2 * move_time) * move_time;
+}
+
+// Return the distance moved given a time in a move
+static double
+move_get_distance(struct move *m, double move_time)
+{
+    if (unlikely(move_time < m->accel_t))
+        // Acceleration phase of move
+        return move_eval_accel(&m->accel, move_time);
+    move_time -= m->accel_t;
+    if (likely(move_time <= m->cruise_t))
+        // Cruising phase
+        return m->cruise_start_d + m->cruise_v * move_time;
+    // Deceleration phase
+    move_time -= m->cruise_t;
+    return m->decel_start_d + move_eval_accel(&m->decel, move_time);
+}
+
+// Return the XYZ coordinates given a time in a move
+inline struct coord
+move_get_coord(struct move *m, double move_time)
+{
+    double move_dist = move_get_distance(m, move_time);
+    return (struct coord) {
+        .x = m->start_pos.x + m->axes_r.x * move_dist,
+        .y = m->start_pos.y + m->axes_r.y * move_dist,
+        .z = m->start_pos.z + m->axes_r.z * move_dist };
+}
+
+
+/****************************************************************
+ * Iterative solver
+ ****************************************************************/
+
+struct timepos {
+    double time, position;
+};
+
+// Find step using "false position" method
+static struct timepos
+itersolve_find_step(struct stepper_kinematics *sk, struct move *m
+                    , struct timepos low, struct timepos high
+                    , double target)
+{
+    sk_callback calc_position = sk->calc_position;
+    struct timepos best_guess = high;
+    low.position -= target;
+    high.position -= target;
+    if (!high.position)
+        // The high range was a perfect guess for the next step
+        return best_guess;
+    int high_sign = signbit(high.position);
+    if (high_sign == signbit(low.position))
+        // The target is not in the low/high range - return low range
+        return (struct timepos){ low.time, target };
+    for (;;) {
+        double guess_time = ((low.time*high.position - high.time*low.position)
+                             / (high.position - low.position));
+        if (fabs(guess_time - best_guess.time) <= .000000001)
+            break;
+        best_guess.time = guess_time;
+        best_guess.position = calc_position(sk, m, guess_time);
+        double guess_position = best_guess.position - target;
+        int guess_sign = signbit(guess_position);
+        if (guess_sign == high_sign) {
+            high.time = guess_time;
+            high.position = guess_position;
+        } else {
+            low.time = guess_time;
+            low.position = guess_position;
+        }
+    }
+    return best_guess;
+}
+
+// Generate step times for a stepper during a move
+int32_t __visible
+itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m)
+{
+    struct stepcompress *sc = sk->sc;
+    sk_callback calc_position = sk->calc_position;
+    double half_step = .5 * sk->step_dist;
+    double mcu_freq = stepcompress_get_mcu_freq(sc);
+    struct timepos last = { 0., sk->commanded_pos }, low = last, high = last;
+    double seek_time_delta = 0.000100;
+    int steps = 0, sdir = stepcompress_get_step_dir(sc);
+    struct queue_append qa = queue_append_start(sc, m->print_time, .5);
+    for (;;) {
+        // Determine if next step is in forward or reverse direction
+        double dist = high.position - last.position;
+        if (fabs(dist) < half_step) {
+        seek_new_high_range:
+            if (high.time >= m->move_t)
+                // At end of move
+                break;
+            // Need to increase next step search range
+            low = high;
+            high.time = last.time + seek_time_delta;
+            seek_time_delta += seek_time_delta;
+            if (high.time > m->move_t)
+                high.time = m->move_t;
+            high.position = calc_position(sk, m, high.time);
+            continue;
+        }
+        int next_sdir = dist > 0.;
+        if (unlikely(next_sdir != sdir)) {
+            // Direction change
+            if (fabs(dist) < half_step + .000000001)
+                // Only change direction if going past midway point
+                goto seek_new_high_range;
+            if (last.time >= low.time && high.time > last.time) {
+                // Must seek new low range to avoid re-finding previous time
+                high.time = (last.time + high.time) * .5;
+                high.position = calc_position(sk, m, high.time);
+                continue;
+            }
+            int ret = queue_append_set_next_step_dir(&qa, next_sdir);
+            if (ret)
+                return ret;
+            sdir = next_sdir;
+        }
+        // Find step
+        double target = last.position + (sdir ? half_step : -half_step);
+        struct timepos next = itersolve_find_step(sk, m, low, high, target);
+        // Add step at given time
+        int ret = queue_append(&qa, next.time * mcu_freq);
+        if (ret)
+            return ret;
+        steps += sdir ? 1 : -1;
+        seek_time_delta = next.time - last.time;
+        if (seek_time_delta < .000000001)
+            seek_time_delta = .000000001;
+        last.position = target + (sdir ? half_step : -half_step);
+        last.time = next.time;
+        low = next;
+        if (last.time >= high.time)
+            // The high range is no longer valid - recalculate it
+            goto seek_new_high_range;
+    }
+    queue_append_finish(qa);
+    sk->commanded_pos = last.position;
+    return steps;
+}
+
+void __visible
+itersolve_set_stepcompress(struct stepper_kinematics *sk
+                           , struct stepcompress *sc, double step_dist)
+{
+    sk->sc = sc;
+    sk->step_dist = step_dist;
+}
+
+void __visible
+itersolve_set_commanded_pos(struct stepper_kinematics *sk, double pos)
+{
+    sk->commanded_pos = pos;
+}
diff --git a/klippy/chelper/itersolve.h b/klippy/chelper/itersolve.h
new file mode 100644
index 00000000..bd8fba0e
--- /dev/null
+++ b/klippy/chelper/itersolve.h
@@ -0,0 +1,32 @@
+#ifndef ITERSOLVE_H
+#define ITERSOLVE_H
+
+#include <stdint.h> // uint32_t
+
+struct coord {
+    double x, y, z;
+};
+
+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);
+struct coord move_get_coord(struct move *m, double move_time);
+
+struct stepper_kinematics;
+typedef double (*sk_callback)(struct stepper_kinematics *sk, struct move *m
+                              , double move_time);
+struct stepper_kinematics {
+    double step_dist, commanded_pos;
+    struct stepcompress *sc;
+    sk_callback calc_position;
+};
+
+int32_t itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m);
+void itersolve_set_stepcompress(struct stepper_kinematics *sk
+                                , struct stepcompress *sc, double step_dist);
+void itersolve_set_commanded_pos(struct stepper_kinematics *sk, double pos);
+
+#endif // itersolve.h
diff --git a/klippy/chelper/stepcompress.c b/klippy/chelper/stepcompress.c
index ebab287a..788965ba 100644
--- a/klippy/chelper/stepcompress.c
+++ b/klippy/chelper/stepcompress.c
@@ -389,6 +389,12 @@ stepcompress_get_oid(struct stepcompress *sc)
     return sc->oid;
 }
 
+int
+stepcompress_get_step_dir(struct stepcompress *sc)
+{
+    return sc->sdir;
+}
+
 
 /****************************************************************
  * Queue management
@@ -485,6 +491,22 @@ queue_append(struct queue_append *qa, double step_clock)
     return 0;
 }
 
+inline int
+queue_append_set_next_step_dir(struct queue_append *qa, int sdir)
+{
+    struct stepcompress *sc = qa->sc;
+    uint64_t old_last_step_clock = sc->last_step_clock;
+    sc->queue_next = qa->qnext;
+    int ret = set_next_step_dir(sc, sdir);
+    if (ret)
+        return ret;
+    qa->qnext = sc->queue_next;
+    qa->qend = sc->queue_end;
+    qa->last_step_clock_32 = sc->last_step_clock;
+    qa->clock_offset -= sc->last_step_clock - old_last_step_clock;
+    return 0;
+}
+
 
 /****************************************************************
  * Step compress synchronization
diff --git a/klippy/chelper/stepcompress.h b/klippy/chelper/stepcompress.h
index a926409c..bf756143 100644
--- a/klippy/chelper/stepcompress.h
+++ b/klippy/chelper/stepcompress.h
@@ -16,6 +16,7 @@ int stepcompress_set_homing(struct stepcompress *sc, uint64_t homing_clock);
 int stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len);
 double stepcompress_get_mcu_freq(struct stepcompress *sc);
 uint32_t stepcompress_get_oid(struct stepcompress *sc);
+int stepcompress_get_step_dir(struct stepcompress *sc);
 
 struct queue_append {
     struct stepcompress *sc;
@@ -26,6 +27,7 @@ struct queue_append queue_append_start(
     struct stepcompress *sc, double print_time, double adjust);
 void queue_append_finish(struct queue_append qa);
 int queue_append(struct queue_append *qa, double step_clock);
+int queue_append_set_next_step_dir(struct queue_append *qa, int sdir);
 
 struct serialqueue;
 struct steppersync *steppersync_alloc(