diff --git a/dda.c b/dda.c index 07e6471..4f223da 100644 --- a/dda.c +++ b/dda.c @@ -10,6 +10,7 @@ #include #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 diff --git a/dda.h b/dda.h index 37cc69e..3c1cbe3 100644 --- a/dda.h +++ b/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)); diff --git a/dda_lookahead.c b/dda_lookahead.c index 0db7adc..f05bce1 100644 --- a/dda_lookahead.c +++ b/dda_lookahead.c @@ -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 diff --git a/dda_lookahead.h b/dda_lookahead.h index aa8b8aa..a87dd42 100644 --- a/dda_lookahead.h +++ b/dda_lookahead.h @@ -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) diff --git a/dda_maths.h b/dda_maths.h index 28028a9..c51fd47 100644 --- a/dda_maths.h +++ b/dda_maths.h @@ -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 */ diff --git a/dda_queue.c b/dda_queue.c index 2b9e646..098d985 100644 --- a/dda_queue.c +++ b/dda_queue.c @@ -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; }