diff options
| -rw-r--r-- | klippy/chelper.py | 16 | ||||
| -rw-r--r-- | klippy/delta.py | 133 | ||||
| -rw-r--r-- | klippy/mcu.py | 20 | ||||
| -rw-r--r-- | klippy/stepcompress.c | 40 |
4 files changed, 83 insertions, 126 deletions
diff --git a/klippy/chelper.py b/klippy/chelper.py index 58049582..4384fe28 100644 --- a/klippy/chelper.py +++ b/klippy/chelper.py @@ -24,14 +24,14 @@ defs_stepcompress = """ int32_t stepcompress_push_sqrt(struct stepcompress *sc , double steps, double step_offset , double clock_offset, double sqrt_offset, double factor); - int32_t stepcompress_push_delta_const( - struct stepcompress *sc, double clock_offset, double dist - , double step_dist, double start_pos, double closest_height2 - , double height, double movez_r, double inv_velocity); - int32_t stepcompress_push_delta_accel( - struct stepcompress *sc, double clock_offset, double dist - , double step_dist, double start_pos, double closest_height2 - , double height, double movez_r, double accel_multiplier); + int32_t stepcompress_push_delta_const(struct stepcompress *sc + , double clock_offset, double dist, double start_pos + , double inv_velocity, double step_dist, double height + , double closestxy_d, double closest_height2, double movez_r); + int32_t stepcompress_push_delta_accel(struct stepcompress *sc + , double clock_offset, double dist, double start_pos + , double accel_multiplier, double step_dist, double height + , double closestxy_d, double closest_height2, double movez_r); void stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock); void stepcompress_queue_msg(struct stepcompress *sc , uint32_t *data, int len); diff --git a/klippy/delta.py b/klippy/delta.py index 19984496..7d7b063f 100644 --- a/klippy/delta.py +++ b/klippy/delta.py @@ -125,72 +125,25 @@ class DeltaKinematics: raise homing.EndstopMoveError(end_pos) if move.axes_d[2]: move.limit_speed(self.max_z_velocity, 9999999.9) - def move_z(self, move_time, move): - if not move.axes_d[2]: - return - if self.need_motor_enable: - self.check_motor_enable(move_time) - inv_accel = 1. / move.accel - inv_cruise_v = 1. / move.cruise_v - for i in StepList: - towerx_d = self.towers[i][0] - move.start_pos[0] - towery_d = self.towers[i][1] - move.start_pos[1] - tower_d2 = towerx_d**2 + towery_d**2 - height = math.sqrt(self.arm_length2 - tower_d2) + move.start_pos[2] - - 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 - height * inv_step_dist - step_dist = self.steppers[i].step_dist - steps = move.axes_d[2] * inv_step_dist - - # Acceleration steps - accel_multiplier = 2.0 * step_dist * 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 = step_dist * 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) def move(self, move_time, move): 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]: - self.move_z(move_time, move) - return + if not axes_d[2]: + return + movez_r = axes_d[2] * inv_movexy_d + movexy_d = movexy_r = inv_movexy_d = 0. + elif axes_d[2]: + 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) - move_d = move.move_d - movez_r = 0. - inv_movexy_d = 1. / move_d - inv_movexy_r = 1. - if axes_d[2]: - movez_r = axes_d[2] * inv_movexy_d - inv_movexy_d = 1. / math.sqrt(axes_d[0]**2 + axes_d[1]**2) - inv_movexy_r = move_d * inv_movexy_d origx, origy, origz = move.start_pos[:3] @@ -214,80 +167,76 @@ class DeltaKinematics: 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 - closest_height = math.sqrt(closest_height2) - closest_d = closestxy_d * inv_movexy_r - closestz = origz + closest_d*movez_r # Calculate accel/cruise/decel portions of move - reverse_d = closest_d + closest_height*movez_r*inv_movexy_r + reversexy_d = closestxy_d + math.sqrt(closest_height2)*movez_r accel_up_d = cruise_up_d = decel_up_d = 0. accel_down_d = cruise_down_d = decel_down_d = 0. - if reverse_d <= 0.: + if reversexy_d <= 0.: accel_down_d = accel_d cruise_down_d = cruise_end_d decel_down_d = move_d - elif reverse_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 reverse_d < accel_d: - accel_up_d = reverse_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 reverse_d < cruise_end_d: + elif reversexy_d < cruise_end_d * movexy_r: accel_up_d = accel_d - cruise_up_d = reverse_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 = reverse_d + decel_up_d = reversexy_d * move_d * inv_movexy_d decel_down_d = move_d # Generate steps 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_dist = self.steppers[i].step_dist - height = step_pos*step_dist - closestz + height = step_pos*step_dist - origz if accel_up_d > 0.: count = mcu_stepper.step_delta_accel( - mcu_time - accel_time_offset, closest_d - accel_up_d, - step_dist, closest_d + accel_offset, - closest_height2, height, movez_r, accel_multiplier) + mcu_time - accel_time_offset, accel_up_d, + accel_offset, accel_multiplier, step_dist, + height, closestxy_d, closest_height2, movez_r) height += count * step_dist if cruise_up_d > 0.: count = mcu_stepper.step_delta_const( - mcu_time + accel_t, closest_d - cruise_up_d, - step_dist, closest_d - accel_d, - closest_height2, height, movez_r, inv_cruise_v) + mcu_time + accel_t, cruise_up_d, + -accel_d, inv_cruise_v, step_dist, + height, closestxy_d, closest_height2, movez_r) height += count * step_dist if decel_up_d > 0.: count = mcu_stepper.step_delta_accel( - mcu_time + decel_time_offset, closest_d - decel_up_d, - step_dist, closest_d - decel_offset, - closest_height2, height, movez_r, -accel_multiplier) + mcu_time + decel_time_offset, decel_up_d, + -decel_offset, -accel_multiplier, step_dist, + height, closestxy_d, closest_height2, movez_r) height += count * step_dist if accel_down_d > 0.: count = mcu_stepper.step_delta_accel( - mcu_time - accel_time_offset, closest_d - accel_down_d, - -step_dist, closest_d + accel_offset, - closest_height2, height, movez_r, accel_multiplier) + mcu_time - accel_time_offset, accel_down_d, + accel_offset, accel_multiplier, -step_dist, + height, closestxy_d, closest_height2, movez_r) height += count * step_dist if cruise_down_d > 0.: count = mcu_stepper.step_delta_const( - mcu_time + accel_t, closest_d - cruise_down_d, - -step_dist, closest_d - accel_d, - closest_height2, height, movez_r, inv_cruise_v) + mcu_time + accel_t, cruise_down_d, + -accel_d, inv_cruise_v, -step_dist, + height, closestxy_d, closest_height2, movez_r) height += count * step_dist if decel_down_d > 0.: count = mcu_stepper.step_delta_accel( - mcu_time + decel_time_offset, closest_d - decel_down_d, - -step_dist, closest_d - decel_offset, - closest_height2, height, movez_r, -accel_multiplier) + mcu_time + decel_time_offset, decel_down_d, + -decel_offset, -accel_multiplier, -step_dist, + height, closestxy_d, closest_height2, movez_r) ###################################################################### diff --git a/klippy/mcu.py b/klippy/mcu.py index c1abb959..4c2bbc68 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -86,21 +86,25 @@ class MCU_stepper: self._stepqueue, steps, step_offset, clock, factor * self._mcu_freq) self.commanded_position += count return count - def step_delta_const(self, mcu_time, dist, step_dist, start_pos - , closest_height2, height, movez_r, inv_velocity): + def step_delta_const(self, mcu_time, dist, start_pos + , inv_velocity, step_dist + , height, closestxy_d, closest_height2, movez_r): clock = mcu_time * self._mcu_freq count = self.ffi_lib.stepcompress_push_delta_const( - self._stepqueue, clock, dist, step_dist, start_pos - , closest_height2, height, movez_r, inv_velocity * self._mcu_freq) + self._stepqueue, clock, dist, start_pos + , inv_velocity * self._mcu_freq, step_dist + , height, closestxy_d, closest_height2, movez_r) self.commanded_position += count return count - def step_delta_accel(self, mcu_time, dist, step_dist, start_pos - , closest_height2, height, movez_r, accel_multiplier): + def step_delta_accel(self, mcu_time, dist, start_pos + , accel_multiplier, step_dist + , height, closestxy_d, closest_height2, movez_r): clock = mcu_time * self._mcu_freq mcu_freq2 = self._mcu_freq**2 count = self.ffi_lib.stepcompress_push_delta_accel( - self._stepqueue, clock, dist, step_dist, start_pos - , closest_height2, height, movez_r, accel_multiplier * mcu_freq2) + self._stepqueue, clock, dist, start_pos + , accel_multiplier * mcu_freq2, step_dist + , height, closestxy_d, closest_height2, movez_r) self.commanded_position += count return count def get_errors(self): diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c index 4f41cfee..0af0672e 100644 --- a/klippy/stepcompress.c +++ b/klippy/stepcompress.c @@ -442,14 +442,15 @@ stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset // Schedule 'count' number of steps using the delta kinematic const speed int32_t stepcompress_push_delta_const( - struct stepcompress *sc, double clock_offset, double dist, double step_dist - , double start_pos, double closest_height2, double height, double movez_r - , double inv_velocity) + struct stepcompress *sc, double clock_offset, double dist, double start_pos + , double inv_velocity, double step_dist + , double height, double closestxy_d, double closest_height2, double movez_r) { // Calculate number of steps to take - double zdist = dist * movez_r; - int count = (safe_sqrt(closest_height2 - dist*dist + zdist*zdist) - - height - zdist) / step_dist + .5; + double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.; + double reldist = closestxy_d - movexy_r*dist; + double end_height = safe_sqrt(closest_height2 - reldist*reldist); + int count = (end_height - height + movez_r*dist) / step_dist + .5; if (count <= 0 || count > 1000000) { if (count) errorf("push_delta_const invalid count %d %d %f %f %f %f %f %f %f %f" @@ -462,11 +463,12 @@ stepcompress_push_delta_const( // Calculate each step time uint64_t *qn = sc->queue_next, *end = &qn[count]; clock_offset += 0.5; + start_pos += movexy_r*closestxy_d; height += .5 * step_dist; while (qn < end) { - double zh = height*movez_r; - double v = safe_sqrt(closest_height2 - height*height + zh*zh); - double pos = start_pos + zh + (step_dist > 0. ? -v : v); + double relheight = movexy_r*height - movez_r*closestxy_d; + double v = safe_sqrt(closest_height2 - relheight*relheight); + double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v); *qn++ = clock_offset + pos * inv_velocity; height += step_dist; } @@ -477,14 +479,15 @@ stepcompress_push_delta_const( // Schedule 'count' number of steps using delta kinematic acceleration int32_t stepcompress_push_delta_accel( - struct stepcompress *sc, double clock_offset, double dist, double step_dist - , double start_pos, double closest_height2, double height, double movez_r - , double accel_multiplier) + struct stepcompress *sc, double clock_offset, double dist, double start_pos + , double accel_multiplier, double step_dist + , double height, double closestxy_d, double closest_height2, double movez_r) { // Calculate number of steps to take - double zdist = dist * movez_r; - int count = (safe_sqrt(closest_height2 - dist*dist + zdist*zdist) - - height - zdist) / step_dist + .5; + double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.; + double reldist = closestxy_d - movexy_r*dist; + double end_height = safe_sqrt(closest_height2 - reldist*reldist); + int count = (end_height - height + movez_r*dist) / step_dist + .5; if (count <= 0 || count > 1000000) { if (count) errorf("push_delta_accel invalid count %d %d %f %f %f %f %f %f %f %f" @@ -497,11 +500,12 @@ stepcompress_push_delta_accel( // Calculate each step time uint64_t *qn = sc->queue_next, *end = &qn[count]; clock_offset += 0.5; + start_pos += movexy_r*closestxy_d; height += .5 * step_dist; while (qn < end) { - double zh = height*movez_r; - double v = safe_sqrt(closest_height2 - height*height + zh*zh); - double pos = start_pos + zh + (step_dist > 0. ? -v : v); + double relheight = movexy_r*height - movez_r*closestxy_d; + double v = safe_sqrt(closest_height2 - 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); height += step_dist; |