From fdbdb3c70777e6c953b13d5455ccab56d024b029 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Sun, 27 Oct 2019 13:23:03 -0400 Subject: [PATCH] trapq: Add new trapq.c file with code for trapezoidal velocity handling Move the "struct move" code from itersolve.c to new file trapq.c. Signed-off-by: Kevin O'Connor --- klippy/chelper/__init__.py | 22 ++++---- klippy/chelper/itersolve.c | 92 ++-------------------------------- klippy/chelper/itersolve.h | 29 +---------- klippy/chelper/kin_cartesian.c | 3 +- klippy/chelper/kin_corexy.c | 1 + klippy/chelper/kin_delta.c | 1 + klippy/chelper/kin_extruder.c | 1 + klippy/chelper/kin_polar.c | 1 + klippy/chelper/kin_winch.c | 1 + klippy/chelper/trapq.c | 87 ++++++++++++++++++++++++++++++++ klippy/chelper/trapq.h | 30 +++++++++++ 11 files changed, 142 insertions(+), 126 deletions(-) create mode 100644 klippy/chelper/trapq.c create mode 100644 klippy/chelper/trapq.h 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 // sqrt -#include // malloc +#include // fabs #include // 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 // 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 // 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 // malloc #include // 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 // 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 // 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 // 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 // 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 +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include // sqrt +#include // malloc +#include // 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