From ec9cb3a1b302bddf10bde84fc87ae2a8ad6230b3 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Thu, 23 Aug 2018 14:21:58 -0400 Subject: polar: Experimental support for polar kinematics Signed-off-by: Kevin O'Connor --- klippy/chelper/__init__.py | 10 +++- klippy/chelper/kin_polar.c | 41 ++++++++++++++ klippy/kinematics/polar.py | 131 +++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 180 insertions(+), 2 deletions(-) create mode 100644 klippy/chelper/kin_polar.c create mode 100644 klippy/kinematics/polar.py (limited to 'klippy') diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 567807fa..6427ba0f 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -16,7 +16,8 @@ COMPILE_CMD = ("gcc -Wall -g -O2 -shared -fPIC" " -o %s %s") SOURCE_FILES = [ 'pyhelper.c', 'serialqueue.c', 'stepcompress.c', 'itersolve.c', - 'kin_cartesian.c', 'kin_corexy.c', 'kin_delta.c', 'kin_extruder.c' + 'kin_cartesian.c', 'kin_corexy.c', 'kin_delta.c', 'kin_polar.c', + 'kin_extruder.c', ] DEST_LIB = "c_helper.so" OTHER_FILES = [ @@ -70,6 +71,10 @@ defs_kin_delta = """ , double tower_x, double tower_y); """ +defs_kin_polar = """ + struct stepper_kinematics *polar_stepper_alloc(char type); +""" + defs_kin_extruder = """ struct stepper_kinematics *extruder_stepper_alloc(void); void extruder_move_fill(struct move *m, double print_time @@ -116,7 +121,8 @@ 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_extruder + defs_kin_cartesian, defs_kin_corexy, defs_kin_delta, defs_kin_polar, + defs_kin_extruder ] # Return the list of file modification times diff --git a/klippy/chelper/kin_polar.c b/klippy/chelper/kin_polar.c new file mode 100644 index 00000000..cc1f2eab --- /dev/null +++ b/klippy/chelper/kin_polar.c @@ -0,0 +1,41 @@ +// Polar kinematics stepper pulse time generation +// +// Copyright (C) 2018 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include // sqrt +#include // malloc +#include // memset +#include "compiler.h" // __visible +#include "itersolve.h" // struct stepper_kinematics + +static double +polar_stepper_radius_calc_position(struct stepper_kinematics *sk, struct move *m + , double move_time) +{ + struct coord c = move_get_coord(m, move_time); + return sqrt(c.x*c.x + c.y*c.y); +} + +static double +polar_stepper_angle_calc_position(struct stepper_kinematics *sk, struct move *m + , double move_time) +{ + struct coord c = move_get_coord(m, move_time); + // XXX - handle x==y==0 + // XXX - handle angle wrapping + return atan2(c.y, c.x); +} + +struct stepper_kinematics * __visible +polar_stepper_alloc(char type) +{ + struct stepper_kinematics *sk = malloc(sizeof(*sk)); + memset(sk, 0, sizeof(*sk)); + if (type == 'r') + sk->calc_position = polar_stepper_radius_calc_position; + else if (type == 'a') + sk->calc_position = polar_stepper_angle_calc_position; + return sk; +} diff --git a/klippy/kinematics/polar.py b/klippy/kinematics/polar.py new file mode 100644 index 00000000..2930152c --- /dev/null +++ b/klippy/kinematics/polar.py @@ -0,0 +1,131 @@ +# Code for handling the kinematics of polar robots +# +# Copyright (C) 2018 Kevin O'Connor +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import logging, math +import stepper, homing + +class PolarKinematics: + def __init__(self, toolhead, config): + # Setup axis steppers + stepper_bed = stepper.PrinterStepper(config.getsection('stepper_bed')) + rail_arm = stepper.PrinterRail(config.getsection('stepper_arm')) + rail_z = stepper.LookupMultiRail(config.getsection('stepper_z')) + stepper_bed.setup_itersolve('polar_stepper_alloc', 'a') + rail_arm.setup_itersolve('polar_stepper_alloc', 'r') + rail_z.setup_itersolve('cartesian_stepper_alloc', 'z') + self.rails = [rail_arm, rail_z] + self.steppers = [stepper_bed] + [ s for r in self.rails + for s in r.get_steppers() ] + # 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.need_motor_enable = True + self.limit_z = [(1.0, -1.0)] + self.limit_xy2 = -1. + # Setup stepper max halt velocity + max_halt_velocity = toolhead.get_max_axis_halt() + stepper_bed.set_max_jerk(max_halt_velocity, max_accel) + rail_arm.set_max_jerk(max_halt_velocity, max_accel) + rail_z.set_max_jerk(max_halt_velocity, max_accel) + def get_steppers(self, flags=""): + if flags == "Z": + return self.rails[1].get_steppers() + return list(self.steppers) + def calc_position(self): + bed_angle = self.steppers[0].get_commanded_position() + arm_pos = self.rails[0].get_commanded_position() + z_pos = self.rails[1].get_commanded_position() + return [math.cos(bed_angle) * arm_pos, math.sin(bed_angle) * arm_pos, + z_pos] + def set_position(self, newpos, homing_axes): + for s in self.steppers: + s.set_position(newpos) + if 2 in homing_axes: + self.limit_z = self.rails[1].get_range() + if 0 in homing_axes and 1 in homing_axes: + self.limit_xy2 = self.rails[0].get_range()[1]**2 + def _home_axis(self, homing_state, axis, rail): + # Determine movement + position_min, position_max = rail.get_range() + hi = rail.get_homing_info() + homepos = [None, None, None, None] + homepos[axis] = hi.position_endstop + if axis == 0: + homepos[1] = 0. + forcepos = list(homepos) + if hi.positive_dir: + forcepos[axis] -= hi.position_endstop - position_min + else: + forcepos[axis] += position_max - hi.position_endstop + # Perform homing + limit_speed = None + if axis == 2: + limit_speed = self.max_z_velocity + homing_state.home_rails([rail], forcepos, homepos, limit_speed) + def home(self, homing_state): + # Always home XY together + homing_axes = homing_state.get_axes() + home_xy = 0 in homing_axes or 1 in homing_axes + home_z = 2 in homing_axes + updated_axes = [] + if home_xy: + updated_axes = [0, 1] + if home_z: + updated_axes.append(2) + homing_state.set_axes(updated_axes) + # Do actual homing + if home_xy: + self._home_axis(homing_state, 0, self.rails[0]) + if home_z: + self._home_axis(homing_state, 2, self.rails[1]) + def motor_off(self, print_time): + self.limit_z = [(1.0, -1.0)] + self.limit_xy2 = -1. + for s in self.steppers: + s.motor_enable(print_time, 0) + self.need_motor_enable = True + def _check_motor_enable(self, print_time, move): + if move.axes_d[0] or move.axes_d[1]: + self.steppers[0].motor_enable(print_time, 1) + self.rails[0].motor_enable(print_time, 1) + if move.axes_d[2]: + self.rails[1].motor_enable(print_time, 1) + need_motor_enable = not self.steppers[0].is_motor_enabled() + for rail in self.rails: + need_motor_enable |= not rail.is_motor_enabled() + self.need_motor_enable = need_motor_enable + def check_move(self, move): + end_pos = move.end_pos + xy2 = end_pos[0]**2 + end_pos[1]**2 + if xy2 > self.limit_xy2: + if self.limit_xy2 < 0.: + raise homing.EndstopMoveError(end_pos, "Must home axis first") + raise homing.EndstopMoveError(end_pos) + if move.axes_d[2]: + if end_pos[2] < self.limit_z[0] or end_pos[2] > self.limit_z[1]: + if self.limit_z[0] > self.limit_z[1]: + raise homing.EndstopMoveError( + end_pos, "Must home axis first") + raise homing.EndstopMoveError(end_pos) + # Move with Z - update velocity and accel for slower Z axis + 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) + def move(self, print_time, move): + if self.need_motor_enable: + self._check_motor_enable(print_time, move) + axes_d = move.axes_d + cmove = move.cmove + if axes_d[0] or axes_d[1]: + self.steppers[0].step_itersolve(cmove) + self.rails[0].step_itersolve(cmove) + if axes_d[2]: + self.rails[1].step_itersolve(cmove) + +def load_kinematics(toolhead, config): + return PolarKinematics(toolhead, config) -- cgit v1.2.3-70-g09d2