From ec9cb3a1b302bddf10bde84fc87ae2a8ad6230b3 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Thu, 23 Aug 2018 14:21:58 -0400 Subject: [PATCH] polar: Experimental support for polar kinematics Signed-off-by: Kevin O'Connor --- config/example-polar.cfg | 86 ++++++++++++++++++++++++ klippy/chelper/__init__.py | 10 ++- klippy/chelper/kin_polar.c | 41 ++++++++++++ klippy/kinematics/polar.py | 131 +++++++++++++++++++++++++++++++++++++ test/klippy/polar.test | 32 +++++++++ 5 files changed, 298 insertions(+), 2 deletions(-) create mode 100644 config/example-polar.cfg create mode 100644 klippy/chelper/kin_polar.c create mode 100644 klippy/kinematics/polar.py create mode 100644 test/klippy/polar.test diff --git a/config/example-polar.cfg b/config/example-polar.cfg new file mode 100644 index 00000000..3e5c1f62 --- /dev/null +++ b/config/example-polar.cfg @@ -0,0 +1,86 @@ +# This file serves as documentation for config parameters of "polar" +# style printers. One may copy and edit this file to configure a new +# polar printer. + +# POLAR KINEMATICS ARE A WORK IN PROGRESS. Moves around the 0,0 +# position are known to not work properly. Moves to a negative Y +# coordinate from a positive Y coordinate (and vice-versa) when the +# head is at a negative X coordinate also do not work properly. + +# Only parameters unique to polar printers are described here - see +# the "example.cfg" file for description of common config parameters. + +# The stepper_bed section is used to describe the stepper controlling +# the bed. +[stepper_bed] +step_pin: ar54 +dir_pin: ar55 +enable_pin: !ar38 +step_distance: 0.000981748 +# On a polar printer the step_distance is the amount each step pulse +# moves the bed in radians (for example, a 1.8 degree stepper with +# 16 micro-steps would be 1.8 / 360 * pi / 16 == 0.000981748). This +# parameter must be provided. + +# The stepper_arm section is used to describe the stepper controlling +# the carriage on the arm. +[stepper_arm] +step_pin: ar60 +dir_pin: ar61 +enable_pin: !ar56 +step_distance: .01 +endstop_pin: ^ar14 +position_endstop: 300 +position_max: 300 +homing_speed: 50 + +# The stepper_z section is used to describe the stepper controlling +# the Z axis. +[stepper_z] +step_pin: ar46 +dir_pin: ar48 +enable_pin: !ar62 +step_distance: .0025 +endstop_pin: ^ar18 +position_endstop: 0.5 +position_max: 200 + +[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 + +[fan] +pin: ar9 + +[mcu] +serial: /dev/ttyACM0 +pin_map: arduino + +[printer] +kinematics: polar +# This option must be "polar" for polar printers. +max_velocity: 300 +max_accel: 3000 +max_z_velocity: 25 +max_z_accel: 30 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) diff --git a/test/klippy/polar.test b/test/klippy/polar.test new file mode 100644 index 00000000..54cd1c14 --- /dev/null +++ b/test/klippy/polar.test @@ -0,0 +1,32 @@ +# Test case for basic movement on polar printers +CONFIG ../../config/example-polar.cfg +DICTIONARY atmega2560-16mhz.dict + +; Start by homing the printer. +G28 +G90 +G1 F6000 + +; Z / X / Y moves +G1 Z1 +G1 X1 +G1 Y1 + +; Delayed moves +G1 Y2 +G4 P100 +G1 Y1.5 +M400 +G1 Y1 + +; diagonal moves +G1 X10 Y0 +G1 X1 Z2 +G1 X0 Y1 Z1 + +; extrude only moves +G1 E1 +G1 E0 + +; regular extrude move +G1 X10 Y0 E.01