mirror of https://github.com/Desuuuu/klipper.git
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 <kevin@koconnor.net>
This commit is contained in:
parent
374eb98765
commit
fdbdb3c707
|
@ -15,13 +15,14 @@ COMPILE_CMD = ("gcc -Wall -g -O2 -shared -fPIC"
|
||||||
" -flto -fwhole-program -fno-use-linker-plugin"
|
" -flto -fwhole-program -fno-use-linker-plugin"
|
||||||
" -o %s %s")
|
" -o %s %s")
|
||||||
SOURCE_FILES = [
|
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_cartesian.c', 'kin_corexy.c', 'kin_delta.c', 'kin_polar.c',
|
||||||
'kin_winch.c', 'kin_extruder.c',
|
'kin_winch.c', 'kin_extruder.c',
|
||||||
]
|
]
|
||||||
DEST_LIB = "c_helper.so"
|
DEST_LIB = "c_helper.so"
|
||||||
OTHER_FILES = [
|
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 = """
|
defs_stepcompress = """
|
||||||
|
@ -43,12 +44,6 @@ defs_stepcompress = """
|
||||||
"""
|
"""
|
||||||
|
|
||||||
defs_itersolve = """
|
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);
|
int32_t itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m);
|
||||||
void itersolve_set_stepcompress(struct stepper_kinematics *sk
|
void itersolve_set_stepcompress(struct stepper_kinematics *sk
|
||||||
, struct stepcompress *sc, double step_dist);
|
, struct stepcompress *sc, double step_dist);
|
||||||
|
@ -58,6 +53,15 @@ defs_itersolve = """
|
||||||
double itersolve_get_commanded_pos(struct stepper_kinematics *sk);
|
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 = """
|
defs_kin_cartesian = """
|
||||||
struct stepper_kinematics *cartesian_stepper_alloc(char axis);
|
struct stepper_kinematics *cartesian_stepper_alloc(char axis);
|
||||||
"""
|
"""
|
||||||
|
@ -127,7 +131,7 @@ defs_std = """
|
||||||
|
|
||||||
defs_all = [
|
defs_all = [
|
||||||
defs_pyhelper, defs_serialqueue, defs_std,
|
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_cartesian, defs_kin_corexy, defs_kin_delta, defs_kin_polar,
|
||||||
defs_kin_winch, defs_kin_extruder
|
defs_kin_winch, defs_kin_extruder
|
||||||
]
|
]
|
||||||
|
|
|
@ -4,99 +4,13 @@
|
||||||
//
|
//
|
||||||
// This file may be distributed under the terms of the GNU GPLv3 license.
|
// This file may be distributed under the terms of the GNU GPLv3 license.
|
||||||
|
|
||||||
#include <math.h> // sqrt
|
#include <math.h> // fabs
|
||||||
#include <stdlib.h> // malloc
|
|
||||||
#include <string.h> // memset
|
#include <string.h> // memset
|
||||||
#include "compiler.h" // __visible
|
#include "compiler.h" // __visible
|
||||||
#include "itersolve.h" // struct coord
|
#include "itersolve.h" // itersolve_gen_steps
|
||||||
#include "pyhelper.h" // errorf
|
#include "pyhelper.h" // errorf
|
||||||
#include "stepcompress.h" // queue_append_start
|
#include "stepcompress.h" // queue_append_start
|
||||||
|
#include "trapq.h" // struct move
|
||||||
|
|
||||||
/****************************************************************
|
|
||||||
* 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
|
|
||||||
****************************************************************/
|
|
||||||
|
|
||||||
struct timepos {
|
struct timepos {
|
||||||
double time, position;
|
double time, position;
|
||||||
|
|
|
@ -1,35 +1,10 @@
|
||||||
#ifndef ITERSOLVE_H
|
#ifndef ITERSOLVE_H
|
||||||
#define ITERSOLVE_H
|
#define ITERSOLVE_H
|
||||||
|
|
||||||
#include <stdint.h> // uint32_t
|
#include <stdint.h> // int32_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);
|
|
||||||
|
|
||||||
struct stepper_kinematics;
|
struct stepper_kinematics;
|
||||||
|
struct move;
|
||||||
typedef double (*sk_calc_callback)(struct stepper_kinematics *sk, struct move *m
|
typedef double (*sk_calc_callback)(struct stepper_kinematics *sk, struct move *m
|
||||||
, double move_time);
|
, double move_time);
|
||||||
typedef void (*sk_post_callback)(struct stepper_kinematics *sk);
|
typedef void (*sk_post_callback)(struct stepper_kinematics *sk);
|
||||||
|
|
|
@ -7,8 +7,9 @@
|
||||||
#include <stdlib.h> // malloc
|
#include <stdlib.h> // malloc
|
||||||
#include <string.h> // memset
|
#include <string.h> // memset
|
||||||
#include "compiler.h" // __visible
|
#include "compiler.h" // __visible
|
||||||
#include "itersolve.h" // move_get_coord
|
#include "itersolve.h" // struct stepper_kinematics
|
||||||
#include "pyhelper.h" // errorf
|
#include "pyhelper.h" // errorf
|
||||||
|
#include "trapq.h" // move_get_coord
|
||||||
|
|
||||||
static double
|
static double
|
||||||
cart_stepper_x_calc_position(struct stepper_kinematics *sk, struct move *m
|
cart_stepper_x_calc_position(struct stepper_kinematics *sk, struct move *m
|
||||||
|
|
|
@ -8,6 +8,7 @@
|
||||||
#include <string.h> // memset
|
#include <string.h> // memset
|
||||||
#include "compiler.h" // __visible
|
#include "compiler.h" // __visible
|
||||||
#include "itersolve.h" // struct stepper_kinematics
|
#include "itersolve.h" // struct stepper_kinematics
|
||||||
|
#include "trapq.h" // move_get_coord
|
||||||
|
|
||||||
static double
|
static double
|
||||||
corexy_stepper_plus_calc_position(struct stepper_kinematics *sk, struct move *m
|
corexy_stepper_plus_calc_position(struct stepper_kinematics *sk, struct move *m
|
||||||
|
|
|
@ -10,6 +10,7 @@
|
||||||
#include <string.h> // memset
|
#include <string.h> // memset
|
||||||
#include "compiler.h" // __visible
|
#include "compiler.h" // __visible
|
||||||
#include "itersolve.h" // struct stepper_kinematics
|
#include "itersolve.h" // struct stepper_kinematics
|
||||||
|
#include "trapq.h" // move_get_coord
|
||||||
|
|
||||||
struct delta_stepper {
|
struct delta_stepper {
|
||||||
struct stepper_kinematics sk;
|
struct stepper_kinematics sk;
|
||||||
|
|
|
@ -9,6 +9,7 @@
|
||||||
#include "compiler.h" // __visible
|
#include "compiler.h" // __visible
|
||||||
#include "itersolve.h" // struct stepper_kinematics
|
#include "itersolve.h" // struct stepper_kinematics
|
||||||
#include "pyhelper.h" // errorf
|
#include "pyhelper.h" // errorf
|
||||||
|
#include "trapq.h" // move_get_distance
|
||||||
|
|
||||||
static double
|
static double
|
||||||
extruder_calc_position(struct stepper_kinematics *sk, struct move *m
|
extruder_calc_position(struct stepper_kinematics *sk, struct move *m
|
||||||
|
|
|
@ -9,6 +9,7 @@
|
||||||
#include <string.h> // memset
|
#include <string.h> // memset
|
||||||
#include "compiler.h" // __visible
|
#include "compiler.h" // __visible
|
||||||
#include "itersolve.h" // struct stepper_kinematics
|
#include "itersolve.h" // struct stepper_kinematics
|
||||||
|
#include "trapq.h" // move_get_coord
|
||||||
|
|
||||||
static double
|
static double
|
||||||
polar_stepper_radius_calc_position(struct stepper_kinematics *sk, struct move *m
|
polar_stepper_radius_calc_position(struct stepper_kinematics *sk, struct move *m
|
||||||
|
|
|
@ -10,6 +10,7 @@
|
||||||
#include <string.h> // memset
|
#include <string.h> // memset
|
||||||
#include "compiler.h" // __visible
|
#include "compiler.h" // __visible
|
||||||
#include "itersolve.h" // struct stepper_kinematics
|
#include "itersolve.h" // struct stepper_kinematics
|
||||||
|
#include "trapq.h" // move_get_coord
|
||||||
|
|
||||||
struct winch_stepper {
|
struct winch_stepper {
|
||||||
struct stepper_kinematics sk;
|
struct stepper_kinematics sk;
|
||||||
|
|
|
@ -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 };
|
||||||
|
}
|
|
@ -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
|
Loading…
Reference in New Issue