kinematics: Add deltesian printers (#5743)
Initial push of the working deltesian kinematics after some successful tests. Signed-off-by: Fabrice GALLET <tircown@gmail.com>
This commit is contained in:
parent
ec4ecd7a70
commit
354915d2ad
|
@ -0,0 +1,73 @@
|
|||
# This file is an example config file for deltesian style printers.
|
||||
# One may copy and edit this file to configure a new deltesian
|
||||
# printer.
|
||||
|
||||
# DO NOT COPY THIS FILE WITHOUT CAREFULLY READING AND UPDATING IT
|
||||
# FIRST. Incorrectly configured parameters may cause damage.
|
||||
|
||||
# See docs/Config_Reference.md for a description of parameters.
|
||||
|
||||
[stepper_left]
|
||||
step_pin: PF0
|
||||
dir_pin: PF1
|
||||
enable_pin: !PD7
|
||||
microsteps: 16
|
||||
rotation_distance: 40
|
||||
endstop_pin: ^PE5
|
||||
homing_speed: 50
|
||||
position_endstop: 268
|
||||
arm_length: 217
|
||||
arm_x_length: 160
|
||||
|
||||
[stepper_right]
|
||||
step_pin: PL3
|
||||
dir_pin: PL1
|
||||
enable_pin: !PK0
|
||||
microsteps: 16
|
||||
rotation_distance: 40
|
||||
endstop_pin: ^PD3
|
||||
|
||||
[stepper_y]
|
||||
step_pin: PF6
|
||||
dir_pin: !PF7
|
||||
enable_pin: !PF2
|
||||
microsteps: 16
|
||||
rotation_distance: 40
|
||||
endstop_pin: ^PJ1
|
||||
position_endstop: 0
|
||||
position_max: 200
|
||||
|
||||
[extruder]
|
||||
step_pin: PA4
|
||||
dir_pin: PA6
|
||||
enable_pin: !PA2
|
||||
microsteps: 16
|
||||
rotation_distance: 33.500
|
||||
nozzle_diameter: 0.500
|
||||
filament_diameter: 3.500
|
||||
heater_pin: PB4
|
||||
sensor_type: EPCOS 100K B57560G104F
|
||||
sensor_pin: PK5
|
||||
control: pid
|
||||
pid_Kp: 22.2
|
||||
pid_Ki: 1.08
|
||||
pid_Kd: 114
|
||||
min_temp: 0
|
||||
max_temp: 210
|
||||
|
||||
[heater_bed]
|
||||
heater_pin: PH5
|
||||
sensor_type: EPCOS 100K B57560G104F
|
||||
sensor_pin: PK6
|
||||
control: watermark
|
||||
min_temp: 0
|
||||
max_temp: 110
|
||||
|
||||
[mcu]
|
||||
serial: /dev/ttyACM0
|
||||
|
||||
[printer]
|
||||
kinematics: deltesian
|
||||
max_velocity: 500
|
||||
max_accel: 3000
|
||||
max_z_velocity: 150
|
|
@ -85,8 +85,7 @@ The printer section controls high level printer settings.
|
|||
kinematics:
|
||||
# The type of printer in use. This option may be one of: cartesian,
|
||||
# corexy, corexz, hybrid_corexy, hybrid_corexz, rotary_delta, delta,
|
||||
# polar, winch, or none. This
|
||||
# parameter must be specified.
|
||||
# deltesian, polar, winch, or none. This parameter must be specified.
|
||||
max_velocity:
|
||||
# Maximum velocity (in mm/s) of the toolhead (relative to the
|
||||
# print). This parameter must be specified.
|
||||
|
@ -318,6 +317,81 @@ radius:
|
|||
# just prior to starting a probe operation. The default is 5.
|
||||
```
|
||||
|
||||
### Deltesian Kinematics
|
||||
|
||||
See [example-deltesian.cfg](../config/example-deltesian.cfg) for an
|
||||
example deltesian kinematics config file.
|
||||
|
||||
Only parameters specific to deltesian printers are described here - see
|
||||
[common kinematic settings](#common-kinematic-settings) for available
|
||||
parameters.
|
||||
|
||||
```
|
||||
[printer]
|
||||
kinematics: deltesian
|
||||
max_z_velocity:
|
||||
# For deltesian printers, this limits the maximum velocity (in mm/s) of
|
||||
# moves with z axis movement. This setting can be used to reduce the
|
||||
# maximum speed of up/down moves (which require a higher step rate
|
||||
# than other moves on a deltesian printer). The default is to use
|
||||
# max_velocity for max_z_velocity.
|
||||
#max_z_accel:
|
||||
# This sets the maximum acceleration (in mm/s^2) of movement along
|
||||
# the z axis. Setting this may be useful if the printer can reach higher
|
||||
# acceleration on XY moves than Z moves (eg, when using input shaper).
|
||||
# The default is to use max_accel for max_z_accel.
|
||||
#minimum_z_position: 0
|
||||
# The minimum Z position that the user may command the head to move
|
||||
# to. The default is 0.
|
||||
#min_angle: 5
|
||||
# This represents the minimum angle (in degrees) relative to horizontal
|
||||
# that the deltesian arms are allowed to achieve. This parameter is
|
||||
# intended to restrict the arms from becomming completely horizontal,
|
||||
# which would risk accidental inversion of the XZ axis. The default is 5.
|
||||
#print_width:
|
||||
# The distance (in mm) of valid toolhead X coordinates. One may use
|
||||
# this setting to customize the range checking of toolhead moves. If
|
||||
# a large value is specified here then it may be possible to command
|
||||
# the toolhead into a collision with a tower. This setting usually
|
||||
# corresponds to bed width (in mm).
|
||||
#slow_ratio: 3
|
||||
# The ratio used to limit velocity and acceleration on moves near the
|
||||
# extremes of the X axis. If vertical distance divided by horizontal
|
||||
# distance exceeds the value of slow_ratio, then velocity and
|
||||
# acceleration are limited to half their nominal values. If vertical
|
||||
# distance divided by horizontal distance exceeds twice the value of
|
||||
# the slow_ratio, then velocity and acceleration are limited to one
|
||||
# quarter of their nominal values. The default is 3.
|
||||
|
||||
# The stepper_left section is used to describe the stepper controlling
|
||||
# the left tower. This section also controls the homing parameters
|
||||
# (homing_speed, homing_retract_dist) for all towers.
|
||||
[stepper_left]
|
||||
position_endstop:
|
||||
# Distance (in mm) between the nozzle and the bed when the nozzle is
|
||||
# in the center of the build area and the endstops are triggered. This
|
||||
# parameter must be provided for stepper_left; for stepper_right this
|
||||
# parameter defaults to the value specified for stepper_left.
|
||||
arm_length:
|
||||
# Length (in mm) of the diagonal rod that connects the tower carriage to
|
||||
# the print head. This parameter must be provided for stepper_left; for
|
||||
# stepper_right, this parameter defaults to the value specified for
|
||||
# stepper_left.
|
||||
arm_x_length:
|
||||
# Horizontal distance between the print head and the tower when the
|
||||
# printers is homed. This parameter must be provided for stepper_left;
|
||||
# for stepper_right, this parameter defaults to the value specified for
|
||||
# stepper_left.
|
||||
|
||||
# The stepper_right section is used to desribe the stepper controlling the
|
||||
# right tower.
|
||||
[stepper_right]
|
||||
|
||||
# The stepper_y section is used to describe the stepper controlling
|
||||
# the Y axis in a deltesian robot.
|
||||
[stepper_y]
|
||||
```
|
||||
|
||||
### CoreXY Kinematics
|
||||
|
||||
See [example-corexy.cfg](../config/example-corexy.cfg) for an example
|
||||
|
|
|
@ -20,8 +20,8 @@ SOURCE_FILES = [
|
|||
'pyhelper.c', 'serialqueue.c', 'stepcompress.c', 'itersolve.c', 'trapq.c',
|
||||
'pollreactor.c', 'msgblock.c', 'trdispatch.c',
|
||||
'kin_cartesian.c', 'kin_corexy.c', 'kin_corexz.c', 'kin_delta.c',
|
||||
'kin_polar.c', 'kin_rotary_delta.c', 'kin_winch.c', 'kin_extruder.c',
|
||||
'kin_shaper.c',
|
||||
'kin_deltesian.c', 'kin_polar.c', 'kin_rotary_delta.c', 'kin_winch.c',
|
||||
'kin_extruder.c', 'kin_shaper.c',
|
||||
]
|
||||
DEST_LIB = "c_helper.so"
|
||||
OTHER_FILES = [
|
||||
|
@ -117,6 +117,11 @@ defs_kin_delta = """
|
|||
, double tower_x, double tower_y);
|
||||
"""
|
||||
|
||||
defs_kin_deltesian = """
|
||||
struct stepper_kinematics *deltesian_stepper_alloc(double arm2
|
||||
, double arm_x);
|
||||
"""
|
||||
|
||||
defs_kin_polar = """
|
||||
struct stepper_kinematics *polar_stepper_alloc(char type);
|
||||
"""
|
||||
|
@ -205,8 +210,8 @@ defs_all = [
|
|||
defs_pyhelper, defs_serialqueue, defs_std, defs_stepcompress,
|
||||
defs_itersolve, defs_trapq, defs_trdispatch,
|
||||
defs_kin_cartesian, defs_kin_corexy, defs_kin_corexz, defs_kin_delta,
|
||||
defs_kin_polar, defs_kin_rotary_delta, defs_kin_winch, defs_kin_extruder,
|
||||
defs_kin_shaper,
|
||||
defs_kin_deltesian, defs_kin_polar, defs_kin_rotary_delta, defs_kin_winch,
|
||||
defs_kin_extruder, defs_kin_shaper,
|
||||
]
|
||||
|
||||
# Update filenames to an absolute path
|
||||
|
|
|
@ -0,0 +1,41 @@
|
|||
// Deltesian kinematics stepper pulse time generation
|
||||
//
|
||||
// Copyright (C) 2022 Fabrice Gallet <tircown@gmail.com>
|
||||
//
|
||||
// This file may be distributed under the terms of the GNU GPLv3 license.
|
||||
|
||||
#include <math.h> // sqrt
|
||||
#include <stddef.h> // offsetof
|
||||
#include <stdlib.h> // malloc
|
||||
#include <string.h> // memset
|
||||
#include "compiler.h" // __visible
|
||||
#include "itersolve.h" // struct stepper_kinematics
|
||||
#include "trapq.h" // move_get_coord
|
||||
|
||||
struct deltesian_stepper {
|
||||
struct stepper_kinematics sk;
|
||||
double arm2, arm_x;
|
||||
};
|
||||
|
||||
static double
|
||||
deltesian_stepper_calc_position(struct stepper_kinematics *sk, struct move *m
|
||||
, double move_time)
|
||||
{
|
||||
struct deltesian_stepper *ds = container_of(
|
||||
sk, struct deltesian_stepper, sk);
|
||||
struct coord c = move_get_coord(m, move_time);
|
||||
double dx = c.x - ds->arm_x;
|
||||
return sqrt(ds->arm2 - dx*dx) + c.z;
|
||||
}
|
||||
|
||||
struct stepper_kinematics * __visible
|
||||
deltesian_stepper_alloc(double arm2, double arm_x)
|
||||
{
|
||||
struct deltesian_stepper *ds = malloc(sizeof(*ds));
|
||||
memset(ds, 0, sizeof(*ds));
|
||||
ds->arm2 = arm2;
|
||||
ds->arm_x = arm_x;
|
||||
ds->sk.calc_position_cb = deltesian_stepper_calc_position;
|
||||
ds->sk.active_flags = AF_X | AF_Z;
|
||||
return &ds->sk;
|
||||
}
|
|
@ -0,0 +1,184 @@
|
|||
# Code for handling the kinematics of deltesian robots
|
||||
#
|
||||
# Copyright (C) 2022 Fabrice Gallet <tircown@gmail.com>
|
||||
#
|
||||
# This file may be distributed under the terms of the GNU GPLv3 license.
|
||||
import math, logging
|
||||
import stepper
|
||||
|
||||
# Slow moves once the ratio of tower to XY movement exceeds SLOW_RATIO
|
||||
SLOW_RATIO = 3.
|
||||
|
||||
# Minimum angle with the horizontal for the arm to not exceed - in degrees
|
||||
MIN_ANGLE = 5.
|
||||
|
||||
class DeltesianKinematics:
|
||||
def __init__(self, toolhead, config):
|
||||
self.rails = [None] * 3
|
||||
stepper_configs = [config.getsection('stepper_' + s)
|
||||
for s in ['left', 'right', 'y']]
|
||||
self.rails[0] = stepper.PrinterRail(
|
||||
stepper_configs[0], need_position_minmax = False)
|
||||
def_pos_es = self.rails[0].get_homing_info().position_endstop
|
||||
self.rails[1] = stepper.PrinterRail(
|
||||
stepper_configs[1], need_position_minmax = False,
|
||||
default_position_endstop = def_pos_es)
|
||||
self.rails[2] = stepper.LookupMultiRail(stepper_configs[2])
|
||||
arm_x = self.arm_x = [None] * 2
|
||||
arm_x[0] = stepper_configs[0].getfloat('arm_x_length', above=0.)
|
||||
arm_x[1] = stepper_configs[1].getfloat('arm_x_length', arm_x[0],
|
||||
above=0.)
|
||||
arm = [None] * 2
|
||||
arm[0] = stepper_configs[0].getfloat('arm_length', above=arm_x[0])
|
||||
arm[1] = stepper_configs[1].getfloat('arm_length', arm[0],
|
||||
above=arm_x[1])
|
||||
arm2 = self.arm2 = [a**2 for a in arm]
|
||||
self.rails[0].setup_itersolve(
|
||||
'deltesian_stepper_alloc', arm2[0], -arm_x[0])
|
||||
self.rails[1].setup_itersolve(
|
||||
'deltesian_stepper_alloc', arm2[1], arm_x[1])
|
||||
self.rails[2].setup_itersolve('cartesian_stepper_alloc', b'y')
|
||||
for s in self.get_steppers():
|
||||
s.set_trapq(toolhead.get_trapq())
|
||||
toolhead.register_step_generator(s.generate_steps)
|
||||
config.get_printer().register_event_handler(
|
||||
"stepper_enable:motor_off", self._motor_off)
|
||||
self.limits = [(1.0, -1.0)] * 3
|
||||
# X axis limits
|
||||
min_angle = config.getfloat('min_angle', MIN_ANGLE,
|
||||
minval=0., maxval=90.)
|
||||
cos_angle = math.cos(math.radians(min_angle))
|
||||
x_kin_min = math.ceil( -min(arm_x[0], cos_angle * arm[1] - arm_x[1]))
|
||||
x_kin_max = math.floor( min(arm_x[1], cos_angle * arm[0] - arm_x[0]))
|
||||
x_kin_range = min(x_kin_max - x_kin_min, x_kin_max * 2, -x_kin_min * 2)
|
||||
print_width = config.getfloat('print_width', None, minval=0.,
|
||||
maxval=x_kin_range)
|
||||
if print_width:
|
||||
self.limits[0] = (-print_width * 0.5, print_width * 0.5)
|
||||
else:
|
||||
self.limits[0] = (x_kin_min, x_kin_max)
|
||||
# Y axis limits
|
||||
self.limits[1] = self.rails[2].get_range()
|
||||
# Z axis limits
|
||||
pmax = [r.get_homing_info().position_endstop for r in self.rails[:2]]
|
||||
self._abs_endstop = [p + math.sqrt(a2 - ax**2) for p, a2, ax
|
||||
in zip( pmax, arm2, arm_x )]
|
||||
self.home_z = self._actuator_to_cartesian(self._abs_endstop)[1]
|
||||
z_max = min([self._pillars_z_max(x) for x in self.limits[0]])
|
||||
z_min = config.getfloat('minimum_z_position', 0, maxval=z_max)
|
||||
self.limits[2] = (z_min, z_max)
|
||||
# Limit the speed/accel if is is at the extreme values of the x axis
|
||||
slow_ratio = config.getfloat('slow_ratio', SLOW_RATIO, minval=0.)
|
||||
self.slow_x2 = self.very_slow_x2 = None
|
||||
if slow_ratio > 0.:
|
||||
sr2 = slow_ratio ** 2
|
||||
self.slow_x2 = min([math.sqrt((sr2 * a2) / (sr2 + 1))
|
||||
- axl for a2, axl in zip(arm2, arm_x)]) ** 2
|
||||
self.very_slow_x2 = min([math.sqrt((2 * sr2 * a2) / (2 * sr2 + 1))
|
||||
- axl for a2, axl in zip(arm2, arm_x)]) ** 2
|
||||
logging.info("Deltesian kinematics: moves slowed past %.2fmm"
|
||||
" and %.2fmm"
|
||||
% (math.sqrt(self.slow_x2),
|
||||
math.sqrt(self.very_slow_x2)))
|
||||
# Setup boundary checks
|
||||
max_velocity, max_accel = toolhead.get_max_velocity()
|
||||
self.max_z_velocity = config.getfloat('max_z_velocity', max_velocity,
|
||||
above=0., maxval=max_velocity)
|
||||
self.max_z_accel = config.getfloat('max_z_accel', max_accel,
|
||||
above=0., maxval=max_accel)
|
||||
self.axes_min = toolhead.Coord(*[l[0] for l in self.limits], e=0.)
|
||||
self.axes_max = toolhead.Coord(*[l[1] for l in self.limits], e=0.)
|
||||
self.homed_axis = [False] * 3
|
||||
self.set_position([0., 0., 0.], ())
|
||||
def get_steppers(self):
|
||||
return [s for rail in self.rails for s in rail.get_steppers()]
|
||||
def _actuator_to_cartesian(self, sp):
|
||||
arm_x, arm2 = self.arm_x, self.arm2
|
||||
dx, dz = sum(arm_x), sp[1] - sp[0]
|
||||
pivots = math.sqrt(dx**2 + dz**2)
|
||||
# Trilateration w/ reference frame along left to right pivots
|
||||
xt = (pivots**2 + arm2[0] - arm2[1]) / (2 * pivots)
|
||||
zt = math.sqrt(arm2[0] - xt**2)
|
||||
# Rotation and translation of the reference frame
|
||||
x = xt * dx / pivots + zt * dz / pivots - arm_x[0]
|
||||
z = xt * dz / pivots - zt * dx / pivots + sp[0]
|
||||
return [x, z]
|
||||
def _pillars_z_max(self, x):
|
||||
arm_x, arm2 = self.arm_x, self.arm2
|
||||
dz = (math.sqrt(arm2[0] - (arm_x[0] + x)**2),
|
||||
math.sqrt(arm2[1] - (arm_x[1] - x)**2))
|
||||
return min([o - z for o, z in zip(self._abs_endstop, dz)])
|
||||
def calc_position(self, stepper_positions):
|
||||
sp = [stepper_positions[rail.get_name()] for rail in self.rails]
|
||||
x, z = self._actuator_to_cartesian(sp[:2])
|
||||
return [x, sp[2], z]
|
||||
def set_position(self, newpos, homing_axes):
|
||||
for rail in self.rails:
|
||||
rail.set_position(newpos)
|
||||
for n in homing_axes:
|
||||
self.homed_axis[n] = True
|
||||
def home(self, homing_state):
|
||||
homing_axes = homing_state.get_axes()
|
||||
home_xz = 0 in homing_axes or 2 in homing_axes
|
||||
home_y = 1 in homing_axes
|
||||
forceaxes = ([0, 1, 2] if (home_xz and home_y) else
|
||||
[0, 2] if home_xz else [1] if home_y else [])
|
||||
homing_state.set_axes(forceaxes)
|
||||
homepos = [None] * 4
|
||||
if home_xz:
|
||||
homing_state.set_axes([0, 1, 2] if home_y else [0, 2])
|
||||
homepos[0], homepos[2] = 0., self.home_z
|
||||
forcepos = list(homepos)
|
||||
dz2 = [(a2 - ax ** 2) for a2, ax in zip(self.arm2, self.arm_x)]
|
||||
forcepos[2] = -1.5 * math.sqrt(max(dz2))
|
||||
homing_state.home_rails(self.rails[:2], forcepos, homepos)
|
||||
if home_y:
|
||||
position_min, position_max = self.rails[2].get_range()
|
||||
hi = self.rails[2].get_homing_info()
|
||||
homepos[1] = hi.position_endstop
|
||||
forcepos = list(homepos)
|
||||
if hi.positive_dir:
|
||||
forcepos[1] -= 1.5 * (hi.position_endstop - position_min)
|
||||
else:
|
||||
forcepos[1] += 1.5 * (position_max - hi.position_endstop)
|
||||
homing_state.home_rails([self.rails[2]], forcepos, homepos)
|
||||
def _motor_off(self, print_time):
|
||||
self.homed_axis = [False] * 3
|
||||
def check_move(self, move):
|
||||
limits = list(map(list, self.limits))
|
||||
spos, epos = move.start_pos, move.end_pos
|
||||
homing_move = False
|
||||
if epos[0] == 0. and epos[2] == self.home_z and not move.axes_d[1]:
|
||||
# Identify a homing move
|
||||
homing_move = True
|
||||
elif epos[2] > limits[2][1]:
|
||||
# Moves at the very top - adapt limits depending on the X position
|
||||
limits[2][1] = self._pillars_z_max(epos[0])
|
||||
for i in (i for i, v in enumerate(move.axes_d[:3]) if v):
|
||||
if not self.homed_axis[i]:
|
||||
raise move.move_error("Must home axis first")
|
||||
# Move out of range - verify not a homing move
|
||||
if epos[i] < limits[i][0] or epos[i] > limits[i][1]:
|
||||
if not homing_move:
|
||||
raise move.move_error()
|
||||
if move.axes_d[2]:
|
||||
z_ratio = move.move_d / abs(move.axes_d[2])
|
||||
move.limit_speed(
|
||||
self.max_z_velocity * z_ratio, self.max_z_accel * z_ratio)
|
||||
# Limit the speed/accel if is is at the extreme values of the x axis
|
||||
if move.axes_d[0] and self.slow_x2 and self.very_slow_x2:
|
||||
move_x2 = max(spos[0] ** 2, epos[0] ** 2)
|
||||
if move_x2 > self.very_slow_x2:
|
||||
move.limit_speed(self.max_velocity *0.25, self.max_accel *0.25)
|
||||
elif move_x2 > self.slow_x2:
|
||||
move.limit_speed(self.max_velocity *0.50, self.max_accel *0.50)
|
||||
def get_status(self, eventtime):
|
||||
axes = [a for a, b in zip("xyz", self.homed_axis) if b]
|
||||
return {
|
||||
'homed_axes': "".join(axes),
|
||||
'axis_minimum': self.axes_min,
|
||||
'axis_maximum': self.axes_max,
|
||||
}
|
||||
|
||||
def load_kinematics(toolhead, config):
|
||||
return DeltesianKinematics(toolhead, config)
|
|
@ -9,6 +9,7 @@ CONFIG ../../config/example-corexz.cfg
|
|||
CONFIG ../../config/example-hybrid-corexy.cfg
|
||||
CONFIG ../../config/example-hybrid-corexz.cfg
|
||||
CONFIG ../../config/example-delta.cfg
|
||||
CONFIG ../../config/example-deltesian.cfg
|
||||
CONFIG ../../config/example-rotary-delta.cfg
|
||||
CONFIG ../../config/example-winch.cfg
|
||||
|
||||
|
|
Loading…
Reference in New Issue