Mirror
/
klipper-dgus
mirror of https://github.com/Desuuuu/klipper.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

itersolve: Add kinematic iterative solver code 

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2018-06-08 19:55:24 -04:00
parent ba3428822d
commit 2511471b0d
5 changed files with 315 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
klippy/chelper/__init__.py
View File

@ -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

236
klippy/chelper/itersolve.c Normal file
View File

@ -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;
}

32
klippy/chelper/itersolve.h Normal file
View File

@ -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

22
klippy/chelper/stepcompress.c
View File

@ -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

2
klippy/chelper/stepcompress.h
View File

@ -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(