Make lookahead basically working.
This means, modify existing code to let the lookahead algorithms do their work. It also means to remove some unused code in dda_lookahead.c and reordering some code to make it work with LOOKAHEAD undefined.
This commit is contained in:
Markus Hitter
parent
e04b69b9c6
commit
1aca61c277
71
dda.c
71
dda.c
|
|
@ -10,6 +10,7 @@
|
|||
#include <avr/interrupt.h>
|
||||
|
||||
#include "dda_maths.h"
|
||||
#include "dda_lookahead.h"
|
||||
#include "timer.h"
|
||||
#include "serial.h"
|
||||
#include "sermsg.h"
|
||||
|
|
@ -82,9 +83,13 @@ void dda_new_startpoint(void) {
|
|||
|
||||
This algorithm is probably the main limiting factor to print speed in terms of firmware limitations
|
||||
*/
|
||||
void dda_create(DDA *dda, TARGET *target) {
|
||||
void dda_create(DDA *dda, TARGET *target, DDA *prev_dda) {
|
||||
uint32_t steps, x_delta_um, y_delta_um, z_delta_um, e_delta_um;
|
||||
uint32_t distance, c_limit, c_limit_calc;
|
||||
#ifdef LOOKAHEAD
|
||||
// Number the moves to identify them; allowed to overflow.
|
||||
static uint8_t idcnt = 0;
|
||||
#endif
|
||||
|
||||
// initialise DDA to a known state
|
||||
dda->allflags = 0;
|
||||
|
|
@ -95,6 +100,15 @@ void dda_create(DDA *dda, TARGET *target) {
|
|||
// we end at the passed target
|
||||
memcpy(&(dda->endpoint), target, sizeof(TARGET));
|
||||
|
||||
#ifdef LOOKAHEAD
|
||||
// Set the start and stop speeds to zero for now = full stops between
|
||||
// moves. Also fallback if lookahead calculations fail to finish in time.
|
||||
dda->F_start = 0;
|
||||
dda->F_end = 0;
|
||||
// Give this move an identifier.
|
||||
dda->id = idcnt++;
|
||||
#endif
|
||||
|
||||
// TODO TODO: We should really make up a loop for all axes.
|
||||
// Think of what happens when a sixth axis (multi colour extruder)
|
||||
// appears?
|
||||
|
|
@ -129,6 +143,14 @@ void dda_create(DDA *dda, TARGET *target) {
|
|||
dda->e_direction = (target->E >= startpoint.E)?1:0;
|
||||
}
|
||||
|
||||
#ifdef LOOKAHEAD
|
||||
// Also displacements in micrometers, but for the lookahead alogrithms.
|
||||
dda->delta.X = target->X - startpoint.X;
|
||||
dda->delta.Y = target->Y - startpoint.Y;
|
||||
dda->delta.Z = target->Z - startpoint.Z;
|
||||
dda->delta.E = target->e_relative ? target->E : target->E - startpoint.E;
|
||||
#endif
|
||||
|
||||
if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
|
||||
sersendf_P(PSTR("%ld,%ld,%ld,%ld] ["), target->X - startpoint.X, target->Y - startpoint.Y, target->Z - startpoint.Z, target->E - startpoint.E);
|
||||
|
||||
|
|
@ -285,13 +307,43 @@ void dda_create(DDA *dda, TARGET *target) {
|
|||
dda->c_min = c_limit;
|
||||
// This section is plain wrong, like in it's only half of what we need. This factor 960000 is dependant on STEPS_PER_MM.
|
||||
// overflows at target->F > 65535; factor 16. found by try-and-error; will overshoot target speed a bit
|
||||
dda->rampup_steps = target->F * target->F / (uint32_t)(STEPS_PER_M_X * ACCELERATION / 960000.);
|
||||
//dda->rampup_steps = target->F * target->F / (uint32_t)(STEPS_PER_M_X * ACCELERATION / 960000.);
|
||||
//sersendf_P(PSTR("rampup calc %lu\n"), dda->rampup_steps);
|
||||
dda->rampup_steps = 100000; // replace mis-calculation by a safe value
|
||||
//dda->rampup_steps = 100000; // replace mis-calculation by a safe value
|
||||
// End of wrong section.
|
||||
if (dda->rampup_steps > dda->total_steps / 2)
|
||||
dda->rampup_steps = dda->total_steps / 2;
|
||||
dda->rampdown_steps = dda->total_steps - dda->rampup_steps;
|
||||
/**
|
||||
Assuming: F is in mm/min, STEPS_PER_M_X is in steps/m, ACCELERATION is in mm/s²
|
||||
Given:
|
||||
- Velocity v at time t given acceleration a: v(t) = a*t
|
||||
- Displacement s at time t given acceleration a: s(t) = 1/2 * a * t²
|
||||
- Displacement until reaching target velocity v: s = 1/2 * (v² / a)
|
||||
- Final result: steps needed to reach velocity v given acceleration a:
|
||||
steps = (STEPS_PER_M_X * F^2) / (7200000 * ACCELERATION)
|
||||
To keep precision, break up in floating point and integer part:
|
||||
F^2 * (int)(STEPS_PER_M_X / (7200000 * ACCELERATION))
|
||||
Note: the floating point part is static so its calculated during compilation.
|
||||
Note 2: the floating point part will be smaller than one, invert it:
|
||||
steps = F^2 / (int)((7200000 * ACCELERATION) / STEPS_PER_M_X)
|
||||
Note 3: As mentioned, setting F to 65535 or larger will overflow the
|
||||
calculation. Make sure this does not happen.
|
||||
Note 4: Anyone trying to run their machine at 65535 mm/min > 1m/s is nuts
|
||||
*/
|
||||
if (target->F > 65534) target->F = 65534;
|
||||
dda->rampup_steps = ACCELERATE_RAMP_LEN(target->F);
|
||||
// Quick hack: we do not do Z move joins as jerk on the Z axis is undesirable;
|
||||
// as the ramp length is calculated for XY, its incorrect for Z: apply the original
|
||||
// 'fix' to simply specify a large enough ramp for any speed.
|
||||
if (x_delta_um == 0 && y_delta_um == 0) {
|
||||
dda->rampup_steps = 100000; // replace mis-calculation by a safe value
|
||||
}
|
||||
|
||||
if (dda->rampup_steps > dda->total_steps / 2)
|
||||
dda->rampup_steps = dda->total_steps / 2;
|
||||
dda->rampdown_steps = dda->total_steps - dda->rampup_steps;
|
||||
|
||||
#ifdef LOOKAHEAD
|
||||
dda_join_moves(prev_dda, dda);
|
||||
#endif
|
||||
#elif defined ACCELERATION_TEMPORAL
|
||||
// TODO: limit speed of individual axes to MAXIMUM_FEEDRATE
|
||||
// TODO: calculate acceleration/deceleration for each axis
|
||||
|
|
@ -617,6 +669,8 @@ void dda_step(DDA *dda) {
|
|||
move_state.n += 4;
|
||||
// be careful of signedness!
|
||||
move_state.c = (int32_t)move_state.c - ((int32_t)(move_state.c * 2) / (int32_t)move_state.n);
|
||||
//sersendf_P(PSTR("n:%ld; c:%ld; steps: %ld / %lu\n"), move_state.n,
|
||||
// move_state.c, move_state.step_no, move_state.y_steps);
|
||||
}
|
||||
move_state.step_no++;
|
||||
// Print the number of steps actually needed for ramping up
|
||||
|
|
@ -687,6 +741,11 @@ void dda_step(DDA *dda) {
|
|||
if (move_state.x_steps == 0 && move_state.y_steps == 0 &&
|
||||
move_state.z_steps == 0 && move_state.e_steps == 0) {
|
||||
dda->live = 0;
|
||||
#ifdef LOOKAHEAD
|
||||
// If look-ahead was using this move, it could have missed our activation:
|
||||
// make sure the ids do not match.
|
||||
dda->id--;
|
||||
#endif
|
||||
#ifdef DC_EXTRUDER
|
||||
heater_set(DC_EXTRUDER, 0);
|
||||
#endif
|
||||
|
|
|
|||
33
dda.h
33
dda.h
|
|
@ -15,6 +15,9 @@
|
|||
types
|
||||
*/
|
||||
|
||||
// Enum to denote an axis
|
||||
enum axis_e { X, Y, Z, E };
|
||||
|
||||
/**
|
||||
\struct TARGET
|
||||
\brief target is simply a point in space/time
|
||||
|
|
@ -34,6 +37,19 @@ typedef struct {
|
|||
uint8_t e_relative :1; ///< bool: e axis relative? Overrides all_relative
|
||||
} TARGET;
|
||||
|
||||
/**
|
||||
\struct VECTOR4D
|
||||
\brief 4 dimensional vector used to describe the difference between moves.
|
||||
|
||||
Units are in micrometers and usually based off 'TARGET'.
|
||||
*/
|
||||
typedef struct {
|
||||
int32_t X;
|
||||
int32_t Y;
|
||||
int32_t Z;
|
||||
int32_t E;
|
||||
} VECTOR4D;
|
||||
|
||||
/**
|
||||
\struct MOVE_STATE
|
||||
\brief this struct is made for tracking the current state of the movement
|
||||
|
|
@ -128,6 +144,21 @@ typedef struct {
|
|||
uint32_t rampdown_steps;
|
||||
/// 24.8 fixed point timer value, maximum speed
|
||||
uint32_t c_min;
|
||||
#ifdef LOOKAHEAD
|
||||
// With the look-ahead functionality, it is possible to retain physical
|
||||
// movement between G1 moves. These variables keep track of the entry and
|
||||
// exit speeds between moves.
|
||||
uint32_t F_start;
|
||||
uint32_t F_end;
|
||||
// Displacement vector, in um, based between the difference of the starting
|
||||
// point and the target. Required to obtain the jerk between 2 moves.
|
||||
// Note: x_delta and co are in steps, not um.
|
||||
VECTOR4D delta;
|
||||
// Number the moves to be able to test at the end of lookahead if the moves
|
||||
// are the same. Note: we do not need a lot of granularity here: more than
|
||||
// MOVEBUFFER_SIZE is already enough.
|
||||
uint8_t id;
|
||||
#endif
|
||||
#endif
|
||||
#ifdef ACCELERATION_TEMPORAL
|
||||
uint32_t x_step_interval; ///< time between steps on X axis
|
||||
|
|
@ -166,7 +197,7 @@ void dda_init(void);
|
|||
void dda_new_startpoint(void);
|
||||
|
||||
// create a DDA
|
||||
void dda_create(DDA *dda, TARGET *target);
|
||||
void dda_create(DDA *dda, TARGET *target, DDA *prev_dda);
|
||||
|
||||
// start a created DDA (called from timer interrupt)
|
||||
void dda_start(DDA *dda) __attribute__ ((hot));
|
||||
|
|
|
|||
|
|
@ -25,7 +25,6 @@
|
|||
#include "debug.h"
|
||||
#include "sersendf.h"
|
||||
#include "pinio.h"
|
||||
#include "config.h"
|
||||
|
||||
extern uint8_t use_lookahead;
|
||||
|
||||
|
|
@ -144,7 +143,7 @@ void dda_emergency_shutdown(PGM_P msg) {
|
|||
serial_writestr_P(PSTR("error: emergency stop:"));
|
||||
if(msg!=NULL) serial_writestr_P(msg);
|
||||
serial_writestr_P(PSTR("\r\n"));
|
||||
delay(20000); // Delay so the buffer can be flushed - otherwise the message is never sent
|
||||
delay_ms(20); // Delay so the buffer can be flushed - otherwise the message is never sent
|
||||
timer_stop();
|
||||
queue_flush();
|
||||
power_off();
|
||||
|
|
@ -168,8 +167,6 @@ void dda_emergency_shutdown(PGM_P msg) {
|
|||
* last move (= 'current'); as a result a lot of small moves will still limit the speed.
|
||||
*/
|
||||
void dda_join_moves(DDA *prev, DDA *current) {
|
||||
// Run-time option: only proceed if we are enabled.
|
||||
if(use_lookahead==0) return;
|
||||
|
||||
// Calculating the look-ahead settings can take a while; before modifying
|
||||
// the previous move, we need to locally store any values and write them
|
||||
|
|
@ -181,7 +178,6 @@ void dda_join_moves(DDA *prev, DDA *current) {
|
|||
// 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_rampup, this_rampdown;
|
||||
enum axis_e prev_lead;
|
||||
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
|
||||
|
|
@ -224,7 +220,6 @@ void dda_join_moves(DDA *prev, DDA *current) {
|
|||
prev_rampup = prev->rampup_steps;
|
||||
prev_rampdown = prev->rampdown_steps;
|
||||
prev_total_steps = prev->total_steps;
|
||||
prev_lead = prev->lead;
|
||||
}
|
||||
|
||||
// The initial crossing speed is the minimum between both target speeds
|
||||
|
|
|
|||
|
|
@ -27,8 +27,6 @@
|
|||
#error "Look-ahead requires steps per m to be identical on the X and Y axis (for now)"
|
||||
#endif
|
||||
|
||||
// Note: the floating point bit is optimized away during compilation
|
||||
#define ACCELERATE_RAMP_LEN(speed) (((speed)*(speed)) / (uint32_t)((7200000.0f * ACCELERATION) / (float)STEPS_PER_M_X))
|
||||
// This is the same to ACCELERATE_RAMP_LEN but now the steps per m can be switched.
|
||||
// Note: use this with a macro so the float is removed by the preprocessor
|
||||
#define ACCELERATE_RAMP_SCALER(spm) (uint32_t)((7200000.0f * ACCELERATION) / (float)spm)
|
||||
|
|
|
|||
|
|
@ -63,4 +63,7 @@ uint16_t int_sqrt(uint32_t a);
|
|||
// 2 ^ msbloc(v) >= v
|
||||
const uint8_t msbloc (uint32_t v);
|
||||
|
||||
// Note: the floating point bit is optimized away during compilation
|
||||
#define ACCELERATE_RAMP_LEN(speed) (((speed)*(speed)) / (uint32_t)((7200000.0f * ACCELERATION) / (float)STEPS_PER_M_X))
|
||||
|
||||
#endif /* _DDA_MATHS_H */
|
||||
|
|
|
|||
|
|
@ -100,9 +100,10 @@ void enqueue_home(TARGET *t, uint8_t endstop_check, uint8_t endstop_stop_cond) {
|
|||
h &= (MOVEBUFFER_SIZE - 1);
|
||||
|
||||
DDA* new_movebuffer = &(movebuffer[h]);
|
||||
|
||||
if (t != NULL) {
|
||||
dda_create(new_movebuffer, t);
|
||||
DDA* prev_movebuffer = (queue_empty() != 0) ? NULL : &movebuffer[mb_head];
|
||||
|
||||
if (t != NULL) {
|
||||
dda_create(new_movebuffer, t, prev_movebuffer);
|
||||
new_movebuffer->endstop_check = endstop_check;
|
||||
new_movebuffer->endstop_stop_cond = endstop_stop_cond;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue