From a29adde5c0706a572261c40a02c61e1c966928a4 Mon Sep 17 00:00:00 2001 From: etagle Date: Fri, 6 Apr 2018 22:48:06 -0300 Subject: [PATCH] Implement BEZIER_JERK_CONTROL Enable 6th-order jerk-controlled motion planning in real-time. Only for 32bit MCUs. (AVR simply does not have enough processing power for this!) --- .travis.yml | 1 + Marlin/Configuration.h | 11 + Marlin/src/backtrace/unwarm_thumb.cpp | 2 +- Marlin/src/backtrace/unwinder.cpp | 2 +- Marlin/src/inc/SanityCheck.h | 2 + Marlin/src/module/planner.cpp | 28 ++- Marlin/src/module/planner.h | 22 +- Marlin/src/module/planner_bezier.h | 2 +- Marlin/src/module/stepper.cpp | 350 +++++++++++++++++++++++--- Marlin/src/module/stepper.h | 60 ++--- 10 files changed, 400 insertions(+), 80 deletions(-) diff --git a/.travis.yml b/.travis.yml index c4a9f1b30..b9a943138 100644 --- a/.travis.yml +++ b/.travis.yml @@ -436,6 +436,7 @@ script: - export TEST_PLATFORM="-e DUE" - restore_configs - opt_set MOTHERBOARD BOARD_RAMPS4DUE_EFB + - opt_set BEZIER_JERK_CONTROL - cp Marlin/Configuration.h Marlin/src/config/default/Configuration.h - cp Marlin/Configuration_adv.h Marlin/src/config/default/Configuration_adv.h - build_marlin_pio ${TRAVIS_BUILD_DIR} ${TEST_PLATFORM} diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h index f8bbc0bf7..fa688acb5 100644 --- a/Marlin/Configuration.h +++ b/Marlin/Configuration.h @@ -608,6 +608,17 @@ #define DEFAULT_ZJERK 0.3 #define DEFAULT_EJERK 5.0 +/** + * Realtime Jerk Control + * + * This option eliminates vibration during printing by fitting a Bézier + * curve to move acceleration, producing much smoother direction changes. + * Because this is computationally-intensive, a 32-bit MCU is required. + * + * See https://github.com/synthetos/TinyG/wiki/Jerk-Controlled-Motion-Explained + */ +//#define BEZIER_JERK_CONTROL + //=========================================================================== //============================= Z Probe Options ============================= //=========================================================================== diff --git a/Marlin/src/backtrace/unwarm_thumb.cpp b/Marlin/src/backtrace/unwarm_thumb.cpp index 8c14f6ecd..f2edc1372 100644 --- a/Marlin/src/backtrace/unwarm_thumb.cpp +++ b/Marlin/src/backtrace/unwarm_thumb.cpp @@ -260,7 +260,7 @@ UnwResult UnwStartThumb(UnwState * const state) { UnwPrintd5("TB%c [r%d,r%d%s]\n", H ? 'H' : 'B', rn, rm, H ? ",LSL #1" : ""); - // We are only interested if the RN is the PC. Let´s choose the 1st destination + // We are only interested if the RN is the PC. Let's choose the 1st destination if (rn == 15) { if (H) { uint16_t rv; diff --git a/Marlin/src/backtrace/unwinder.cpp b/Marlin/src/backtrace/unwinder.cpp index 88a61ee64..66281670c 100644 --- a/Marlin/src/backtrace/unwinder.cpp +++ b/Marlin/src/backtrace/unwinder.cpp @@ -28,7 +28,7 @@ extern "C" const UnwTabEntry __exidx_end[]; // Detect if unwind information is present or not static int HasUnwindTableInfo(void) { - // > 16 because there are default entries we can´t supress + // > 16 because there are default entries we can't supress return ((char*)(&__exidx_end) - (char*)(&__exidx_start)) > 16 ? 1 : 0; } diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index cd895f340..845fcf3da 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -99,6 +99,8 @@ #error "Z_ENDSTOP_SERVO_NR is now Z_PROBE_SERVO_NR. Please update your configuration." #elif defined(DEFAULT_XYJERK) #error "DEFAULT_XYJERK is deprecated. Use DEFAULT_XJERK and DEFAULT_YJERK instead." +#elif ENABLED(BEZIER_JERK_CONTROL) && !defined(CPU_32_BIT) + #error "BEZIER_JERK_CONTROL is computationally intensive and requires a 32-bit board." #elif defined(XY_TRAVEL_SPEED) #error "XY_TRAVEL_SPEED is deprecated. Use XY_PROBE_SPEED instead." #elif defined(PROBE_SERVO_DEACTIVATION_DELAY) diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 3d0e87134..cd95bb04f 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -229,6 +229,10 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e NOLESS(initial_rate, MINIMAL_STEP_RATE); NOLESS(final_rate, MINIMAL_STEP_RATE); + #if ENABLED(BEZIER_JERK_CONTROL) + uint32_t cruise_rate = initial_rate; + #endif + const int32_t accel = block->acceleration_steps_per_s2; // Steps required for acceleration, deceleration to/from nominal rate @@ -246,16 +250,36 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e NOLESS(accelerate_steps, 0); // Check limits due to numerical round-off accelerate_steps = min((uint32_t)accelerate_steps, block->step_event_count);//(We can cast here to unsigned, because the above line ensures that we are above zero) plateau_steps = 0; + + #if ENABLED(BEZIER_JERK_CONTROL) + // We won't reach the cruising rate. Let's calculate the speed we will reach + cruise_rate = final_speed(initial_rate, accel, accelerate_steps); + #endif } + #if ENABLED(BEZIER_JERK_CONTROL) + else // We have some plateau time, so the cruise rate will be the nominal rate + cruise_rate = block->nominal_rate; + #endif // block->accelerate_until = accelerate_steps; // block->decelerate_after = accelerate_steps+plateau_steps; + #if ENABLED(BEZIER_JERK_CONTROL) + // Jerk controlled speed requires to express speed versus time, NOT steps + int32_t acceleration_time = ((float)(cruise_rate - initial_rate) / accel) * HAL_STEPPER_TIMER_RATE, + deceleration_time = ((float)(cruise_rate - final_rate) / accel) * HAL_STEPPER_TIMER_RATE; + #endif + CRITICAL_SECTION_START; // Fill variables used by the stepper in a critical section if (!TEST(block->flag, BLOCK_BIT_BUSY)) { // Don't update variables if block is busy. block->accelerate_until = accelerate_steps; block->decelerate_after = accelerate_steps + plateau_steps; block->initial_rate = initial_rate; + #if ENABLED(BEZIER_JERK_CONTROL) + block->acceleration_time = acceleration_time; + block->deceleration_time = deceleration_time; + block->cruise_rate = cruise_rate; + #endif block->final_rate = final_rate; } CRITICAL_SECTION_END; @@ -1303,7 +1327,9 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE] } block->acceleration_steps_per_s2 = accel; block->acceleration = accel / steps_per_mm; - block->acceleration_rate = (long)(accel * (4096.0 * 4096.0 / (HAL_STEPPER_TIMER_RATE))); + #if DISABLED(BEZIER_JERK_CONTROL) + block->acceleration_rate = (long)(accel * (4096.0 * 4096.0 / (HAL_STEPPER_TIMER_RATE))); + #endif #if ENABLED(LIN_ADVANCE) if (block->use_advance_lead) { block->advance_speed = (HAL_STEPPER_TIMER_RATE) / (extruder_advance_K * block->e_D_ratio * block->acceleration * axis_steps_per_mm[E_AXIS_N]); diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index e2fedc444..0c752c1f8 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -90,9 +90,17 @@ typedef struct { uint32_t mix_event_count[MIXING_STEPPERS]; // Scaled step_event_count for the mixing steppers #endif + // Settings for the trapezoid generator int32_t accelerate_until, // The index of the step event on which to stop acceleration - decelerate_after, // The index of the step event on which to start decelerating - acceleration_rate; // The acceleration rate used for acceleration calculation + decelerate_after; // The index of the step event on which to start decelerating + + #if ENABLED(BEZIER_JERK_CONTROL) + uint32_t cruise_rate; // The actual cruise rate to use, between end of the acceleration phase and start of deceleration phase + int32_t acceleration_time, // Acceleration time and deceleration time in STEP timer counts + deceleration_time; + #else + int32_t acceleration_rate; // The acceleration rate used for acceleration calculation + #endif uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) @@ -112,7 +120,6 @@ typedef struct { millimeters, // The total travel of this block in mm acceleration; // acceleration mm/sec^2 - // Settings for the trapezoid generator uint32_t nominal_rate, // The nominal step rate for this block in step_events/sec initial_rate, // The jerk-adjusted step rate at start of block final_rate, // The minimal rate at exit @@ -639,6 +646,15 @@ class Planner { return SQRT(sq(target_velocity) - 2 * accel * distance); } + #if ENABLED(BEZIER_JERK_CONTROL) + /** + * Calculate the speed reached given initial speed, acceleration and distance + */ + static float final_speed(const float &initial_velocity, const float &accel, const float &distance) { + return SQRT(sq(initial_velocity) + 2 * accel * distance); + } + #endif + static void calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor); static void reverse_pass_kernel(block_t* const current, const block_t * const next); diff --git a/Marlin/src/module/planner_bezier.h b/Marlin/src/module/planner_bezier.h index e0ca8d178..7e4ea2365 100644 --- a/Marlin/src/module/planner_bezier.h +++ b/Marlin/src/module/planner_bezier.h @@ -23,7 +23,7 @@ /** * planner_bezier.h * - * Compute and buffer movement commands for bezier curves + * Compute and buffer movement commands for Bézier curves * */ diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 70ddd1f82..2b0974efa 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -44,6 +44,13 @@ /* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith and Philipp Tiefenbacher. */ +/* Jerk controlled movements planner added by Eduardo José Tagle in April + 2018, Equations based on Synthethos TinyG2 sources, but the fixed-point + implementation is a complete new one, as we are running the ISR with a + variable period. + Also implemented the Bézier velocity curve evaluation in ARM assembler, + to avoid impacting ISR speed. */ + #include "stepper.h" #ifdef __AVR__ @@ -109,6 +116,15 @@ long Stepper::counter_X = 0, volatile uint32_t Stepper::step_events_completed = 0; // The number of step events executed in the current block +#if ENABLED(BEZIER_JERK_CONTROL) + int32_t Stepper::bezier_A, // A coefficient in Bézier speed curve + Stepper::bezier_B, // B coefficient in Bézier speed curve + Stepper::bezier_C, // C coefficient in Bézier speed curve + Stepper::bezier_F; // F coefficient in Bézier speed curve + uint32_t Stepper::bezier_AV; // AV coefficient in Bézier speed curve + bool Stepper::bezier_2nd_half; // =false If Bézier curve has been initialized or not +#endif + #if ENABLED(LIN_ADVANCE) uint32_t Stepper::LA_decelerate_after; @@ -134,9 +150,9 @@ volatile uint32_t Stepper::step_events_completed = 0; // The number of step even #endif // LIN_ADVANCE -long Stepper::acceleration_time, Stepper::deceleration_time; +int32_t Stepper::acceleration_time, Stepper::deceleration_time; -volatile long Stepper::count_position[NUM_AXIS] = { 0 }; +volatile int32_t Stepper::count_position[NUM_AXIS] = { 0 }; volatile signed char Stepper::count_direction[NUM_AXIS] = { 1, 1, 1, 1 }; #if ENABLED(MIXING_EXTRUDER) @@ -145,8 +161,10 @@ volatile signed char Stepper::count_direction[NUM_AXIS] = { 1, 1, 1, 1 }; uint8_t Stepper::step_loops, Stepper::step_loops_nominal; -hal_timer_t Stepper::OCR1A_nominal, - Stepper::acc_step_rate; // needed for deceleration start point +hal_timer_t Stepper::OCR1A_nominal; +#if DISABLED(BEZIER_JERK_CONTROL) + hal_timer_t Stepper::acc_step_rate; // needed for deceleration start point +#endif volatile long Stepper::endstops_trigsteps[XYZ]; @@ -298,6 +316,207 @@ void Stepper::set_directions() { extern volatile uint8_t e_hit; #endif +#if ENABLED(BEZIER_JERK_CONTROL) + /** + * We are using a quintic (fifth-degree) Bézier polynomial for the velocity curve. + * This gives us a "linear pop" velocity curve; with pop being the sixth derivative of position: + * velocity - 1st, acceleration - 2nd, jerk - 3rd, snap - 4th, crackle - 5th, pop - 6th + * + * The Bézier curve takes the form: + * + * V(t) = P_0 * B_0(t) + P_1 * B_1(t) + P_2 * B_2(t) + P_3 * B_3(t) + P_4 * B_4(t) + P_5 * B_5(t) + * + * Where 0 <= t <= 1, and V(t) is the velocity. P_0 through P_5 are the control points, and B_0(t) + * through B_5(t) are the Bernstein basis as follows: + * + * B_0(t) = (1-t)^5 = -t^5 + 5t^4 - 10t^3 + 10t^2 - 5t + 1 + * B_1(t) = 5(1-t)^4 * t = 5t^5 - 20t^4 + 30t^3 - 20t^2 + 5t + * B_2(t) = 10(1-t)^3 * t^2 = -10t^5 + 30t^4 - 30t^3 + 10t^2 + * B_3(t) = 10(1-t)^2 * t^3 = 10t^5 - 20t^4 + 10t^3 + * B_4(t) = 5(1-t) * t^4 = -5t^5 + 5t^4 + * B_5(t) = t^5 = t^5 + * ^ ^ ^ ^ ^ ^ + * | | | | | | + * A B C D E F + * + * Unfortunately, we cannot use forward-differencing to calculate each position through + * the curve, as Marlin uses variable timer periods. So, we require a formula of the form: + * + * V_f(t) = A*t^5 + B*t^4 + C*t^3 + D*t^2 + E*t + F + * + * Looking at the above B_0(t) through B_5(t) expanded forms, if we take the coefficients of t^5 + * through t of the Bézier form of V(t), we can determine that: + * + * A = -P_0 + 5*P_1 - 10*P_2 + 10*P_3 - 5*P_4 + P_5 + * B = 5*P_0 - 20*P_1 + 30*P_2 - 20*P_3 + 5*P_4 + * C = -10*P_0 + 30*P_1 - 30*P_2 + 10*P_3 + * D = 10*P_0 - 20*P_1 + 10*P_2 + * E = - 5*P_0 + 5*P_1 + * F = P_0 + * + * Now, since we will (currently) *always* want the initial acceleration and jerk values to be 0, + * We set P_i = P_0 = P_1 = P_2 (initial velocity), and P_t = P_3 = P_4 = P_5 (target velocity), + * which, after simplification, resolves to: + * + * A = - 6*P_i + 6*P_t = 6*(P_t - P_i) + * B = 15*P_i - 15*P_t = 15*(P_i - P_t) + * C = -10*P_i + 10*P_t = 10*(P_t - P_i) + * D = 0 + * E = 0 + * F = P_i + * + * As the t is evaluated in non uniform steps here, there is no other way rather than evaluating + * the Bézier curve at each point: + * + * V_f(t) = A*t^5 + B*t^4 + C*t^3 + F [0 <= t <= 1] + * + * Floating point arithmetic execution time cost is prohibitive, so we will transform the math to + * use fixed point values to be able to evaluate it in realtime. Assuming a maximum of 250000 steps + * per second (driver pulses should at least be 2uS hi/2uS lo), and allocating 2 bits to avoid + * overflows on the evaluation of the Bézier curve, means we can use + * + * t: unsigned Q0.32 (0 <= t < 1) |range 0 to 0xFFFFFFFF unsigned + * A: signed Q24.7 , |range = +/- 250000 * 6 * 128 = +/- 192000000 = 0x0B71B000 | 28 bits + sign + * B: signed Q24.7 , |range = +/- 250000 *15 * 128 = +/- 480000000 = 0x1C9C3800 | 29 bits + sign + * C: signed Q24.7 , |range = +/- 250000 *10 * 128 = +/- 320000000 = 0x1312D000 | 29 bits + sign + * F: signed Q24.7 , |range = +/- 250000 * 128 = 32000000 = 0x01E84800 | 25 bits + sign + * + * The trapezoid generator state contains the following information, that we will use to create and evaluate + * the Bézier curve: + * + * blk->step_event_count [TS] = The total count of steps for this movement. (=distance) + * blk->initial_rate [VI] = The initial steps per second (=velocity) + * blk->final_rate [VF] = The ending steps per second (=velocity) + * and the count of events completed (step_events_completed) [CS] (=distance until now) + * + * Note the abbreviations we use in the following formulae are between []s + * + * At the start of each trapezoid, we calculate the coefficients A,B,C,F and Advance [AV], as follows: + * + * A = 6*128*(VF - VI) = 768*(VF - VI) + * B = 15*128*(VI - VF) = 1920*(VI - VF) + * C = 10*128*(VF - VI) = 1280*(VF - VI) + * F = 128*VI = 128*VI + * AV = (1<<32)/TS ~= 0xFFFFFFFF / TS (To use ARM UDIV, that is 32 bits) + * + * And for each point, we will evaluate the curve with the following sequence: + * + * uint32_t t = bezier_AV * curr_step; // t: Range 0 - 1^32 = 32 bits + * uint64_t f = t; + * f *= t; // Range 32*2 = 64 bits (unsigned) + * f >>= 32; // Range 32 bits (unsigned) + * f *= t; // Range 32*2 = 64 bits (unsigned) + * f >>= 32; // Range 32 bits : f = t^3 (unsigned) + * int64_t acc = (int64_t) bezier_F << 31; // Range 63 bits (signed) + * acc += ((uint32_t) f >> 1) * (int64_t) bezier_C; // Range 29bits + 31 = 60bits (plus sign) + * f *= t; // Range 32*2 = 64 bits + * f >>= 32; // Range 32 bits : f = t^3 (unsigned) + * acc += ((uint32_t) f >> 1) * (int64_t) bezier_B; // Range 29bits + 31 = 60bits (plus sign) + * f *= t; // Range 32*2 = 64 bits + * f >>= 32; // Range 32 bits : f = t^3 (unsigned) + * acc += ((uint32_t) f >> 1) * (int64_t) bezier_A; // Range 28bits + 31 = 59bits (plus sign) + * acc >>= (31 + 7); // Range 24bits (plus sign) + * + * This can be translated to the following ARM assembly sequence: + * + * At start: + * fhi = AV, flo = CS, alo = F + * + * muls fhi,flo | f = AV * CS 1 cycles + * mov t,fhi | t = AV * CS 1 cycles + * lsrs ahi,alo,#1 | a = F << 31 1 cycles + * lsls alo,alo,#31 | 1 cycles + * umull flo,fhi,fhi,t | f *= t 5 cycles [fhi:flo=64bits + * umull flo,fhi,fhi,t | f>>=32; f*=t 5 cycles [fhi:flo=64bits + * lsrs flo,fhi,#1 | 1 cycles [31bits + * smlal alo,ahi,flo,C | a+=(f>>33)*C; 5 cycles + * umull flo,fhi,fhi,t | f>>=32; f*=t 5 cycles [fhi:flo=64bits + * lsrs flo,fhi,#1 | 1 cycles [31bits + * smlal alo,ahi,flo,B | a+=(f>>33)*B; 5 cycles + * umull flo,fhi,fhi,t | f>>=32; f*=t 5 cycles [fhi:flo=64bits + * lsrs flo,fhi,#1 | f>>=33; 1 cycles [31bits + * smlal alo,ahi,flo,A | a+=(f>>33)*A; 5 cycles + * lsrs alo,ahi,#6 | a>>=38 1 cycles + * 43 cycles total + */ + + FORCE_INLINE void Stepper::_calc_bezier_curve_coeffs(const int32_t v0, const int32_t v1, const uint32_t interval) { + // Calculate the Bézier coefficients + bezier_A = 768 * (v1 - v0); + bezier_B = 1920 * (v0 - v1); + bezier_C = 1280 * (v1 - v0); + bezier_F = 128 * v0; + bezier_AV = 0xFFFFFFFF / interval; + } + + FORCE_INLINE int32_t Stepper::_eval_bezier_curve(const uint32_t curr_step) { + #if defined(__ARM__) || defined(__thumb__) + + // For ARM CORTEX M3/M4 CPUs, we have the optimized assembler version, that takes 43 cycles to execute + register uint32_t flo = 0; + register uint32_t fhi = bezier_AV * curr_step; + register uint32_t t = fhi; + register int32_t alo = bezier_F; + register int32_t ahi = 0; + register int32_t A = bezier_A; + register int32_t B = bezier_B; + register int32_t C = bezier_C; + + __asm__ __volatile__( + ".syntax unified" "\n\t" // is to prevent CM0,CM1 non-unified syntax + " lsrs %[ahi],%[alo],#1" "\n\t" // a = F << 31 1 cycles + " lsls %[alo],%[alo],#31" "\n\t" // 1 cycles + " umull %[flo],%[fhi],%[fhi],%[t]" "\n\t" // f *= t 5 cycles [fhi:flo=64bits] + " umull %[flo],%[fhi],%[fhi],%[t]" "\n\t" // f>>=32; f*=t 5 cycles [fhi:flo=64bits] + " lsrs %[flo],%[fhi],#1" "\n\t" // 1 cycles [31bits] + " smlal %[alo],%[ahi],%[flo],%[C]" "\n\t" // a+=(f>>33)*C; 5 cycles + " umull %[flo],%[fhi],%[fhi],%[t]" "\n\t" // f>>=32; f*=t 5 cycles [fhi:flo=64bits] + " lsrs %[flo],%[fhi],#1" "\n\t" // 1 cycles [31bits] + " smlal %[alo],%[ahi],%[flo],%[B]" "\n\t" // a+=(f>>33)*B; 5 cycles + " umull %[flo],%[fhi],%[fhi],%[t]" "\n\t" // f>>=32; f*=t 5 cycles [fhi:flo=64bits] + " lsrs %[flo],%[fhi],#1" "\n\t" // f>>=33; 1 cycles [31bits] + " smlal %[alo],%[ahi],%[flo],%[A]" "\n\t" // a+=(f>>33)*A; 5 cycles + " lsrs %[alo],%[ahi],#6" "\n\t" // a>>=38 1 cycles + : [alo]"+r"( alo ) , + [flo]"+r"( flo ) , + [fhi]"+r"( fhi ) , + [ahi]"+r"( ahi ) , + [A]"+r"( A ) , // <== Note: Even if A, B, C, and t registers are INPUT ONLY + [B]"+r"( B ) , // GCC does bad optimizations on the code if we list them as + [C]"+r"( C ) , // such, breaking this function. So, to avoid that problem, + [t]"+r"( t ) // we list all registers as input-outputs. + : + : "cc" + ); + return alo; + + #else + + // For non ARM targets, we provide a fallback implementation. Really doubt it + // will be useful, unless the processor is extremely fast. + + uint32_t t = bezier_AV * curr_step; // t: Range 0 - 1^32 = 32 bits + uint64_t f = t; + f *= t; // Range 32*2 = 64 bits (unsigned) + f >>= 32; // Range 32 bits (unsigned) + f *= t; // Range 32*2 = 64 bits (unsigned) + f >>= 32; // Range 32 bits : f = t^3 (unsigned) + int64_t acc = (int64_t) bezier_F << 31; // Range 63 bits (signed) + acc += ((uint32_t) f >> 1) * (int64_t) bezier_C; // Range 29bits + 31 = 60bits (plus sign) + f *= t; // Range 32*2 = 64 bits + f >>= 32; // Range 32 bits : f = t^3 (unsigned) + acc += ((uint32_t) f >> 1) * (int64_t) bezier_B; // Range 29bits + 31 = 60bits (plus sign) + f *= t; // Range 32*2 = 64 bits + f >>= 32; // Range 32 bits : f = t^3 (unsigned) + acc += ((uint32_t) f >> 1) * (int64_t) bezier_A; // Range 28bits + 31 = 59bits (plus sign) + acc >>= (31 + 7); // Range 24bits (plus sign) + return (int32_t) acc; + + #endif + } + +#endif // BEZIER_JERK_CONTROL + /** * Stepper Driver Interrupt * @@ -394,26 +613,73 @@ void Stepper::isr() { // If there is no current block, attempt to pop one from the buffer if (!current_block) { + // Anything in the buffer? if ((current_block = planner.get_current_block())) { - trapezoid_generator_reset(); + + // Initialize the trapezoid generator from the current block. + static int8_t last_extruder = -1; + + #if ENABLED(LIN_ADVANCE) + #if E_STEPPERS > 1 + if (current_block->active_extruder != last_extruder) { + current_adv_steps = 0; // If the now active extruder wasn't in use during the last move, its pressure is most likely gone. + LA_active_extruder = current_block->active_extruder; + } + #endif + + if ((use_advance_lead = current_block->use_advance_lead)) { + LA_decelerate_after = current_block->decelerate_after; + final_adv_steps = current_block->final_adv_steps; + max_adv_steps = current_block->max_adv_steps; + } + #endif + + if (current_block->direction_bits != last_direction_bits || current_block->active_extruder != last_extruder) { + last_direction_bits = current_block->direction_bits; + last_extruder = current_block->active_extruder; + set_directions(); + } + + // No acceleration / deceleration time elapsed so far + acceleration_time = deceleration_time = 0; + + // No step events completed so far + step_events_completed = 0; + + // step_rate to timer interval + OCR1A_nominal = calc_timer_interval(current_block->nominal_rate); + + // make a note of the number of step loops required at nominal speed + step_loops_nominal = step_loops; + + #if DISABLED(BEZIER_JERK_CONTROL) + // Set as deceleration point the initial rate of the block + acc_step_rate = current_block->initial_rate; + #endif + + #if ENABLED(BEZIER_JERK_CONTROL) + // Initialize the Bézier speed curve + _calc_bezier_curve_coeffs(current_block->initial_rate, current_block->cruise_rate, current_block->acceleration_time); + + // We have not started the 2nd half of the trapezoid + bezier_2nd_half = false; + #endif // Initialize Bresenham counters to 1/2 the ceiling counter_X = counter_Y = counter_Z = counter_E = -(current_block->step_event_count >> 1); - #if ENABLED(MIXING_EXTRUDER) MIXING_STEPPERS_LOOP(i) counter_m[i] = -(current_block->mix_event_count[i] >> 1); #endif - step_events_completed = 0; - #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE) e_hit = 2; // Needed for the case an endstop is already triggered before the new move begins. // No 'change' can be detected. #endif #if ENABLED(Z_LATE_ENABLE) + // If delayed Z enable, postpone move for 1mS if (current_block->steps[Z_AXIS] > 0) { enable_Z(); _NEXT_ISR(HAL_STEPPER_TIMER_RATE / 1000); // Run at slow speed - 1 KHz @@ -423,6 +689,7 @@ void Stepper::isr() { #endif } else { + // If no more queued moves, postpone next check for 1mS _NEXT_ISR(HAL_STEPPER_TIMER_RATE / 1000); // Run at slow speed - 1 KHz HAL_ENABLE_ISRs(); return; @@ -542,7 +809,6 @@ void Stepper::isr() { #endif #if ENABLED(LIN_ADVANCE) - counter_E += current_block->steps[E_AXIS]; if (counter_E > 0) { #if DISABLED(MIXING_EXTRUDER) @@ -640,15 +906,23 @@ void Stepper::isr() { // Calculate new timer value if (step_events_completed <= (uint32_t)current_block->accelerate_until) { - #ifdef CPU_32_BIT - MultiU32X24toH32(acc_step_rate, acceleration_time, current_block->acceleration_rate); + #if ENABLED(BEZIER_JERK_CONTROL) + // Get the next speed to use (Jerk limited!) + hal_timer_t acc_step_rate = + acceleration_time < current_block->acceleration_time + ? _eval_bezier_curve(acceleration_time) + : current_block->cruise_rate; #else - MultiU24X32toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate); - #endif - acc_step_rate += current_block->initial_rate; + #ifdef CPU_32_BIT + MultiU32X24toH32(acc_step_rate, acceleration_time, current_block->acceleration_rate); + #else + MultiU24X32toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate); + #endif + acc_step_rate += current_block->initial_rate; - // upper limit - NOMORE(acc_step_rate, current_block->nominal_rate); + // upper limit + NOMORE(acc_step_rate, current_block->nominal_rate); + #endif // step_rate to timer interval const hal_timer_t interval = calc_timer_interval(acc_step_rate); @@ -659,7 +933,6 @@ void Stepper::isr() { acceleration_time += interval; #if ENABLED(LIN_ADVANCE) - if (current_block->use_advance_lead) { if (step_events_completed == step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) { nextAdvanceISR = 0; // Wake up eISR on first acceleration loop and fire ISR if final adv_rate is reached @@ -670,23 +943,40 @@ void Stepper::isr() { eISR_Rate = ADV_NEVER; if (e_steps) nextAdvanceISR = 0; } - #endif // LIN_ADVANCE } else if (step_events_completed > (uint32_t)current_block->decelerate_after) { hal_timer_t step_rate; - #ifdef CPU_32_BIT - MultiU32X24toH32(step_rate, deceleration_time, current_block->acceleration_rate); + + #if ENABLED(BEZIER_JERK_CONTROL) + // If this is the 1st time we process the 2nd half of the trapezoid... + if (!bezier_2nd_half) { + + // Initialize the Bézier speed curve + _calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time); + bezier_2nd_half = true; + } + + // Calculate the next speed to use + step_rate = deceleration_time < current_block->deceleration_time + ? _eval_bezier_curve(deceleration_time) + : current_block->final_rate; #else - MultiU24X32toH16(step_rate, deceleration_time, current_block->acceleration_rate); - #endif - if (step_rate < acc_step_rate) { // Still decelerating? - step_rate = acc_step_rate - step_rate; - NOLESS(step_rate, current_block->final_rate); - } - else - step_rate = current_block->final_rate; + // Using the old trapezoidal control + #ifdef CPU_32_BIT + MultiU32X24toH32(step_rate, deceleration_time, current_block->acceleration_rate); + #else + MultiU24X32toH16(step_rate, deceleration_time, current_block->acceleration_rate); + #endif + + if (step_rate < acc_step_rate) { // Still decelerating? + step_rate = acc_step_rate - step_rate; + NOLESS(step_rate, current_block->final_rate); + } + else + step_rate = current_block->final_rate; + #endif // step_rate to timer interval const hal_timer_t interval = calc_timer_interval(step_rate); @@ -697,7 +987,6 @@ void Stepper::isr() { deceleration_time += interval; #if ENABLED(LIN_ADVANCE) - if (current_block->use_advance_lead) { if (step_events_completed <= (uint32_t)current_block->decelerate_after + step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) { nextAdvanceISR = 0; // Wake up eISR on first deceleration loop @@ -708,16 +997,13 @@ void Stepper::isr() { eISR_Rate = ADV_NEVER; if (e_steps) nextAdvanceISR = 0; } - #endif // LIN_ADVANCE } else { #if ENABLED(LIN_ADVANCE) - // If we have esteps to execute, fire the next advance_isr "now" if (e_steps && eISR_Rate != current_block->advance_speed) nextAdvanceISR = 0; - #endif SPLIT(OCR1A_nominal); // split step into multiple ISRs if larger than ENDSTOP_NOMINAL_OCR_VAL diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 568a7e792..d7fca16f2 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -97,6 +97,15 @@ class Stepper { static long counter_X, counter_Y, counter_Z, counter_E; static volatile uint32_t step_events_completed; // The number of step events executed in the current block + #if ENABLED(BEZIER_JERK_CONTROL) + static int32_t bezier_A, // A coefficient in Bézier speed curve + bezier_B, // B coefficient in Bézier speed curve + bezier_C, // C coefficient in Bézier speed curve + bezier_F; // F coefficient in Bézier speed curve + static uint32_t bezier_AV; // AV coefficient in Bézier speed curve + static bool bezier_2nd_half; // If Bézier curve has been initialized or not + #endif + #if ENABLED(LIN_ADVANCE) static uint32_t LA_decelerate_after; // Copy from current executed block. Needed because current_block is set to NULL "too early". @@ -117,11 +126,13 @@ class Stepper { #endif // !LIN_ADVANCE - static long acceleration_time, deceleration_time; + static int32_t acceleration_time, deceleration_time; static uint8_t step_loops, step_loops_nominal; - static hal_timer_t OCR1A_nominal, - acc_step_rate; // needed for deceleration start point + static hal_timer_t OCR1A_nominal; + #if DISABLED(BEZIER_JERK_CONTROL) + static hal_timer_t acc_step_rate; // needed for deceleration start point + #endif static volatile long endstops_trigsteps[XYZ]; static volatile long endstops_stepsTotal, endstops_stepsDone; @@ -129,7 +140,7 @@ class Stepper { // // Positions of stepper motors, in step units // - static volatile long count_position[NUM_AXIS]; + static volatile int32_t count_position[NUM_AXIS]; // // Current direction of stepper motors (+1 or -1) @@ -349,43 +360,10 @@ class Stepper { return timer; } - // Initialize the trapezoid generator from the current block. - // Called whenever a new block begins. - FORCE_INLINE static void trapezoid_generator_reset() { - - static int8_t last_extruder = -1; - - #if ENABLED(LIN_ADVANCE) - #if E_STEPPERS > 1 - if (current_block->active_extruder != last_extruder) { - current_adv_steps = 0; // If the now active extruder wasn't in use during the last move, its pressure is most likely gone. - LA_active_extruder = current_block->active_extruder; - } - #endif - - if ((use_advance_lead = current_block->use_advance_lead)) { - LA_decelerate_after = current_block->decelerate_after; - final_adv_steps = current_block->final_adv_steps; - max_adv_steps = current_block->max_adv_steps; - } - #endif - - if (current_block->direction_bits != last_direction_bits || current_block->active_extruder != last_extruder) { - last_direction_bits = current_block->direction_bits; - last_extruder = current_block->active_extruder; - set_directions(); - } - - deceleration_time = 0; - // step_rate to timer interval - OCR1A_nominal = calc_timer_interval(current_block->nominal_rate); - // make a note of the number of step loops required at nominal speed - step_loops_nominal = step_loops; - acc_step_rate = current_block->initial_rate; - acceleration_time = calc_timer_interval(acc_step_rate); - _NEXT_ISR(acceleration_time); - - } + #if ENABLED(BEZIER_JERK_CONTROL) + static void _calc_bezier_curve_coeffs(const int32_t v0, const int32_t v1, const uint32_t steps); + static int32_t _eval_bezier_curve(const uint32_t curr_step); + #endif #if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM static void digipot_init();