felix/Marlin
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Update stepper.cpp Bresenham for 2.0.x parity

Browse Source
This commit is contained in:
Scott Lahteine
2018-05-27 01:52:39 -05:00
parent 0d6ef67e48
commit 33b7488f68
1 changed files with 36 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
Marlin/stepper.cpp
View File

@@ -100,6 +100,13 @@ bool Stepper::abort_current_block;
bool Stepper::locked_z_motor = false, Stepper::locked_z2_motor = false; bool Stepper::locked_z_motor = false, Stepper::locked_z2_motor = false;
#endif #endif
/**
* Marlin uses the Bresenham algorithm. For a detailed explanation of theory and
* method see https://www.cs.helsinki.fi/group/goa/mallinnus/lines/bresenh.html
*
* The implementation used here additionally rounds up the starting seed.
*/
int32_t Stepper::counter_X = 0, int32_t Stepper::counter_X = 0,
Stepper::counter_Y = 0, Stepper::counter_Y = 0,
Stepper::counter_Z = 0, Stepper::counter_Z = 0,
@@ -369,8 +376,8 @@ void Stepper::set_directions() {
#if ENABLED(S_CURVE_ACCELERATION) #if ENABLED(S_CURVE_ACCELERATION)
/** /**
* We are using a quintic (fifth-degree) Bézier polynomial for the velocity curve. * This uses a quintic (fifth-degree) Bézier polynomial for the velocity curve, giving
* This gives us a "linear pop" velocity curve; with pop being the sixth derivative of position: * 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 * velocity - 1st, acceleration - 2nd, jerk - 3rd, snap - 4th, crackle - 5th, pop - 6th
* *
* The Bézier curve takes the form: * The Bézier curve takes the form:
@@ -444,7 +451,7 @@ void Stepper::set_directions() {
* *
* For Any 32bit CPU: * For Any 32bit CPU:
* *
* At the start of each trapezoid, we calculate the coefficients A,B,C,F and Advance [AV], as follows: * At the start of each trapezoid, calculate the coefficients A,B,C,F and Advance [AV], as follows:
* *
* A = 6*128*(VF - VI) = 768*(VF - VI) * A = 6*128*(VF - VI) = 768*(VF - VI)
* B = 15*128*(VI - VF) = 1920*(VI - VF) * B = 15*128*(VI - VF) = 1920*(VI - VF)
@@ -452,7 +459,7 @@ void Stepper::set_directions() {
* F = 128*VI = 128*VI * F = 128*VI = 128*VI
* AV = (1<<32)/TS ~= 0xFFFFFFFF / TS (To use ARM UDIV, that is 32 bits) (this is computed at the planner, to offload expensive calculations from the ISR) * AV = (1<<32)/TS ~= 0xFFFFFFFF / TS (To use ARM UDIV, that is 32 bits) (this is computed at the planner, to offload expensive calculations from the ISR)
* *
* And for each point, we will evaluate the curve with the following sequence: * And for each point, evaluate the curve with the following sequence:
* *
* void lsrs(uint32_t& d, uint32_t s, int cnt) { * void lsrs(uint32_t& d, uint32_t s, int cnt) {
* d = s >> cnt; * d = s >> cnt;
@@ -505,10 +512,10 @@ void Stepper::set_directions() {
* return alo; * return alo;
* } * }
* *
* This will be rewritten in ARM assembly to get peak performance and will take 43 cycles to execute * This is rewritten in ARM assembly for optimal performance (43 cycles to execute).
* *
* For AVR, we scale precision of coefficients to make it possible to evaluate the Bézier curve in * For AVR, the precision of coefficients is scaled so the Bézier curve can be evaluated in real-time:
* realtime: Let's reduce precision as much as possible. After some experimentation we found that: * Let's reduce precision as much as possible. After some experimentation we found that:
* *
* Assume t and AV with 24 bits is enough * Assume t and AV with 24 bits is enough
* A = 6*(VF - VI) * A = 6*(VF - VI)
@@ -529,7 +536,7 @@ void Stepper::set_directions() {
* C: signed Q24 , range = 250000 *10 = 2500000 = 0x1312D0 | 21 bits * C: signed Q24 , range = 250000 *10 = 2500000 = 0x1312D0 | 21 bits
* F: signed Q24 , range = 250000 = 250000 = 0x0ED090 | 20 bits * F: signed Q24 , range = 250000 = 250000 = 0x0ED090 | 20 bits
* *
* And for each curve, we estimate its coefficients with: * And for each curve, estimate its coefficients with:
* *
* void _calc_bezier_curve_coeffs(int32_t v0, int32_t v1, uint32_t av) { * void _calc_bezier_curve_coeffs(int32_t v0, int32_t v1, uint32_t av) {
* // Calculate the Bézier coefficients * // Calculate the Bézier coefficients
@@ -548,7 +555,7 @@ void Stepper::set_directions() {
* bezier_F = v0; * bezier_F = v0;
* } * }
* *
* And for each point, we will evaluate the curve with the following sequence: * And for each point, evaluate the curve with the following sequence:
* *
* // unsigned multiplication of 24 bits x 24bits, return upper 16 bits * // unsigned multiplication of 24 bits x 24bits, return upper 16 bits
* void umul24x24to16hi(uint16_t& r, uint24_t op1, uint24_t op2) { * void umul24x24to16hi(uint16_t& r, uint24_t op1, uint24_t op2) {
@@ -598,9 +605,8 @@ void Stepper::set_directions() {
* } * }
* return acc; * return acc;
* } * }
* Those functions will be translated into assembler to get peak performance. coefficient calculations takes 70 cycles, * These functions are translated to assembler for optimal performance.
* Bezier point evaluation takes 150 cycles * Coefficient calculation takes 70 cycles. Bezier point evaluation takes 150 cycles.
*
*/ */
// For AVR we use assembly to maximize speed // For AVR we use assembly to maximize speed
@@ -1258,12 +1264,12 @@ void Stepper::stepper_pulse_phase_isr() {
// Advance the Bresenham counter; start a pulse if the axis needs a step // Advance the Bresenham counter; start a pulse if the axis needs a step
#define PULSE_START(AXIS) do{ \ #define PULSE_START(AXIS) do{ \
_COUNTER(AXIS) += current_block->steps[_AXIS(AXIS)]; \ _COUNTER(AXIS) += current_block->steps[_AXIS(AXIS)]; \
if (_COUNTER(AXIS) > 0) { _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS), 0); } \ if (_COUNTER(AXIS) >= 0) { _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS), 0); } \
}while(0) }while(0)
// Advance the Bresenham counter; start a pulse if the axis needs a step // Advance the Bresenham counter; start a pulse if the axis needs a step
#define STEP_TICK(AXIS) do { \ #define STEP_TICK(AXIS) do { \
if (_COUNTER(AXIS) > 0) { \ if (_COUNTER(AXIS) >= 0) { \
_COUNTER(AXIS) -= current_block->step_event_count; \ _COUNTER(AXIS) -= current_block->step_event_count; \
count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \ count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \
} \ } \
@@ -1351,7 +1357,7 @@ void Stepper::stepper_pulse_phase_isr() {
#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)
// Don't step E here for mixing extruder // Don't step E here for mixing extruder
motor_direction(E_AXIS) ? --e_steps : ++e_steps; motor_direction(E_AXIS) ? --e_steps : ++e_steps;
@@ -1363,7 +1369,7 @@ void Stepper::stepper_pulse_phase_isr() {
const bool dir = motor_direction(E_AXIS); const bool dir = motor_direction(E_AXIS);
MIXING_STEPPERS_LOOP(j) { MIXING_STEPPERS_LOOP(j) {
counter_m[j] += current_block->steps[E_AXIS]; counter_m[j] += current_block->steps[E_AXIS];
if (counter_m[j] > 0) { if (counter_m[j] >= 0) {
counter_m[j] -= current_block->mix_event_count[j]; counter_m[j] -= current_block->mix_event_count[j];
dir ? --e_steps[j] : ++e_steps[j]; dir ? --e_steps[j] : ++e_steps[j];
} }
@@ -1380,7 +1386,7 @@ void Stepper::stepper_pulse_phase_isr() {
// Step mixing steppers (proportionally) // Step mixing steppers (proportionally)
counter_m[j] += current_block->steps[E_AXIS]; counter_m[j] += current_block->steps[E_AXIS];
// Step when the counter goes over zero // Step when the counter goes over zero
if (counter_m[j] > 0) En_STEP_WRITE(j, !INVERT_E_STEP_PIN); if (counter_m[j] >= 0) En_STEP_WRITE(j, !INVERT_E_STEP_PIN);
} }
#else // !MIXING_EXTRUDER #else // !MIXING_EXTRUDER
PULSE_START(E); PULSE_START(E);
@@ -1420,7 +1426,7 @@ void Stepper::stepper_pulse_phase_isr() {
#if DISABLED(LIN_ADVANCE) #if DISABLED(LIN_ADVANCE)
#if ENABLED(MIXING_EXTRUDER) #if ENABLED(MIXING_EXTRUDER)
MIXING_STEPPERS_LOOP(j) { MIXING_STEPPERS_LOOP(j) {
if (counter_m[j] > 0) { if (counter_m[j] >= 0) {
counter_m[j] -= current_block->mix_event_count[j]; counter_m[j] -= current_block->mix_event_count[j];
En_STEP_WRITE(j, INVERT_E_STEP_PIN); En_STEP_WRITE(j, INVERT_E_STEP_PIN);
} }
@@ -1702,11 +1708,11 @@ uint32_t Stepper::stepper_block_phase_isr() {
bezier_2nd_half = false; bezier_2nd_half = false;
#endif #endif
// Initialize Bresenham counters to 1/2 the ceiling // Initialize Bresenham counters to 1/2 the ceiling, with proper roundup (as explained in the article linked above)
counter_X = counter_Y = counter_Z = counter_E = -((int32_t)(current_block->step_event_count >> 1)); counter_X = counter_Y = counter_Z = counter_E = -int32_t((current_block->step_event_count >> 1) + (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] = -int32_t((current_block->mix_event_count[i] >> 1) + (current_block->mix_event_count[i] & 1));
#endif #endif
#if ENABLED(Z_LATE_ENABLE) #if ENABLED(Z_LATE_ENABLE)