From 5dc74f315231121ed56695fb4f68499d9bc5b5fa Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Thu, 12 Jul 2018 23:28:58 -0400 Subject: [PATCH] winch: Add experimental support for cable winch kinematics Signed-off-by: Kevin O'Connor --- config/example-winch.cfg | 96 ++++++++++++++++++++++++++++++++++++++ klippy/chelper/__init__.py | 9 +++- klippy/chelper/kin_winch.c | 40 ++++++++++++++++ klippy/kinematics/winch.py | 62 ++++++++++++++++++++++++ test/klippy/move.gcode | 3 ++ test/klippy/printers.test | 1 + 6 files changed, 209 insertions(+), 2 deletions(-) create mode 100644 config/example-winch.cfg create mode 100644 klippy/chelper/kin_winch.c create mode 100644 klippy/kinematics/winch.py diff --git a/config/example-winch.cfg b/config/example-winch.cfg new file mode 100644 index 00000000..3b33f9d8 --- /dev/null +++ b/config/example-winch.cfg @@ -0,0 +1,96 @@ +# This file serves as documentation for config parameters of cable +# winch style printers. One may copy and edit this file to configure a +# new cable winch printer. + +# CABLE WINCH SUPPORT IS EXPERIMENTAL - PROCEED WITH CAUTION! + +# Homing is not implemented on cable winch kinematics. In order to +# home the printer, manually send movement commands until the toolhead +# is at 0,0,0 and then issue a G28 command. + +# Only parameters unique to cable winch printers are described here - +# see the "example.cfg" file for description of common config +# parameters. + +# The stepper_a section describes the stepper connected to the first +# cable winch. A minimum of 3 and a maximum of 26 cable winches may be +# defined (stepper_a to stepper_z) though it is common to define 4. +[stepper_a] +step_pin: ar54 +dir_pin: ar55 +enable_pin: !ar38 +step_distance: .01 +# The step_distance is the nominal distance (in mm) the toolhead +# moves towards the cable winch on each step pulse. This parameter +# must be provided. +anchor_x: 0 +anchor_y: -2000 +anchor_z: -100 +# The x, y, and z position of the cable winch in cartesian space. +# These parameters must be provided. + +[stepper_b] +step_pin: ar60 +dir_pin: ar61 +enable_pin: !ar56 +step_distance: .01 +anchor_x: 2000 +anchor_y: 1000 +anchor_z: -100 + +[stepper_c] +step_pin: ar46 +dir_pin: ar48 +enable_pin: !ar62 +step_distance: .01 +anchor_x: -2000 +anchor_y: 1000 +anchor_z: -100 + +[stepper_d] +step_pin: ar36 +dir_pin: ar34 +enable_pin: !ar30 +step_distance: .01 +anchor_x: 0 +anchor_y: 0 +anchor_z: 3000 + +[extruder] +step_pin: ar26 +dir_pin: ar28 +enable_pin: !ar24 +step_distance: .0022 +nozzle_diameter: 0.400 +filament_diameter: 1.750 +heater_pin: ar10 +sensor_type: ATC Semitec 104GT-2 +sensor_pin: analog13 +control: pid +pid_Kp: 22.2 +pid_Ki: 1.08 +pid_Kd: 114 +min_temp: 0 +max_temp: 250 + +[heater_bed] +heater_pin: ar8 +sensor_type: EPCOS 100K B57560G104F +sensor_pin: analog14 +control: watermark +min_temp: 0 +max_temp: 130 + +# Print cooling fan (omit section if fan not present). +#[fan] +#pin: ar9 + +[mcu] +serial: /dev/ttyACM0 +pin_map: arduino + +[printer] +kinematics: winch +# This option must be "winch" for cable winch printers. +max_velocity: 300 +max_accel: 3000 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 +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include // sqrt +#include // offsetof +#include // malloc +#include // 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 +# +# 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) diff --git a/test/klippy/move.gcode b/test/klippy/move.gcode index 9bc1f0f6..b7217816 100644 --- a/test/klippy/move.gcode +++ b/test/klippy/move.gcode @@ -26,6 +26,9 @@ G1 X0 Y1 Z1 G1 E1 G1 E0 +; Verify GET_POSITION works +GET_POSITION + ; regular extrude move G1 X0 Y0 E.01 diff --git a/test/klippy/printers.test b/test/klippy/printers.test index 70e10040..40aa170d 100644 --- a/test/klippy/printers.test +++ b/test/klippy/printers.test @@ -6,6 +6,7 @@ DICTIONARY atmega2560-16mhz.dict CONFIG ../../config/example.cfg CONFIG ../../config/example-corexy.cfg CONFIG ../../config/example-delta.cfg +CONFIG ../../config/example-winch.cfg # Printers using the atmega2560 DICTIONARY atmega2560-16mhz.dict