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

View file

@ -436,6 +436,7 @@ script:
- export TEST_PLATFORM="-e DUE" - export TEST_PLATFORM="-e DUE"
- restore_configs - restore_configs
- opt_set MOTHERBOARD BOARD_RAMPS4DUE_EFB - opt_set MOTHERBOARD BOARD_RAMPS4DUE_EFB
- opt_set BEZIER_JERK_CONTROL
- cp Marlin/Configuration.h Marlin/src/config/default/Configuration.h - cp Marlin/Configuration.h Marlin/src/config/default/Configuration.h
- cp Marlin/Configuration_adv.h Marlin/src/config/default/Configuration_adv.h - cp Marlin/Configuration_adv.h Marlin/src/config/default/Configuration_adv.h
- build_marlin_pio ${TRAVIS_BUILD_DIR} ${TEST_PLATFORM} - build_marlin_pio ${TRAVIS_BUILD_DIR} ${TEST_PLATFORM}

View file

@ -608,6 +608,17 @@
#define DEFAULT_ZJERK 0.3 #define DEFAULT_ZJERK 0.3
#define DEFAULT_EJERK 5.0 #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 ============================= //============================= Z Probe Options =============================
//=========================================================================== //===========================================================================

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" : ""); 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 (rn == 15) {
if (H) { if (H) {
uint16_t rv; uint16_t rv;

View file

@ -28,7 +28,7 @@ extern "C" const UnwTabEntry __exidx_end[];
// Detect if unwind information is present or not // Detect if unwind information is present or not
static int HasUnwindTableInfo(void) { 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; return ((char*)(&__exidx_end) - (char*)(&__exidx_start)) > 16 ? 1 : 0;
} }

View file

@ -99,6 +99,8 @@
#error "Z_ENDSTOP_SERVO_NR is now Z_PROBE_SERVO_NR. Please update your configuration." #error "Z_ENDSTOP_SERVO_NR is now Z_PROBE_SERVO_NR. Please update your configuration."
#elif defined(DEFAULT_XYJERK) #elif defined(DEFAULT_XYJERK)
#error "DEFAULT_XYJERK is deprecated. Use DEFAULT_XJERK and DEFAULT_YJERK instead." #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) #elif defined(XY_TRAVEL_SPEED)
#error "XY_TRAVEL_SPEED is deprecated. Use XY_PROBE_SPEED instead." #error "XY_TRAVEL_SPEED is deprecated. Use XY_PROBE_SPEED instead."
#elif defined(PROBE_SERVO_DEACTIVATION_DELAY) #elif defined(PROBE_SERVO_DEACTIVATION_DELAY)

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(initial_rate, MINIMAL_STEP_RATE);
NOLESS(final_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; const int32_t accel = block->acceleration_steps_per_s2;
// Steps required for acceleration, deceleration to/from nominal rate // 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 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) 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; 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->accelerate_until = accelerate_steps;
// block->decelerate_after = accelerate_steps+plateau_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 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. if (!TEST(block->flag, BLOCK_BIT_BUSY)) { // Don't update variables if block is busy.
block->accelerate_until = accelerate_steps; block->accelerate_until = accelerate_steps;
block->decelerate_after = accelerate_steps + plateau_steps; block->decelerate_after = accelerate_steps + plateau_steps;
block->initial_rate = initial_rate; 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; block->final_rate = final_rate;
} }
CRITICAL_SECTION_END; CRITICAL_SECTION_END;
@ -1303,7 +1327,9 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
} }
block->acceleration_steps_per_s2 = accel; block->acceleration_steps_per_s2 = accel;
block->acceleration = accel / steps_per_mm; 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 ENABLED(LIN_ADVANCE)
if (block->use_advance_lead) { 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]); block->advance_speed = (HAL_STEPPER_TIMER_RATE) / (extruder_advance_K * block->e_D_ratio * block->acceleration * axis_steps_per_mm[E_AXIS_N]);

View file

@ -90,9 +90,17 @@ typedef struct {
uint32_t mix_event_count[MIXING_STEPPERS]; // Scaled step_event_count for the mixing steppers uint32_t mix_event_count[MIXING_STEPPERS]; // Scaled step_event_count for the mixing steppers
#endif #endif
// Settings for the trapezoid generator
int32_t accelerate_until, // The index of the step event on which to stop acceleration 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 decelerate_after; // The index of the step event on which to start decelerating
acceleration_rate; // The acceleration rate used for acceleration calculation
#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) 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 millimeters, // The total travel of this block in mm
acceleration; // acceleration mm/sec^2 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 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 initial_rate, // The jerk-adjusted step rate at start of block
final_rate, // The minimal rate at exit final_rate, // The minimal rate at exit
@ -639,6 +646,15 @@ class Planner {
return SQRT(sq(target_velocity) - 2 * accel * distance); 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 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); static void reverse_pass_kernel(block_t* const current, const block_t * const next);

View file

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

View file

@ -44,6 +44,13 @@
/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith /* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
and Philipp Tiefenbacher. */ 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" #include "stepper.h"
#ifdef __AVR__ #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 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) #if ENABLED(LIN_ADVANCE)
uint32_t Stepper::LA_decelerate_after; 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 #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 }; volatile signed char Stepper::count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
#if ENABLED(MIXING_EXTRUDER) #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; uint8_t Stepper::step_loops, Stepper::step_loops_nominal;
hal_timer_t Stepper::OCR1A_nominal, hal_timer_t Stepper::OCR1A_nominal;
Stepper::acc_step_rate; // needed for deceleration start point #if DISABLED(BEZIER_JERK_CONTROL)
hal_timer_t Stepper::acc_step_rate; // needed for deceleration start point
#endif
volatile long Stepper::endstops_trigsteps[XYZ]; volatile long Stepper::endstops_trigsteps[XYZ];
@ -298,6 +316,207 @@ void Stepper::set_directions() {
extern volatile uint8_t e_hit; extern volatile uint8_t e_hit;
#endif #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 * Stepper Driver Interrupt
* *
@ -394,26 +613,73 @@ void Stepper::isr() {
// If there is no current block, attempt to pop one from the buffer // If there is no current block, attempt to pop one from the buffer
if (!current_block) { if (!current_block) {
// Anything in the buffer? // Anything in the buffer?
if ((current_block = planner.get_current_block())) { 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 // Initialize Bresenham counters to 1/2 the ceiling
counter_X = counter_Y = counter_Z = counter_E = -(current_block->step_event_count >> 1); counter_X = counter_Y = counter_Z = counter_E = -(current_block->step_event_count >> 1);
#if ENABLED(MIXING_EXTRUDER) #if ENABLED(MIXING_EXTRUDER)
MIXING_STEPPERS_LOOP(i) MIXING_STEPPERS_LOOP(i)
counter_m[i] = -(current_block->mix_event_count[i] >> 1); counter_m[i] = -(current_block->mix_event_count[i] >> 1);
#endif #endif
step_events_completed = 0;
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE) #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
e_hit = 2; // Needed for the case an endstop is already triggered before the new move begins. e_hit = 2; // Needed for the case an endstop is already triggered before the new move begins.
// No 'change' can be detected. // No 'change' can be detected.
#endif #endif
#if ENABLED(Z_LATE_ENABLE) #if ENABLED(Z_LATE_ENABLE)
// If delayed Z enable, postpone move for 1mS
if (current_block->steps[Z_AXIS] > 0) { if (current_block->steps[Z_AXIS] > 0) {
enable_Z(); enable_Z();
_NEXT_ISR(HAL_STEPPER_TIMER_RATE / 1000); // Run at slow speed - 1 KHz _NEXT_ISR(HAL_STEPPER_TIMER_RATE / 1000); // Run at slow speed - 1 KHz
@ -423,6 +689,7 @@ void Stepper::isr() {
#endif #endif
} }
else { else {
// If no more queued moves, postpone next check for 1mS
_NEXT_ISR(HAL_STEPPER_TIMER_RATE / 1000); // Run at slow speed - 1 KHz _NEXT_ISR(HAL_STEPPER_TIMER_RATE / 1000); // Run at slow speed - 1 KHz
HAL_ENABLE_ISRs(); HAL_ENABLE_ISRs();
return; return;
@ -542,7 +809,6 @@ void Stepper::isr() {
#endif #endif
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
counter_E += current_block->steps[E_AXIS]; counter_E += current_block->steps[E_AXIS];
if (counter_E > 0) { if (counter_E > 0) {
#if DISABLED(MIXING_EXTRUDER) #if DISABLED(MIXING_EXTRUDER)
@ -640,15 +906,23 @@ void Stepper::isr() {
// Calculate new timer value // Calculate new timer value
if (step_events_completed <= (uint32_t)current_block->accelerate_until) { if (step_events_completed <= (uint32_t)current_block->accelerate_until) {
#ifdef CPU_32_BIT #if ENABLED(BEZIER_JERK_CONTROL)
MultiU32X24toH32(acc_step_rate, acceleration_time, current_block->acceleration_rate); // 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 #else
MultiU24X32toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate); #ifdef CPU_32_BIT
#endif MultiU32X24toH32(acc_step_rate, acceleration_time, current_block->acceleration_rate);
acc_step_rate += current_block->initial_rate; #else
MultiU24X32toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
#endif
acc_step_rate += current_block->initial_rate;
// upper limit // upper limit
NOMORE(acc_step_rate, current_block->nominal_rate); NOMORE(acc_step_rate, current_block->nominal_rate);
#endif
// step_rate to timer interval // step_rate to timer interval
const hal_timer_t interval = calc_timer_interval(acc_step_rate); const hal_timer_t interval = calc_timer_interval(acc_step_rate);
@ -659,7 +933,6 @@ void Stepper::isr() {
acceleration_time += interval; acceleration_time += interval;
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
if (current_block->use_advance_lead) { if (current_block->use_advance_lead) {
if (step_events_completed == step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) { 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 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; eISR_Rate = ADV_NEVER;
if (e_steps) nextAdvanceISR = 0; if (e_steps) nextAdvanceISR = 0;
} }
#endif // LIN_ADVANCE #endif // LIN_ADVANCE
} }
else if (step_events_completed > (uint32_t)current_block->decelerate_after) { else if (step_events_completed > (uint32_t)current_block->decelerate_after) {
hal_timer_t step_rate; 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? #if ENABLED(BEZIER_JERK_CONTROL)
step_rate = acc_step_rate - step_rate; // If this is the 1st time we process the 2nd half of the trapezoid...
NOLESS(step_rate, current_block->final_rate); if (!bezier_2nd_half) {
}
else // Initialize the Bézier speed curve
step_rate = current_block->final_rate; _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 // step_rate to timer interval
const hal_timer_t interval = calc_timer_interval(step_rate); const hal_timer_t interval = calc_timer_interval(step_rate);
@ -697,7 +987,6 @@ void Stepper::isr() {
deceleration_time += interval; deceleration_time += interval;
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
if (current_block->use_advance_lead) { 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)) { 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 nextAdvanceISR = 0; // Wake up eISR on first deceleration loop
@ -708,16 +997,13 @@ void Stepper::isr() {
eISR_Rate = ADV_NEVER; eISR_Rate = ADV_NEVER;
if (e_steps) nextAdvanceISR = 0; if (e_steps) nextAdvanceISR = 0;
} }
#endif // LIN_ADVANCE #endif // LIN_ADVANCE
} }
else { else {
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
// If we have esteps to execute, fire the next advance_isr "now" // If we have esteps to execute, fire the next advance_isr "now"
if (e_steps && eISR_Rate != current_block->advance_speed) nextAdvanceISR = 0; if (e_steps && eISR_Rate != current_block->advance_speed) nextAdvanceISR = 0;
#endif #endif
SPLIT(OCR1A_nominal); // split step into multiple ISRs if larger than ENDSTOP_NOMINAL_OCR_VAL SPLIT(OCR1A_nominal); // split step into multiple ISRs if larger than ENDSTOP_NOMINAL_OCR_VAL

View file

@ -97,6 +97,15 @@ class Stepper {
static long counter_X, counter_Y, counter_Z, counter_E; 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 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) #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". 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 #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 uint8_t step_loops, step_loops_nominal;
static hal_timer_t OCR1A_nominal, static hal_timer_t OCR1A_nominal;
acc_step_rate; // needed for deceleration start point #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_trigsteps[XYZ];
static volatile long endstops_stepsTotal, endstops_stepsDone; static volatile long endstops_stepsTotal, endstops_stepsDone;
@ -129,7 +140,7 @@ class Stepper {
// //
// Positions of stepper motors, in step units // 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) // Current direction of stepper motors (+1 or -1)
@ -349,43 +360,10 @@ class Stepper {
return timer; return timer;
} }
// Initialize the trapezoid generator from the current block. #if ENABLED(BEZIER_JERK_CONTROL)
// Called whenever a new block begins. static void _calc_bezier_curve_coeffs(const int32_t v0, const int32_t v1, const uint32_t steps);
FORCE_INLINE static void trapezoid_generator_reset() { static int32_t _eval_bezier_curve(const uint32_t curr_step);
#endif
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 HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM #if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM
static void digipot_init(); static void digipot_init();