zaubentrucker/muele-marlin
1
0
Fork 0
Code Issues Pull requests Actions Packages Projects Releases Wiki Activity

Implement BEZIER_JERK_CONTROL

Browse source 
Enable 6th-order jerk-controlled motion planning in real-time.
Only for 32bit MCUs. (AVR simply does not have enough processing power for this!)
This commit is contained in:
etagle 2018-04-06 22:48:06 -03:00 committed by Scott Lahteine
parent 5932df7ea1
commit a29adde5c0
10 changed files with 401 additions and 81 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
.travis.yml
View file

@ -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}

11
Marlin/Configuration.h
View file

@ -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 =============================
//===========================================================================

2
Marlin/src/backtrace/unwarm_thumb.cpp
View file

@ -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;

2
Marlin/src/backtrace/unwinder.cpp
View file

@ -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;
}

2
Marlin/src/inc/SanityCheck.h
View file

@ -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)

28
Marlin/src/module/planner.cpp
View file

@ -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]);

22
Marlin/src/module/planner.h
View file

@ -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);

2
Marlin/src/module/planner_bezier.h
View file

@ -23,7 +23,7 @@
/**
* planner_bezier.h
*
* Compute and buffer movement commands for bezier curves
* Compute and buffer movement commands for Bézier curves
*
*/

352
Marlin/src/module/stepper.cpp
View file

@ -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);
#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;
#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
// 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

60
Marlin/src/module/stepper.h
View file

@ -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();