aboutsummaryrefslogtreecommitdiffstats
path: root/klippy
diff options
context:
space:
mode:
Diffstat (limited to 'klippy')
-rw-r--r--klippy/chelper/__init__.py22
-rw-r--r--klippy/chelper/itersolve.c92
-rw-r--r--klippy/chelper/itersolve.h29
-rw-r--r--klippy/chelper/kin_cartesian.c3
-rw-r--r--klippy/chelper/kin_corexy.c1
-rw-r--r--klippy/chelper/kin_delta.c1
-rw-r--r--klippy/chelper/kin_extruder.c1
-rw-r--r--klippy/chelper/kin_polar.c1
-rw-r--r--klippy/chelper/kin_winch.c1
-rw-r--r--klippy/chelper/trapq.c87
-rw-r--r--klippy/chelper/trapq.h30
11 files changed, 142 insertions, 126 deletions
diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py
index 6de764f0..9bbb1199 100644
--- a/klippy/chelper/__init__.py
+++ b/klippy/chelper/__init__.py
@@ -15,13 +15,14 @@ COMPILE_CMD = ("gcc -Wall -g -O2 -shared -fPIC"
" -flto -fwhole-program -fno-use-linker-plugin"
" -o %s %s")
SOURCE_FILES = [
- 'pyhelper.c', 'serialqueue.c', 'stepcompress.c', 'itersolve.c',
+ 'pyhelper.c', 'serialqueue.c', 'stepcompress.c', 'itersolve.c', 'trapq.c',
'kin_cartesian.c', 'kin_corexy.c', 'kin_delta.c', 'kin_polar.c',
'kin_winch.c', 'kin_extruder.c',
]
DEST_LIB = "c_helper.so"
OTHER_FILES = [
- 'list.h', 'serialqueue.h', 'stepcompress.h', 'itersolve.h', 'pyhelper.h'
+ 'list.h', 'serialqueue.h', 'stepcompress.h', 'itersolve.h', 'pyhelper.h',
+ 'trapq.h',
]
defs_stepcompress = """
@@ -43,12 +44,6 @@ defs_stepcompress = """
"""
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_stepcompress(struct stepper_kinematics *sk
, struct stepcompress *sc, double step_dist);
@@ -58,6 +53,15 @@ defs_itersolve = """
double itersolve_get_commanded_pos(struct stepper_kinematics *sk);
"""
+defs_trapq = """
+ 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);
+"""
+
defs_kin_cartesian = """
struct stepper_kinematics *cartesian_stepper_alloc(char axis);
"""
@@ -127,7 +131,7 @@ defs_std = """
defs_all = [
defs_pyhelper, defs_serialqueue, defs_std,
- defs_stepcompress, defs_itersolve,
+ defs_stepcompress, defs_itersolve, defs_trapq,
defs_kin_cartesian, defs_kin_corexy, defs_kin_delta, defs_kin_polar,
defs_kin_winch, defs_kin_extruder
]
diff --git a/klippy/chelper/itersolve.c b/klippy/chelper/itersolve.c
index 2949b9c2..3f35f1d4 100644
--- a/klippy/chelper/itersolve.c
+++ b/klippy/chelper/itersolve.c
@@ -4,99 +4,13 @@
//
// This file may be distributed under the terms of the GNU GPLv3 license.
-#include <math.h> // sqrt
-#include <stdlib.h> // malloc
+#include <math.h> // fabs
#include <string.h> // memset
#include "compiler.h" // __visible
-#include "itersolve.h" // struct coord
+#include "itersolve.h" // itersolve_gen_steps
#include "pyhelper.h" // errorf
#include "stepcompress.h" // queue_append_start
-
-
-/****************************************************************
- * Kinematic moves
- ****************************************************************/
-
-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
-inline 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
- ****************************************************************/
+#include "trapq.h" // struct move
struct timepos {
double time, position;
diff --git a/klippy/chelper/itersolve.h b/klippy/chelper/itersolve.h
index 23743498..da3a9fa7 100644
--- a/klippy/chelper/itersolve.h
+++ b/klippy/chelper/itersolve.h
@@ -1,35 +1,10 @@
#ifndef ITERSOLVE_H
#define ITERSOLVE_H
-#include <stdint.h> // uint32_t
-
-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);
+#include <stdint.h> // int32_t
struct stepper_kinematics;
+struct move;
typedef double (*sk_calc_callback)(struct stepper_kinematics *sk, struct move *m
, double move_time);
typedef void (*sk_post_callback)(struct stepper_kinematics *sk);
diff --git a/klippy/chelper/kin_cartesian.c b/klippy/chelper/kin_cartesian.c
index c59410b5..4ae89e38 100644
--- a/klippy/chelper/kin_cartesian.c
+++ b/klippy/chelper/kin_cartesian.c
@@ -7,8 +7,9 @@
#include <stdlib.h> // malloc
#include <string.h> // memset
#include "compiler.h" // __visible
-#include "itersolve.h" // move_get_coord
+#include "itersolve.h" // struct stepper_kinematics
#include "pyhelper.h" // errorf
+#include "trapq.h" // move_get_coord
static double
cart_stepper_x_calc_position(struct stepper_kinematics *sk, struct move *m
diff --git a/klippy/chelper/kin_corexy.c b/klippy/chelper/kin_corexy.c
index 6339b32b..a5a5022c 100644
--- a/klippy/chelper/kin_corexy.c
+++ b/klippy/chelper/kin_corexy.c
@@ -8,6 +8,7 @@
#include <string.h> // memset
#include "compiler.h" // __visible
#include "itersolve.h" // struct stepper_kinematics
+#include "trapq.h" // move_get_coord
static double
corexy_stepper_plus_calc_position(struct stepper_kinematics *sk, struct move *m
diff --git a/klippy/chelper/kin_delta.c b/klippy/chelper/kin_delta.c
index 04509976..5ead7e98 100644
--- a/klippy/chelper/kin_delta.c
+++ b/klippy/chelper/kin_delta.c
@@ -10,6 +10,7 @@
#include <string.h> // memset
#include "compiler.h" // __visible
#include "itersolve.h" // struct stepper_kinematics
+#include "trapq.h" // move_get_coord
struct delta_stepper {
struct stepper_kinematics sk;
diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c
index 92b220f6..dd2d27d6 100644
--- a/klippy/chelper/kin_extruder.c
+++ b/klippy/chelper/kin_extruder.c
@@ -9,6 +9,7 @@
#include "compiler.h" // __visible
#include "itersolve.h" // struct stepper_kinematics
#include "pyhelper.h" // errorf
+#include "trapq.h" // move_get_distance
static double
extruder_calc_position(struct stepper_kinematics *sk, struct move *m
diff --git a/klippy/chelper/kin_polar.c b/klippy/chelper/kin_polar.c
index 77755d70..15f5ae1b 100644
--- a/klippy/chelper/kin_polar.c
+++ b/klippy/chelper/kin_polar.c
@@ -9,6 +9,7 @@
#include <string.h> // memset
#include "compiler.h" // __visible
#include "itersolve.h" // struct stepper_kinematics
+#include "trapq.h" // move_get_coord
static double
polar_stepper_radius_calc_position(struct stepper_kinematics *sk, struct move *m
diff --git a/klippy/chelper/kin_winch.c b/klippy/chelper/kin_winch.c
index 3d3b8916..47a1ba92 100644
--- a/klippy/chelper/kin_winch.c
+++ b/klippy/chelper/kin_winch.c
@@ -10,6 +10,7 @@
#include <string.h> // memset
#include "compiler.h" // __visible
#include "itersolve.h" // struct stepper_kinematics
+#include "trapq.h" // move_get_coord
struct winch_stepper {
struct stepper_kinematics sk;
diff --git a/klippy/chelper/trapq.c b/klippy/chelper/trapq.c
new file mode 100644
index 00000000..c17471e7
--- /dev/null
+++ b/klippy/chelper/trapq.c
@@ -0,0 +1,87 @@
+// Trapezoidal velocity movement queue
+//
+// Copyright (C) 2018-2019 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" // unlikely
+#include "trapq.h" // move_get_coord
+
+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
+inline 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 };
+}
diff --git a/klippy/chelper/trapq.h b/klippy/chelper/trapq.h
new file mode 100644
index 00000000..15d7b44c
--- /dev/null
+++ b/klippy/chelper/trapq.h
@@ -0,0 +1,30 @@
+#ifndef TRAPQ_H
+#define TRAPQ_H
+
+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);
+
+#endif // trapq.h
generated by cgit v1.2.3-70-g09d2 (git 2.50.1) at 2025-09-02 11:17:11 +0000