diff options
Diffstat (limited to 'klippy')
| -rw-r--r-- | klippy/chelper/__init__.py | 9 | ||||
| -rw-r--r-- | klippy/chelper/kin_winch.c | 40 | ||||
| -rw-r--r-- | klippy/kinematics/winch.py | 62 |
3 files changed, 109 insertions, 2 deletions
diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 6427ba0f..fba5eb48 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -17,7 +17,7 @@ COMPILE_CMD = ("gcc -Wall -g -O2 -shared -fPIC" SOURCE_FILES = [ 'pyhelper.c', 'serialqueue.c', 'stepcompress.c', 'itersolve.c', 'kin_cartesian.c', 'kin_corexy.c', 'kin_delta.c', 'kin_polar.c', - 'kin_extruder.c', + 'kin_winch.c', 'kin_extruder.c', ] DEST_LIB = "c_helper.so" OTHER_FILES = [ @@ -75,6 +75,11 @@ defs_kin_polar = """ struct stepper_kinematics *polar_stepper_alloc(char type); """ +defs_kin_winch = """ + struct stepper_kinematics *winch_stepper_alloc(double anchor_x + , double anchor_y, double anchor_z); +""" + defs_kin_extruder = """ struct stepper_kinematics *extruder_stepper_alloc(void); void extruder_move_fill(struct move *m, double print_time @@ -122,7 +127,7 @@ defs_std = """ defs_all = [ defs_pyhelper, defs_serialqueue, defs_std, defs_stepcompress, defs_itersolve, defs_kin_cartesian, defs_kin_corexy, defs_kin_delta, defs_kin_polar, - defs_kin_extruder + defs_kin_winch, defs_kin_extruder ] # Return the list of file modification times diff --git a/klippy/chelper/kin_winch.c b/klippy/chelper/kin_winch.c new file mode 100644 index 00000000..df894352 --- /dev/null +++ b/klippy/chelper/kin_winch.c @@ -0,0 +1,40 @@ +// Cable winch stepper kinematics +// +// 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 <stddef.h> // offsetof +#include <stdlib.h> // malloc +#include <string.h> // memset +#include "compiler.h" // __visible +#include "itersolve.h" // struct stepper_kinematics + +struct winch_stepper { + struct stepper_kinematics sk; + struct coord anchor; +}; + +static double +winch_stepper_calc_position(struct stepper_kinematics *sk, struct move *m + , double move_time) +{ + struct winch_stepper *hs = container_of(sk, struct winch_stepper, sk); + struct coord c = move_get_coord(m, move_time); + double dx = hs->anchor.x - c.x, dy = hs->anchor.y - c.y; + double dz = hs->anchor.z - c.z; + return sqrt(dx*dx + dy*dy + dz*dz); +} + +struct stepper_kinematics * __visible +winch_stepper_alloc(double anchor_x, double anchor_y, double anchor_z) +{ + struct winch_stepper *hs = malloc(sizeof(*hs)); + memset(hs, 0, sizeof(*hs)); + hs->anchor.x = anchor_x; + hs->anchor.y = anchor_y; + hs->anchor.z = anchor_z; + hs->sk.calc_position = winch_stepper_calc_position; + return &hs->sk; +} diff --git a/klippy/kinematics/winch.py b/klippy/kinematics/winch.py new file mode 100644 index 00000000..56b7658c --- /dev/null +++ b/klippy/kinematics/winch.py @@ -0,0 +1,62 @@ +# Code for handling the kinematics of cable winch robots +# +# Copyright (C) 2018 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import stepper, mathutil + +class WinchKinematics: + def __init__(self, toolhead, config): + # Setup steppers at each anchor + self.steppers = [] + self.anchors = [] + for i in range(26): + name = 'stepper_' + chr(ord('a') + i) + if i >= 3 and not config.has_section(name): + break + stepper_config = config.getsection(name) + s = stepper.PrinterStepper(stepper_config) + self.steppers.append(s) + a = tuple([stepper_config.getfloat('anchor_' + n) for n in 'xyz']) + self.anchors.append(a) + s.setup_itersolve('winch_stepper_alloc', *a) + # Setup stepper max halt velocity + max_velocity, max_accel = toolhead.get_max_velocity() + max_halt_velocity = toolhead.get_max_axis_halt() + for s in self.steppers: + s.set_max_jerk(max_halt_velocity, max_accel) + # Setup boundary checks + self.need_motor_enable = True + self.set_position([0., 0., 0.], ()) + def get_steppers(self, flags=""): + return list(self.steppers) + def calc_position(self): + # Use only first three steppers to calculate cartesian position + spos = [s.get_commanded_position() for s in self.steppers[:3]] + return mathutil.trilateration(self.anchors[:3], [sp*sp for sp in spos]) + def set_position(self, newpos, homing_axes): + for s in self.steppers: + s.set_position(newpos) + def home(self, homing_state): + # XXX - homing not implemented + homing_state.set_axes([0, 1, 2]) + homing_state.set_homed_position([0., 0., 0.]) + def motor_off(self, print_time): + for s in self.steppers: + s.motor_enable(print_time, 0) + self.need_motor_enable = True + def _check_motor_enable(self, print_time): + for s in self.steppers: + s.motor_enable(print_time, 1) + self.need_motor_enable = False + def check_move(self, move): + # XXX - boundary checks and speed limits not implemented + pass + def move(self, print_time, move): + if self.need_motor_enable: + self._check_motor_enable(print_time) + for s in self.steppers: + s.step_itersolve(move.cmove) + +def load_kinematics(toolhead, config): + return WinchKinematics(toolhead, config) |