corexy: Convert corexy to use the iterative solver
Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor
parent
ca0d0135dc
commit
fc4a9e7564
|
|
@ -15,8 +15,8 @@ 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', 'itersolve.c',
|
||||
'serialqueue.c', 'pyhelper.c'
|
||||
'pyhelper.c', 'serialqueue.c', 'stepcompress.c', 'itersolve.c',
|
||||
'kin_cartesian.c', 'kin_corexy.c', 'kin_delta.c',
|
||||
]
|
||||
DEST_LIB = "c_helper.so"
|
||||
OTHER_FILES = [
|
||||
|
|
@ -59,6 +59,11 @@ defs_kin_cartesian = """
|
|||
, int32_t sdir);
|
||||
int32_t stepcompress_push_const(struct stepcompress *sc, double clock_offset
|
||||
, double step_offset, double steps, double start_sv, double accel);
|
||||
struct stepper_kinematics *cartesian_stepper_alloc(char axis);
|
||||
"""
|
||||
|
||||
defs_kin_corexy = """
|
||||
struct stepper_kinematics *corexy_stepper_alloc(char type);
|
||||
"""
|
||||
|
||||
defs_kin_delta = """
|
||||
|
|
@ -103,8 +108,8 @@ defs_std = """
|
|||
"""
|
||||
|
||||
defs_all = [
|
||||
defs_stepcompress, defs_itersolve, defs_kin_cartesian, defs_kin_delta,
|
||||
defs_serialqueue, defs_pyhelper, defs_std
|
||||
defs_pyhelper, defs_serialqueue, defs_std, defs_stepcompress, defs_itersolve,
|
||||
defs_kin_cartesian, defs_kin_corexy, defs_kin_delta
|
||||
]
|
||||
|
||||
# Return the list of file modification times
|
||||
|
|
|
|||
|
|
@ -5,10 +5,18 @@
|
|||
// 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" // likely
|
||||
#include "itersolve.h" // move_get_coord
|
||||
#include "pyhelper.h" // errorf
|
||||
#include "stepcompress.h" // queue_append
|
||||
|
||||
|
||||
/****************************************************************
|
||||
* Direct step generation
|
||||
****************************************************************/
|
||||
|
||||
// Common suffixes: _sd is step distance (a unit length the same
|
||||
// distance the stepper moves on each step), _sv is step velocity (in
|
||||
// units of step distance per time), _sd2 is step distance squared, _r
|
||||
|
|
@ -111,3 +119,43 @@ stepcompress_push_const(
|
|||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
/****************************************************************
|
||||
* Iterative solver
|
||||
****************************************************************/
|
||||
|
||||
static double
|
||||
cart_stepper_x_calc_position(struct stepper_kinematics *sk, struct move *m
|
||||
, double move_time)
|
||||
{
|
||||
return move_get_coord(m, move_time).x;
|
||||
}
|
||||
|
||||
static double
|
||||
cart_stepper_y_calc_position(struct stepper_kinematics *sk, struct move *m
|
||||
, double move_time)
|
||||
{
|
||||
return move_get_coord(m, move_time).y;
|
||||
}
|
||||
|
||||
static double
|
||||
cart_stepper_z_calc_position(struct stepper_kinematics *sk, struct move *m
|
||||
, double move_time)
|
||||
{
|
||||
return move_get_coord(m, move_time).z;
|
||||
}
|
||||
|
||||
struct stepper_kinematics * __visible
|
||||
cartesian_stepper_alloc(char axis)
|
||||
{
|
||||
struct stepper_kinematics *sk = malloc(sizeof(*sk));
|
||||
memset(sk, 0, sizeof(*sk));
|
||||
if (axis == 'x')
|
||||
sk->calc_position = cart_stepper_x_calc_position;
|
||||
else if (axis == 'y')
|
||||
sk->calc_position = cart_stepper_y_calc_position;
|
||||
else if (axis == 'z')
|
||||
sk->calc_position = cart_stepper_z_calc_position;
|
||||
return sk;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,38 @@
|
|||
// CoreXY kinematics stepper pulse time generation
|
||||
//
|
||||
// Copyright (C) 2018 Kevin O'Connor <kevin@koconnor.net>
|
||||
//
|
||||
// This file may be distributed under the terms of the GNU GPLv3 license.
|
||||
|
||||
#include <stdlib.h> // malloc
|
||||
#include <string.h> // memset
|
||||
#include "compiler.h" // __visible
|
||||
#include "itersolve.h" // struct stepper_kinematics
|
||||
|
||||
static double
|
||||
corexy_stepper_plus_calc_position(struct stepper_kinematics *sk, struct move *m
|
||||
, double move_time)
|
||||
{
|
||||
struct coord c = move_get_coord(m, move_time);
|
||||
return c.x + c.y;
|
||||
}
|
||||
|
||||
static double
|
||||
corexy_stepper_minus_calc_position(struct stepper_kinematics *sk, struct move *m
|
||||
, double move_time)
|
||||
{
|
||||
struct coord c = move_get_coord(m, move_time);
|
||||
return c.x - c.y;
|
||||
}
|
||||
|
||||
struct stepper_kinematics * __visible
|
||||
corexy_stepper_alloc(char type)
|
||||
{
|
||||
struct stepper_kinematics *sk = malloc(sizeof(*sk));
|
||||
memset(sk, 0, sizeof(*sk));
|
||||
if (type == '+')
|
||||
sk->calc_position = corexy_stepper_plus_calc_position;
|
||||
else if (type == '-')
|
||||
sk->calc_position = corexy_stepper_minus_calc_position;
|
||||
return sk;
|
||||
}
|
||||
|
|
@ -1,10 +1,10 @@
|
|||
# Code for handling the kinematics of corexy robots
|
||||
#
|
||||
# Copyright (C) 2017 Kevin O'Connor <kevin@koconnor.net>
|
||||
# Copyright (C) 2017-2018 Kevin O'Connor <kevin@koconnor.net>
|
||||
#
|
||||
# This file may be distributed under the terms of the GNU GPLv3 license.
|
||||
import logging, math
|
||||
import stepper, homing
|
||||
import stepper, homing, chelper
|
||||
|
||||
StepList = (0, 1, 2)
|
||||
|
||||
|
|
@ -26,6 +26,15 @@ class CoreXYKinematics:
|
|||
'max_z_accel', max_accel, above=0., maxval=max_accel)
|
||||
self.need_motor_enable = True
|
||||
self.limits = [(1.0, -1.0)] * 3
|
||||
# Setup iterative solver
|
||||
ffi_main, ffi_lib = chelper.get_ffi()
|
||||
self.cmove = ffi_main.gc(ffi_lib.move_alloc(), ffi_lib.free)
|
||||
self.move_fill = ffi_lib.move_fill
|
||||
self.steppers[0].setup_itersolve(ffi_main.gc(
|
||||
ffi_lib.corexy_stepper_alloc('+'), ffi_lib.free))
|
||||
self.steppers[1].setup_itersolve(ffi_main.gc(
|
||||
ffi_lib.corexy_stepper_alloc('-'), ffi_lib.free))
|
||||
self.steppers[2].setup_cartesian_itersolve('z')
|
||||
# Setup stepper max halt velocity
|
||||
max_halt_velocity = toolhead.get_max_axis_halt()
|
||||
max_xy_halt_velocity = max_halt_velocity * math.sqrt(2.)
|
||||
|
|
@ -124,38 +133,17 @@ class CoreXYKinematics:
|
|||
def move(self, print_time, move):
|
||||
if self.need_motor_enable:
|
||||
self._check_motor_enable(print_time, move)
|
||||
sxp = move.start_pos[0]
|
||||
syp = move.start_pos[1]
|
||||
move_start_pos = (sxp + syp, sxp - syp, move.start_pos[2])
|
||||
exp = move.end_pos[0]
|
||||
eyp = move.end_pos[1]
|
||||
axes_d = ((exp + eyp) - move_start_pos[0],
|
||||
(exp - eyp) - move_start_pos[1], move.axes_d[2])
|
||||
for i in StepList:
|
||||
axis_d = axes_d[i]
|
||||
if not axis_d:
|
||||
continue
|
||||
step_const = self.steppers[i].step_const
|
||||
move_time = print_time
|
||||
start_pos = move_start_pos[i]
|
||||
axis_r = abs(axis_d) / move.move_d
|
||||
accel = move.accel * axis_r
|
||||
cruise_v = move.cruise_v * axis_r
|
||||
|
||||
# Acceleration steps
|
||||
if move.accel_r:
|
||||
accel_d = move.accel_r * axis_d
|
||||
step_const(move_time, start_pos, accel_d,
|
||||
move.start_v * axis_r, accel)
|
||||
start_pos += accel_d
|
||||
move_time += move.accel_t
|
||||
# Cruising steps
|
||||
if move.cruise_r:
|
||||
cruise_d = move.cruise_r * axis_d
|
||||
step_const(move_time, start_pos, cruise_d, cruise_v, 0.)
|
||||
start_pos += cruise_d
|
||||
move_time += move.cruise_t
|
||||
# Deceleration steps
|
||||
if move.decel_r:
|
||||
decel_d = move.decel_r * axis_d
|
||||
step_const(move_time, start_pos, decel_d, cruise_v, -accel)
|
||||
axes_d = move.axes_d
|
||||
cmove = self.cmove
|
||||
self.move_fill(
|
||||
cmove, print_time,
|
||||
move.accel_t, move.cruise_t, move.decel_t,
|
||||
move.start_pos[0], move.start_pos[1], move.start_pos[2],
|
||||
axes_d[0], axes_d[1], axes_d[2],
|
||||
move.start_v, move.cruise_v, move.accel)
|
||||
stepper_a, stepper_b, stepper_z = self.steppers
|
||||
if axes_d[0] or axes_d[1]:
|
||||
stepper_a.step_itersolve(cmove)
|
||||
stepper_b.step_itersolve(cmove)
|
||||
if axes_d[2]:
|
||||
stepper_z.step_itersolve(cmove)
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
#
|
||||
# This file may be distributed under the terms of the GNU GPLv3 license.
|
||||
import math, logging
|
||||
import homing
|
||||
import homing, chelper
|
||||
|
||||
# Tracking of shared stepper enable pins
|
||||
class StepperEnablePin:
|
||||
|
|
@ -155,6 +155,10 @@ class PrinterHomingStepper(PrinterStepper):
|
|||
self.homing_stepper_phases = None
|
||||
if self.mcu_endstop.get_mcu().is_fileoutput():
|
||||
self.homing_endstop_accuracy = self.homing_stepper_phases
|
||||
def setup_cartesian_itersolve(self, axis):
|
||||
ffi_main, ffi_lib = chelper.get_ffi()
|
||||
self.setup_itersolve(ffi_main.gc(
|
||||
ffi_lib.cartesian_stepper_alloc(axis), ffi_lib.free))
|
||||
def get_endstops(self):
|
||||
return [(self.mcu_endstop, self.name)]
|
||||
def get_homed_offset(self):
|
||||
|
|
@ -182,6 +186,7 @@ class PrinterMultiStepper(PrinterHomingStepper):
|
|||
self.endstops = PrinterHomingStepper.get_endstops(self)
|
||||
self.extras = []
|
||||
self.all_step_const = [self.step_const]
|
||||
self.all_step_itersolve = [self.step_itersolve]
|
||||
for i in range(1, 99):
|
||||
if not config.has_section(config.get_name() + str(i)):
|
||||
break
|
||||
|
|
@ -189,6 +194,7 @@ class PrinterMultiStepper(PrinterHomingStepper):
|
|||
extra = PrinterStepper(printer, extraconfig)
|
||||
self.extras.append(extra)
|
||||
self.all_step_const.append(extra.step_const)
|
||||
self.all_step_itersolve.append(extra.step_itersolve)
|
||||
extraendstop = extraconfig.get('endstop_pin', None)
|
||||
if extraendstop is not None:
|
||||
ppins = printer.lookup_object('pins')
|
||||
|
|
@ -198,9 +204,20 @@ class PrinterMultiStepper(PrinterHomingStepper):
|
|||
else:
|
||||
self.mcu_endstop.add_stepper(extra.mcu_stepper)
|
||||
self.step_const = self.step_multi_const
|
||||
self.step_itersolve = self.step_multi_itersolve
|
||||
def step_multi_const(self, print_time, start_pos, dist, start_v, accel):
|
||||
for step_const in self.all_step_const:
|
||||
step_const(print_time, start_pos, dist, start_v, accel)
|
||||
def step_multi_itersolve(self, cmove):
|
||||
for step_itersolve in self.all_step_itersolve:
|
||||
step_itersolve(cmove)
|
||||
def setup_cartesian_itersolve(self, axis):
|
||||
ffi_main, ffi_lib = chelper.get_ffi()
|
||||
self.setup_itersolve(ffi_main.gc(
|
||||
ffi_lib.cartesian_stepper_alloc(axis), ffi_lib.free))
|
||||
for extra in self.extras:
|
||||
extra.setup_itersolve(ffi_main.gc(
|
||||
ffi_lib.cartesian_stepper_alloc(axis), ffi_lib.free))
|
||||
def set_max_jerk(self, max_halt_velocity, max_accel):
|
||||
PrinterHomingStepper.set_max_jerk(self, max_halt_velocity, max_accel)
|
||||
for extra in self.extras:
|
||||
|
|
|
|||
Loading…
Reference in New Issue