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 <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor
parent
b915a2ad7d
commit
33b809714f
|
|
@ -1,6 +1,6 @@
|
|||
# Code for handling the kinematics of linear delta robots
|
||||
#
|
||||
# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net>
|
||||
# Copyright (C) 2016,2017 Kevin O'Connor <kevin@koconnor.net>
|
||||
#
|
||||
# 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)
|
||||
|
||||
|
||||
######################################################################
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
// Stepper pulse schedule compression
|
||||
//
|
||||
// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net>
|
||||
// Copyright (C) 2016,2017 Kevin O'Connor <kevin@koconnor.net>
|
||||
//
|
||||
// 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
|
||||
|
|
|
|||
Loading…
Reference in New Issue