josh
/
Teacup_Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

dda_lookahead.c: move crossing speed calculation into a dedicated function. 

This is mostly a preparation for reverse walks through the movement queue,
where crossing speed calculation is done only once, while actually used
speeds can be raised successively with repeated walks.
This commit is contained in:
Markus Hitter 2013-10-29 14:10:32 +01:00
parent 8ddc633531
commit 1b5b40e61d
2 changed files with 134 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

224
dda_lookahead.c
View File

@ -151,16 +151,134 @@ void dda_emergency_shutdown(PGM_P msg) {
}
/**
* Join 2 moves by removing the full stop between them, where possible.
* To join the moves, the expected jerk - or force - of the change in direction is calculated.
* The jerk is used to scale the common feed rate between both moves to obtain an acceptable speed
* to transition between 'prev' and 'current'.
* \brief Find maximum corner speed between two moves.
* \details Find out how fast we can move around around a corner without
* exceeding the expected jerk. Worst case this speed is zero, which means a
* full stop between both moves. Best case it's the lower of the maximum speeds.
*
* Premise: we currently join the last move in the queue and the one before it (if any).
* This means the feed rate at the end of the 'current' move is 0.
* This function is expected to be called from within dda_start().
*
* Premise: the 'current' move is not dispatched in the queue: it should remain constant while this
* function is running.
* \param [in] prev is the DDA structure of the move previous to the current one.
* \param [in] current is the DDA structure of the move currently created.
*
* \return dda->crossF
*/
void dda_find_crossing_speed(DDA *prev, DDA *current) {
int32_t jerk, jerk_e;
uint32_t crossF;
// Bail out if there's nothing to join (e.g. G1 F1500).
if ( ! prev || prev->nullmove) {
current->crossF = 0;
return;
}
// Only prepare for look-ahead if we have 2 available moves to
// join and the Z axis is unused (for now, Z axis moves are NOT joined).
if (prev->live == 0 && prev->delta_um.Z == current->delta_um.Z) {
// Calculate the jerk if the previous move and this move would be joined
// together at full speed.
jerk = dda_jerk_size_2d(prev->delta_um.X, prev->delta_um.Y, prev->endpoint.F,
current->delta_um.X, current->delta_um.Y, current->endpoint.F);
jerk_e = dda_jerk_size_1d(prev->delta_um.E, prev->endpoint.F,
current->delta_um.E, current->endpoint.F);
if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
sersendf_P(PSTR("Jerk %lu Jerk_e: %lu\n"), jerk, jerk_e);
} else {
// Move already executing or Z moved: abort the join
current->crossF = 0;
return;
}
// The initial crossing speed is the minimum between both target speeds
// Note: this is a given: the start speed and end speed can NEVER be
// higher than the target speed in a move!
// Note 2: this provides an upper limit, if needed, the speed is lowered.
crossF = prev->endpoint.F;
if (crossF > current->endpoint.F)
crossF = current->endpoint.F;
// If the XY jerk is too big, scale the proposed cross speed.
if (jerk > LOOKAHEAD_MAX_JERK_XY) {
if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
sersendf_P(PSTR("XY Jerk too big.\n"));
// Perform an exponential scaling.
// Use unsigned to double the range before overflowing.
crossF = (crossF * LOOKAHEAD_MAX_JERK_XY * LOOKAHEAD_MAX_JERK_XY) /
((uint32_t)jerk * (uint32_t)jerk);
// Optimize: if the crossing speed is zero, there is no join possible.
if (crossF == 0) {
current->crossF = 0;
return;
}
// Safety: make sure we never exceed the maximum speed of a move
if (crossF > current->endpoint.F)
crossF = current->endpoint.F;
if (crossF > prev->endpoint.F)
crossF = prev->endpoint.F;
}
// Same to the extruder jerk: make sure we do not yank it.
if (jerk_e > LOOKAHEAD_MAX_JERK_E) {
uint32_t crossF2;
if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
sersendf_P(PSTR("E Jerk too big.\n"));
crossF2 = MAX(current->endpoint.F, prev->endpoint.F);
// Perform an exponential scaling.
// Use unsigned to double the range before overflowing.
crossF2 = (crossF2 * LOOKAHEAD_MAX_JERK_E * LOOKAHEAD_MAX_JERK_E) /
((uint32_t)jerk * (uint32_t)jerk);
// Only continue with joining if there is a feasible crossing speed.
if (crossF2 == 0) {
current->crossF = 0;
return;
}
// Safety: make sure the proposed speed is not higher than the target speeds of each move
crossF2 = MIN(crossF2, current->endpoint.F);
crossF2 = MIN(crossF2, prev->endpoint.F);
if (crossF2 > crossF) {
sersendf_P(PSTR("Jerk_e: %lu => crossF: %lu (original: %lu)\r\n"),
jerk_e, crossF2, crossF);
}
// Pick the crossing speed for these 2 move to be within the jerk limits
crossF = MIN(crossF, crossF2);
}
current->crossF = crossF;
}
/**
* \brief Join 2 moves by removing the full stop between them, where possible.
* \details To join the moves, the deceleration ramp of the previous move and
* the acceleration ramp of the current move are shortened, resulting in a
* non-zero speed at that point. The target speed at the corner is already to
* be found in dda->crossF. See dda_find_corner_speed().
*
* Ideally, both ramps can be reduced to actually have Fcorner at the corner,
* but the surrounding movements might no be long enough to achieve this speed.
* Analysing both moves to find the best result is done here.
*
* TODO: to achieve better results with short moves (move distance < both ramps),
* this function should be able to enhance the corner speed on repeated
* calls when reverse-stepping through the movement queue.
*
* \param [in] prev is the DDA structure of the move previous to the current one.
* \param [in] current is the DDA structure of the move currently created.
*
* Premise: the 'current' move is not dispatched in the queue: it should remain
* constant while this function is running.
*
* Note: the planner always makes sure the movement can be stopped within the
* last move (= 'current'); as a result a lot of small moves will still limit the speed.
@ -172,48 +290,25 @@ void dda_join_moves(DDA *prev, DDA *current) {
// when we are done (and the previous move is not already active).
uint32_t prev_F, prev_F_start, prev_F_end, prev_end;
uint32_t prev_rampup, prev_rampdown, prev_total_steps;
uint32_t crossF;
uint8_t prev_id;
// Similarly, we only want to modify the current move if we have the results of the calculations;
// until then, we do not want to touch the current move settings.
// Note: we assume 'current' will not be dispatched while this function runs, so we do not to
// back up the move settings: they will remain constant.
uint32_t this_F_start, this_start, this_rampup, this_rampdown;
int32_t jerk, jerk_e; // Expresses the forces if we would change directions at full speed
static uint32_t la_cnt = 0; // Counter: how many moves did we join?
#ifdef LOOKAHEAD_DEBUG
static uint32_t moveno = 0; // Debug counter to number the moves - helps while debugging
moveno++;
#endif
dda_find_crossing_speed(prev, current);
// Bail out if there's nothing to join (e.g. G1 F1500).
if ( ! prev || prev->nullmove)
if ( ! prev || prev->nullmove || current->crossF == 0)
return;
serprintf(PSTR("Current Delta: %ld,%ld,%ld E:%ld Live:%d\r\n"),
current->delta_um.X, current->delta_um.Y, current->delta_um.Z,
current->delta_um.E, current->live);
serprintf(PSTR("Prev Delta: %ld,%ld,%ld E:%ld Live:%d\r\n"),
prev->delta_um.X, prev->delta_um.Y, prev->delta_um.Z,
prev->delta_um.E, prev->live);
// Look-ahead: attempt to join moves into smooth movements
// Note: moves are only modified after the calculations are complete.
// Only prepare for look-ahead if we have 2 available moves to
// join and the Z axis is unused (for now, Z axis moves are NOT joined).
if (prev->live == 0 && prev->delta_um.Z == current->delta_um.Z) {
// Calculate the jerk if the previous move and this move would be joined
// together at full speed.
jerk = dda_jerk_size_2d(prev->delta_um.X, prev->delta_um.Y, prev->endpoint.F,
current->delta_um.X, current->delta_um.Y, current->endpoint.F);
serprintf(PSTR("Jerk: %lu\r\n"), jerk);
jerk_e = dda_jerk_size_1d(prev->delta_um.E, prev->endpoint.F,
current->delta_um.E, current->endpoint.F);
serprintf(PSTR("Jerk_e: %lu\r\n"), jerk_e);
} else {
// Move already executing or Z moved: abort the join
return;
}
// Make sure we have 2 moves and the previous move is not already active
if (prev->live == 0) {
// Perform an atomic copy to preserve volatile parameters during the calculations
@ -225,65 +320,12 @@ void dda_join_moves(DDA *prev, DDA *current) {
prev_rampup = prev->rampup_steps;
prev_rampdown = prev->rampdown_steps;
prev_total_steps = prev->total_steps;
crossF = current->crossF;
ATOMIC_END
// The initial crossing speed is the minimum between both target speeds
// Note: this is a given: the start speed and end speed can NEVER be
// higher than the target speed in a move!
// Note 2: this provides an upper limit, if needed, the speed is lowered.
uint32_t crossF = prev_F;
if(crossF > current->endpoint.F) crossF = current->endpoint.F;
//sersendf_P(PSTR("j:%lu - XF:%lu"), jerk, crossF);
// If the XY jerk is too big, scale the proposed cross speed
if(jerk > LOOKAHEAD_MAX_JERK_XY) {
serprintf(PSTR("Jerk too big: scale cross speed between moves\r\n"));
// Get the highest speed between both moves
if(crossF < prev_F)
crossF = prev_F;
// Perform an exponential scaling
uint32_t ujerk = (uint32_t)jerk; // Use unsigned to double the range before overflowing
crossF = (crossF*LOOKAHEAD_MAX_JERK_XY*LOOKAHEAD_MAX_JERK_XY)/(ujerk*ujerk);
// Optimize: if the crossing speed is zero, there is no join possible between these
// two (fast) moves. Stop calculating and leave the full stop that is currently between
// them.
if(crossF == 0)
return;
// Safety: make sure we never exceed the maximum speed of a move
if(crossF > current->endpoint.F) crossF = current->endpoint.F;
if(crossF > prev_F) crossF = prev_F;
sersendf_P(PSTR("=>F:%lu"), crossF);
}
// Same to the extruder jerk: make sure we do not yank it
if(jerk_e > LOOKAHEAD_MAX_JERK_E) {
sersendf_P(PSTR("Jerk_e too big: scale cross speed between moves\r\n"));
uint32_t crossF2 = MAX(current->endpoint.F, prev_F);
// Perform an exponential scaling
uint32_t ujerk = (uint32_t)jerk_e; // Use unsigned to double the range before overflowing
crossF2 = (crossF2*LOOKAHEAD_MAX_JERK_E*LOOKAHEAD_MAX_JERK_E)/(ujerk*ujerk);
// Only continue with joining if there is a feasible crossing speed
if(crossF2 == 0) return;
// Safety: make sure the proposed speed is not higher than the target speeds of each move
crossF2 = MIN(crossF2, current->endpoint.F);
crossF2 = MIN(crossF2, prev_F);
if(crossF2 > crossF) {
sersendf_P(PSTR("Jerk_e: %lu => crossF: %lu (original: %lu)\r\n"), jerk_e, crossF2, crossF);
}
// Pick the crossing speed for these 2 move to be within the jerk limits
crossF = MIN(crossF, crossF2);
}
// Show the proposed crossing speed - this might get adjusted below
serprintf(PSTR("Initial crossing speed: %lu\r\n"), crossF);
if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
sersendf_P(PSTR("Initial crossing speed: %lu\n"), crossF);
// Forward check: test if we can actually reach the target speed in the previous move
// If not: we need to determine the obtainable speed and adjust crossF accordingly.

16
dda_lookahead.h
View File

@ -44,21 +44,7 @@
#define MAX(a,b) (((a)>(b))?(a):(b))
#define MIN(a,b) (((a)<(b))?(a):(b))
/**
* Join 2 moves by removing the full stop between them, where possible.
* To join the moves, the expected jerk - or force - of the change in direction is calculated.
* The jerk is used to scale the common feed rate between both moves to obtain an acceptable speed
* to transition between 'prev' and 'current'.
*
* Premise: we currently join the last move in the queue and the one before it (if any).
* This means the feed rate at the end of the 'current' move is 0.
*
* Premise: the 'current' move is not dispatched in the queue: it should remain constant while this
* function is running.
*
* Note: the planner always makes sure the movement can be stopped within the
* last move (= 'current'); as a result a lot of small moves will still limit the speed.
*/
void dda_find_crossing_speed(DDA *prev, DDA *current);
void dda_join_moves(DDA *prev, DDA *current);
// Debug counters