diff options
| -rw-r--r-- | config/example-corexy.cfg | 84 | ||||
| -rw-r--r-- | config/example.cfg | 4 | ||||
| -rw-r--r-- | docs/Todo.md | 2 | ||||
| -rw-r--r-- | klippy/corexy.py | 164 | ||||
| -rw-r--r-- | klippy/toolhead.py | 3 |
5 files changed, 254 insertions, 3 deletions
diff --git a/config/example-corexy.cfg b/config/example-corexy.cfg new file mode 100644 index 00000000..6d3f8a98 --- /dev/null +++ b/config/example-corexy.cfg @@ -0,0 +1,84 @@ +# This file serves as documentation for config parameters of corexy +# style printers. One may copy and edit this file to configure a new +# corexy printer. Only parameters unique to corexy printers are +# described here - see the "example.cfg" file for description of +# common config parameters. + +# DO NOT COPY THIS FILE WITHOUT CAREFULLY READING AND UPDATING IT +# FIRST. Incorrectly configured parameters may cause damage. + +# The stepper_x section is used to describe the X axis as well as the +# stepper controlling the X+Y movement +[stepper_x] +step_pin: ar54 +dir_pin: ar55 +enable_pin: !ar38 +step_distance: .01 +endstop_pin: ^ar2 +homing_speed: 50.0 +position_min: 0 +position_endstop: 0 +position_max: 200 + +# The stepper_y section is used to describe the Y axis as well as the +# stepper controlling the X-Y movement +[stepper_y] +step_pin: ar60 +dir_pin: ar61 +enable_pin: !ar56 +step_distance: .01 +endstop_pin: ^ar15 +homing_speed: 50.0 +position_min: 0 +position_endstop: 0 +position_max: 200 + +[stepper_z] +step_pin: ar46 +dir_pin: ar48 +enable_pin: !ar62 +step_distance: .01 +endstop_pin: ^ar19 +position_min: 0.1 +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: corexy +# This option must be "corexy" for corexy printers. +max_velocity: 300 +max_accel: 3000 +max_z_velocity: 25 +max_z_accel: 30 diff --git a/config/example.cfg b/config/example.cfg index d94e2b03..8abaf9f7 100644 --- a/config/example.cfg +++ b/config/example.cfg @@ -1,6 +1,8 @@ # This file serves as documentation for config parameters. One may # copy and edit this file to configure a new cartesian style -# printer. For delta style printers, see the "example-delta.cfg" file. +# printer. For delta style printers, see the "example-delta.cfg" +# file. For corexy/h-bot style printers, see the "example-corexy.cfg" +# file. # DO NOT COPY THIS FILE WITHOUT CAREFULLY READING AND UPDATING IT # FIRST. Incorrectly configured parameters may cause damage. diff --git a/docs/Todo.md b/docs/Todo.md index 3237fe64..b6829393 100644 --- a/docs/Todo.md +++ b/docs/Todo.md @@ -85,7 +85,7 @@ Hardware features * Smoothieboard / NXP LPC1769 (ARM cortex-M3) * Unix based scheduling; Unix based real-time scheduling -* Support for additional kinematics: scara, corexy, etc. +* Support for additional kinematics: scara, etc. * Support shared motor enable GPIO lines. diff --git a/klippy/corexy.py b/klippy/corexy.py new file mode 100644 index 00000000..f8b2a052 --- /dev/null +++ b/klippy/corexy.py @@ -0,0 +1,164 @@ +# Code for handling the kinematics of corexy robots +# +# Copyright (C) 2017 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import logging +import stepper, homing + +StepList = (0, 1, 2) + +class CoreXYKinematics: + def __init__(self, printer, config): + self.steppers = [stepper.PrinterStepper( + printer, config.getsection('stepper_' + n), n) + for n in ['x', 'y', 'z']] + self.steppers[0].mcu_endstop.add_stepper(self.steppers[1].mcu_stepper) + self.steppers[1].mcu_endstop.add_stepper(self.steppers[0].mcu_stepper) + self.max_z_velocity = config.getfloat('max_z_velocity', 9999999.9) + self.max_z_accel = config.getfloat('max_z_accel', 9999999.9) + self.need_motor_enable = True + self.limits = [(1.0, -1.0)] * 3 + def set_max_jerk(self, max_xy_halt_velocity, max_velocity, max_accel): + self.steppers[0].set_max_jerk(max_xy_halt_velocity, max_accel) + self.steppers[1].set_max_jerk(max_xy_halt_velocity, max_accel) + self.steppers[2].set_max_jerk(0., self.max_z_accel) + def set_position(self, newpos): + pos = (newpos[0] + newpos[1], newpos[0] - newpos[1], newpos[2]) + for i in StepList: + s = self.steppers[i] + if pos[i] >= 0.: + steppos = int(pos[i]*s.inv_step_dist + 0.5) + else: + steppos = int(pos[i]*s.inv_step_dist - 0.5) + s.mcu_stepper.set_position(steppos) + def home(self, homing_state): + # Each axis is homed independently and in order + for axis in homing_state.get_axes(): + s = self.steppers[axis] + self.limits[axis] = (s.position_min, s.position_max) + # Determine moves + if s.homing_positive_dir: + pos = s.position_endstop - 1.5*( + s.position_endstop - s.position_min) + rpos = s.position_endstop - s.homing_retract_dist + r2pos = rpos - s.homing_retract_dist + else: + pos = s.position_endstop + 1.5*( + s.position_max - s.position_endstop) + rpos = s.position_endstop + s.homing_retract_dist + r2pos = rpos + s.homing_retract_dist + # Initial homing + homepos = [None, None, None, None] + homepos[axis] = s.position_endstop + coord = [None, None, None, None] + coord[axis] = pos + homing_state.home(list(coord), homepos, [s], s.homing_speed) + # Retract + coord[axis] = rpos + homing_state.retract(list(coord), s.homing_speed) + # Home again + coord[axis] = r2pos + homing_state.home( + list(coord), homepos, [s], s.homing_speed/2.0, second_home=True) + if axis == 2: + # Support endstop phase detection on Z axis + coord[axis] = (s.position_endstop + + s.get_homed_offset()*s.step_dist) + homing_state.set_homed_position(coord) + def motor_off(self, move_time): + self.limits = [(1.0, -1.0)] * 3 + for stepper in self.steppers: + stepper.motor_enable(move_time, 0) + self.need_motor_enable = True + def _check_motor_enable(self, move_time, move): + if move.axes_d[0] or move.axes_d[1]: + self.steppers[0].motor_enable(move_time, 1) + self.steppers[1].motor_enable(move_time, 1) + if move.axes_d[2]: + self.steppers[2].motor_enable(move_time, 1) + need_motor_enable = False + for i in StepList: + need_motor_enable |= self.steppers[i].need_motor_enable + self.need_motor_enable = need_motor_enable + def query_endstops(self, print_time): + endstops = [(s, s.query_endstop(print_time)) for s in self.steppers] + return [(s.name, es.query_endstop_wait()) for s, es in endstops] + def _check_endstops(self, move): + end_pos = move.end_pos + for i in StepList: + if (move.axes_d[i] + and (end_pos[i] < self.limits[i][0] + or end_pos[i] > self.limits[i][1])): + if self.limits[i][0] > self.limits[i][1]: + raise homing.EndstopMoveError( + end_pos, "Must home axis first") + raise homing.EndstopMoveError(end_pos) + def check_move(self, move): + limits = self.limits + xpos, ypos = move.end_pos[:2] + if (xpos < limits[0][0] or xpos > limits[0][1] + or ypos < limits[1][0] or ypos > limits[1][1]): + self._check_endstops(move) + if not move.axes_d[2]: + # Normal XY move - use defaults + return + # Move with Z - update velocity and accel for slower Z axis + self._check_endstops(move) + 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, move_time, move): + if self.need_motor_enable: + self._check_motor_enable(move_time, move) + inv_accel = 1. / move.accel + inv_cruise_v = 1. / move.cruise_v + sxp = move.start_pos[0] + syp = move.start_pos[1] + start_pos = (sxp + syp, sxp - syp, move.start_pos[2]) + exp = move.end_pos[0] + eyp = move.end_pos[1] + end_pos = (exp + eyp, exp - eyp, move.start_pos[2]) + axes_d = (end_pos[0] - start_pos[0], end_pos[1] - start_pos[1], + move.axes_d[2]) + for i in StepList: + if not axes_d[i]: + continue + mcu_stepper = self.steppers[i].mcu_stepper + mcu_time = mcu_stepper.print_to_mcu_time(move_time) + step_pos = mcu_stepper.commanded_position + inv_step_dist = self.steppers[i].inv_step_dist + step_offset = step_pos - start_pos[i] * inv_step_dist + steps = axes_d[i] * inv_step_dist + move_step_d = move.move_d / abs(steps) + + # Acceleration steps + accel_multiplier = 2.0 * move_step_d * inv_accel + if move.accel_r: + #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel + accel_time_offset = move.start_v * inv_accel + accel_sqrt_offset = accel_time_offset**2 + accel_steps = move.accel_r * steps + count = mcu_stepper.step_sqrt( + mcu_time - accel_time_offset, accel_steps, step_offset + , accel_sqrt_offset, accel_multiplier) + step_offset += count - accel_steps + mcu_time += move.accel_t + # Cruising steps + if move.cruise_r: + #t = pos/cruise_v + cruise_multiplier = move_step_d * inv_cruise_v + cruise_steps = move.cruise_r * steps + count = mcu_stepper.step_factor( + mcu_time, cruise_steps, step_offset, cruise_multiplier) + step_offset += count - cruise_steps + mcu_time += move.cruise_t + # Deceleration steps + if move.decel_r: + #t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel) + decel_time_offset = move.cruise_v * inv_accel + decel_sqrt_offset = decel_time_offset**2 + decel_steps = move.decel_r * steps + count = mcu_stepper.step_sqrt( + mcu_time + decel_time_offset, decel_steps, step_offset + , decel_sqrt_offset, -accel_multiplier) diff --git a/klippy/toolhead.py b/klippy/toolhead.py index ca7deebc..d7b677ce 100644 --- a/klippy/toolhead.py +++ b/klippy/toolhead.py @@ -4,7 +4,7 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. import math, logging -import cartesian, delta, extruder +import cartesian, corexy, delta, extruder # Common suffixes: _d is distance (in mm), _v is velocity (in # mm/second), _v2 is velocity squared (mm^2/s^2), _t is time (in @@ -185,6 +185,7 @@ class ToolHead: if self.extruder is None: self.extruder = extruder.DummyExtruder() kintypes = {'cartesian': cartesian.CartKinematics, + 'corexy': corexy.CoreXYKinematics, 'delta': delta.DeltaKinematics} self.kin = config.getchoice('kinematics', kintypes)(printer, config) self.max_speed = config.getfloat('max_velocity') |