From b915a2ad7d6eec6f7ef31fc5f13397029b927351 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Thu, 6 Apr 2017 11:09:08 -0400 Subject: [PATCH] delta: Make it clear that a "virtual tower" is created The delta code calculates a "virtual tower" along the line of movement. Rework the variable names and comments to make it clear that this is occurring. It is not necessary to pass the start_pos variable to the C code as it is simple to update the start_pos at the start of each movement. Signed-off-by: Kevin O'Connor --- klippy/chelper.py | 5 ++--- klippy/delta.py | 48 +++++++++++++++++++++++++------------------ klippy/mcu.py | 10 ++++----- klippy/stepcompress.c | 35 ++++++++++++++++--------------- 4 files changed, 52 insertions(+), 46 deletions(-) diff --git a/klippy/chelper.py b/klippy/chelper.py index 86994830..230f6a3d 100644 --- a/klippy/chelper.py +++ b/klippy/chelper.py @@ -25,9 +25,8 @@ defs_stepcompress = """ int32_t stepcompress_push_const(struct stepcompress *sc, double clock_offset , double step_offset, double steps, double start_sv, double accel); int32_t stepcompress_push_delta(struct stepcompress *sc - , double clock_offset, double start_pos, double steps, double start_sv - , double accel, double height, double closestxy_sd - , double closest_height2, double movez_r); + , double clock_offset, double move_sd, double start_sv, double accel + , double height, double startxy_sd, double arm_d, double movez_r); struct steppersync *steppersync_alloc(struct serialqueue *sq , struct stepcompress **sc_list, int sc_num, int move_num); diff --git a/klippy/delta.py b/klippy/delta.py index 5913e53a..8859e17b 100644 --- a/klippy/delta.py +++ b/klippy/delta.py @@ -162,23 +162,24 @@ class DeltaKinematics: limit_xy2 = -1. self.limit_xy2 = min(limit_xy2, self.slow_xy2) def move(self, move_time, move): + if self.need_motor_enable: + self._check_motor_enable(move_time) axes_d = move.axes_d move_d = movexy_d = move.move_d movexy_r = 1. movez_r = 0. inv_movexy_d = 1. / movexy_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. elif axes_d[2]: + # XY+Z move movexy_d = math.sqrt(axes_d[0]**2 + axes_d[1]**2) movexy_r = movexy_d * inv_movexy_d movez_r = axes_d[2] * inv_movexy_d inv_movexy_d = 1. / movexy_d - if self.need_motor_enable: - self._check_motor_enable(move_time) - origx, origy, origz = move.start_pos[:3] accel = move.accel @@ -189,15 +190,16 @@ class DeltaKinematics: cruise_end_d = accel_d + move.cruise_r * move_d for i in StepList: - # Find point on line of movement closest to tower + # Calculate a virtual tower along the line of movement at + # the point closest to this stepper's tower. towerx_d = self.towers[i][0] - origx towery_d = self.towers[i][1] - origy - closestxy_d = (towerx_d*axes_d[0] + towery_d*axes_d[1])*inv_movexy_d - tangentxy_d2 = towerx_d**2 + towery_d**2 - closestxy_d**2 - closest_height2 = self.arm_length2 - tangentxy_d2 + 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 = closestxy_d + math.sqrt(closest_height2)*movez_r + 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.: @@ -229,30 +231,36 @@ class DeltaKinematics: mcu_time = mcu_stepper.print_to_mcu_time(move_time) if accel_up_d > 0.: mcu_stepper.step_delta( - mcu_time, 0., accel_up_d, move.start_v, accel, - origz, closestxy_d, closest_height2, movez_r) + mcu_time, accel_up_d, move.start_v, accel, + origz, vt_startxy_d, vt_arm_d, movez_r) if cruise_up_d > 0.: mcu_stepper.step_delta( - mcu_time + accel_t, accel_d, cruise_up_d, cruise_v, 0., - origz, closestxy_d, closest_height2, movez_r) + 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_stepper.step_delta( - mcu_time + cruise_end_t, cruise_end_d, decel_up_d, + mcu_time + cruise_end_t, decel_up_d - cruise_end_d, cruise_v, -accel, - origz, closestxy_d, closest_height2, movez_r) + 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, 0., -accel_down_d, move.start_v, accel, - origz, closestxy_d, closest_height2, movez_r) + 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, closestxy_d, closest_height2, movez_r) + 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, + mcu_time + cruise_end_t, cruise_end_d - decel_down_d, cruise_v, -accel, - origz, closestxy_d, closest_height2, movez_r) + origz + cruise_end_d*movez_r, + vt_startxy_d - cruise_end_d*movexy_r, + vt_arm_d, movez_r) ###################################################################### diff --git a/klippy/mcu.py b/klippy/mcu.py index d28d21f1..5b12e83f 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -128,16 +128,14 @@ class MCU_stepper: if count == STEPCOMPRESS_ERROR_RET: raise error("Internal error in stepcompress") self._commanded_pos += count - def step_delta(self, mcu_time, start_pos, dist, start_v, accel - , height_base, closestxy_d, closest_height2, movez_r): + def step_delta(self, mcu_time, dist, start_v, accel + , height_base, startxy_d, arm_d, movez_r): inv_step_dist = self._inv_step_dist height = self._commanded_pos - height_base * inv_step_dist count = self._ffi_lib.stepcompress_push_delta( - self._stepqueue, mcu_time * self._mcu_freq, - -start_pos * inv_step_dist, dist * inv_step_dist, + self._stepqueue, mcu_time * self._mcu_freq, dist * inv_step_dist, start_v * self._velocity_factor, accel * self._accel_factor, - height, closestxy_d * inv_step_dist, - closest_height2 * inv_step_dist**2, movez_r) + height, startxy_d * inv_step_dist, arm_d * inv_step_dist, movez_r) if count == STEPCOMPRESS_ERROR_RET: raise error("Internal error in stepcompress") self._commanded_pos += count diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c index 4f59eaa2..d9eeefc3 100644 --- a/klippy/stepcompress.c +++ b/klippy/stepcompress.c @@ -533,25 +533,26 @@ stepcompress_push_const( // Schedule steps using delta kinematics int32_t stepcompress_push_delta( - struct stepcompress *sc, double clock_offset, double start_pos - , double steps, double start_sv, double accel - , double height, double closestxy_sd, double closest_height2, double movez_r) + 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) { // Calculate number of steps to take double step_dist = 1.; - if (steps < 0) { + if (move_sd < 0) { step_dist = -1.; - steps = -steps; + move_sd = -move_sd; } double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.; - double reldist = closestxy_sd - movexy_r*steps; - double end_height = safe_sqrt(closest_height2 - reldist*reldist); - int count = (end_height - height + movez_r*steps) * step_dist + .5; + double arm_sd2 = arm_sd * arm_sd; + double endxy_sd = startxy_sd - movexy_r*move_sd; + double end_height = safe_sqrt(arm_sd2 - endxy_sd*endxy_sd); + int count = (end_height + movez_r*move_sd - height) * step_dist + .5; if (count <= 0 || count > 10000000) { if (count) { - errorf("push_delta invalid count %d %d %f %f %f %f %f %f %f %f %f" - , sc->oid, count, clock_offset, start_pos, steps, start_sv - , accel, height, closestxy_sd, closest_height2, movez_r); + errorf("push_delta invalid count %d %d %f %f %f %f %f %f %f %f" + , sc->oid, count, clock_offset, move_sd, start_sv, accel + , height, startxy_sd, arm_sd, movez_r); return ERROR_RET; } return 0; @@ -563,7 +564,7 @@ stepcompress_push_delta( // Calculate each step time clock_offset += 0.5; - start_pos += movexy_r*closestxy_sd; + double start_pos = movexy_r*startxy_sd; height += .5 * step_dist; uint64_t *qn = sc->queue_next, *qend = sc->queue_end; if (!accel) { @@ -575,7 +576,7 @@ stepcompress_push_delta( int ret = check_expand(sc, &qn, &qend); if (ret) return ret; - double v = safe_sqrt(closest_height2 - height*height); + double v = safe_sqrt(arm_sd2 - height*height); double pos = start_pos + (step_dist > 0. ? -v : v); *qn++ = clock_offset + pos * inv_cruise_sv; height += step_dist; @@ -597,8 +598,8 @@ stepcompress_push_delta( int ret = check_expand(sc, &qn, &qend); if (ret) return ret; - double relheight = movexy_r*height - movez_r*closestxy_sd; - double v = safe_sqrt(closest_height2 - relheight*relheight); + double relheight = movexy_r*height - movez_r*startxy_sd; + double v = safe_sqrt(arm_sd2 - relheight*relheight); double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v); *qn++ = clock_offset + pos * inv_cruise_sv; height += step_dist; @@ -614,8 +615,8 @@ stepcompress_push_delta( int ret = check_expand(sc, &qn, &qend); if (ret) return ret; - double relheight = movexy_r*height - movez_r*closestxy_sd; - double v = safe_sqrt(closest_height2 - relheight*relheight); + double relheight = movexy_r*height - movez_r*startxy_sd; + double v = safe_sqrt(arm_sd2 - relheight*relheight); double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v); v = safe_sqrt(pos * accel_multiplier); *qn++ = clock_offset + (accel_multiplier >= 0. ? v : -v);