From 33b809714f8f0b018024f1636c76cc4c85648fd0 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Fri, 7 Apr 2017 11:47:24 -0400 Subject: [PATCH] delta: Do reverse direction checking in C code Calculate where a tower must reverse direction during a move in the C code instead of the delta.py kinematic code. This simplifies the python code. Signed-off-by: Kevin O'Connor --- klippy/delta.py | 86 +++++++++++-------------------------------- klippy/stepcompress.c | 38 +++++++++++++++++-- 2 files changed, 57 insertions(+), 67 deletions(-) diff --git a/klippy/delta.py b/klippy/delta.py index 8859e17b..ac8d025f 100644 --- a/klippy/delta.py +++ b/klippy/delta.py @@ -1,6 +1,6 @@ # Code for handling the kinematics of linear delta robots # -# Copyright (C) 2016 Kevin O'Connor +# Copyright (C) 2016,2017 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. import math, logging @@ -165,14 +165,14 @@ class DeltaKinematics: if self.need_motor_enable: self._check_motor_enable(move_time) axes_d = move.axes_d - move_d = movexy_d = move.move_d + move_d = move.move_d movexy_r = 1. movez_r = 0. - inv_movexy_d = 1. / movexy_d + inv_movexy_d = 1. / move_d if not axes_d[0] and not axes_d[1]: # Z only move movez_r = axes_d[2] * inv_movexy_d - movexy_d = movexy_r = inv_movexy_d = 0. + movexy_r = inv_movexy_d = 0. elif axes_d[2]: # XY+Z move movexy_d = math.sqrt(axes_d[0]**2 + axes_d[1]**2) @@ -184,10 +184,9 @@ class DeltaKinematics: accel = move.accel cruise_v = move.cruise_v - accel_t = move.accel_t - cruise_end_t = accel_t + move.cruise_t accel_d = move.accel_r * move_d - cruise_end_d = accel_d + move.cruise_r * move_d + cruise_d = move.cruise_r * move_d + decel_d = move.decel_r * move_d for i in StepList: # Calculate a virtual tower along the line of movement at @@ -197,70 +196,29 @@ class DeltaKinematics: vt_startxy_d = (towerx_d*axes_d[0] + towery_d*axes_d[1])*inv_movexy_d tangentxy_d2 = towerx_d**2 + towery_d**2 - vt_startxy_d**2 vt_arm_d = math.sqrt(self.arm_length2 - tangentxy_d2) - - # Calculate accel/cruise/decel portions of move - reversexy_d = vt_startxy_d + vt_arm_d*movez_r - accel_up_d = cruise_up_d = decel_up_d = 0. - accel_down_d = cruise_down_d = decel_down_d = 0. - if reversexy_d <= 0.: - accel_down_d = accel_d - cruise_down_d = cruise_end_d - decel_down_d = move_d - elif reversexy_d >= movexy_d: - accel_up_d = accel_d - cruise_up_d = cruise_end_d - decel_up_d = move_d - elif reversexy_d < accel_d * movexy_r: - accel_up_d = reversexy_d * move_d * inv_movexy_d - accel_down_d = accel_d - cruise_down_d = cruise_end_d - decel_down_d = move_d - elif reversexy_d < cruise_end_d * movexy_r: - accel_up_d = accel_d - cruise_up_d = reversexy_d * move_d * inv_movexy_d - cruise_down_d = cruise_end_d - decel_down_d = move_d - else: - accel_up_d = accel_d - cruise_up_d = cruise_end_d - decel_up_d = reversexy_d * move_d * inv_movexy_d - decel_down_d = move_d + vt_startz = origz # Generate steps mcu_stepper = self.steppers[i].mcu_stepper mcu_time = mcu_stepper.print_to_mcu_time(move_time) - if accel_up_d > 0.: + if accel_d: mcu_stepper.step_delta( - mcu_time, accel_up_d, move.start_v, accel, - origz, vt_startxy_d, vt_arm_d, movez_r) - if cruise_up_d > 0.: + mcu_time, accel_d, move.start_v, accel, + vt_startz, vt_startxy_d, vt_arm_d, movez_r) + vt_startz += accel_d * movez_r + vt_startxy_d -= accel_d * movexy_r + mcu_time += move.accel_t + if cruise_d: mcu_stepper.step_delta( - mcu_time + accel_t, cruise_up_d - accel_d, cruise_v, 0., - origz + accel_d*movez_r, vt_startxy_d - accel_d*movexy_r, - vt_arm_d, movez_r) - if decel_up_d > 0.: + mcu_time, cruise_d, cruise_v, 0., + vt_startz, vt_startxy_d, vt_arm_d, movez_r) + vt_startz += cruise_d * movez_r + vt_startxy_d -= cruise_d * movexy_r + mcu_time += move.cruise_t + if decel_d: mcu_stepper.step_delta( - mcu_time + cruise_end_t, decel_up_d - cruise_end_d, - cruise_v, -accel, - origz + cruise_end_d*movez_r, - vt_startxy_d - cruise_end_d*movexy_r, - vt_arm_d, movez_r) - if accel_down_d > 0.: - mcu_stepper.step_delta( - mcu_time, -accel_down_d, move.start_v, accel, - origz, vt_startxy_d, vt_arm_d, movez_r) - if cruise_down_d > 0.: - mcu_stepper.step_delta( - mcu_time + accel_t, accel_d - cruise_down_d, cruise_v, 0., - origz + accel_d*movez_r, vt_startxy_d - accel_d*movexy_r, - vt_arm_d, movez_r) - if decel_down_d > 0.: - mcu_stepper.step_delta( - mcu_time + cruise_end_t, cruise_end_d - decel_down_d, - cruise_v, -accel, - origz + cruise_end_d*movez_r, - vt_startxy_d - cruise_end_d*movexy_r, - vt_arm_d, movez_r) + mcu_time, decel_d, cruise_v, -accel, + vt_startz, vt_startxy_d, vt_arm_d, movez_r) ###################################################################### diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c index d9eeefc3..761c16c2 100644 --- a/klippy/stepcompress.c +++ b/klippy/stepcompress.c @@ -1,6 +1,6 @@ // Stepper pulse schedule compression // -// Copyright (C) 2016 Kevin O'Connor +// Copyright (C) 2016,2017 Kevin O'Connor // // This file may be distributed under the terms of the GNU GPLv3 license. // @@ -531,8 +531,8 @@ stepcompress_push_const( } // Schedule steps using delta kinematics -int32_t -stepcompress_push_delta( +static int32_t +_stepcompress_push_delta( struct stepcompress *sc, double clock_offset, double move_sd , double start_sv, double accel , double height, double startxy_sd, double arm_sd, double movez_r) @@ -627,6 +627,38 @@ stepcompress_push_delta( return res; } +int32_t +stepcompress_push_delta( + struct stepcompress *sc, double clock_offset, double move_sd + , double start_sv, double accel + , double height, double startxy_sd, double arm_sd, double movez_r) +{ + double reversexy_sd = startxy_sd + arm_sd*movez_r; + if (reversexy_sd <= 0.) + // All steps are in down direction + return _stepcompress_push_delta( + sc, clock_offset, -move_sd, start_sv, accel + , height, startxy_sd, arm_sd, movez_r); + double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.; + if (reversexy_sd >= move_sd * movexy_r) + // All steps are in up direction + return _stepcompress_push_delta( + sc, clock_offset, move_sd, start_sv, accel + , height, startxy_sd, arm_sd, movez_r); + // Steps in both up and down direction + int res1 = _stepcompress_push_delta( + sc, clock_offset, reversexy_sd / movexy_r, start_sv, accel + , height, startxy_sd, arm_sd, movez_r); + if (res1 == ERROR_RET) + return res1; + int res2 = _stepcompress_push_delta( + sc, clock_offset, -move_sd, start_sv, accel + , height + res1, startxy_sd, arm_sd, movez_r); + if (res2 == ERROR_RET) + return res2; + return res1 + res2; +} + /**************************************************************** * Step compress synchronization