Implement TMC2130 constant torque algorithm for improved stepper motor performance #4871
+390
−26
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -8,6 +8,8 @@ | |
| #include "language.h" | ||
| #include "spi.h" | ||
| #include "Timer.h" | ||
| #include "eeprom.h" | ||
| #include <math.h> | ||
|
|
||
| #define TMC2130_GCONF_NORMAL 0x00000000 // spreadCycle | ||
| #define TMC2130_GCONF_SGSENS 0x00000180 // spreadCycle with stallguard (stall activates DIAG0 and DIAG1 [open collector]) | ||
|
|
@@ -883,14 +885,115 @@ void tmc2130_get_wave(uint8_t axis, uint8_t* data) | |
| tmc2130_set_pwr(axis, pwr); | ||
| } | ||
|
|
||
| // Calculate constant torque value for a given microstep positionAdd commentMore actions | ||
| // | ||
| // Maintains |A|² + |B|² = constant throughout the microstep cycle, where A and B are the two motor phases. | ||
| // Uses a two-phase approach: positions 0-127 follow a power-corrected sine curve, | ||
| // while positions 128-255 are calculated using the constant torque constraint equation. | ||
| // | ||
| // Creates only deltas achievable by TMC2130 in at most four segments. It relies on the fact, | ||
| // that for power corrected sine curve with fac [1, 1.2] the slope stays in the range [0, 2] | ||
| // and fits at most three segments. | ||
| // | ||
| // Parameters: | ||
| // - i: microstep position (0-255) | ||
| // - va: previous amplitude value for delta calculation | ||
| // - fac: power factor for linearity correction | ||
| // - tcorr: pre-calculated torque correction factor | ||
| // - carry: quantization error carry-forward (modified by reference) | ||
| // - prev_theoretical_value: previous theoretical value for slope calculation (modified by reference) | ||
| // | ||
| // Returns: 8-bit amplitude value clamped to [SIN0, AMP] range | ||
| // | ||
| // References: | ||
| // - Prusa Forum: "TMC2130 constant torque algorithm" discussion | ||
| // https://forum.prusa3d.com/forum/original-prusa-i3-mk3s-mk3-user-mods-octoprint-enclosures-nozzles/stepper-motor-upgrades-to-eliminate-vfa-s-vertical-fine-artifacts/paged/2/ | ||
| // - Analog Devices AN-026: "Stepper Motor Control Using TMC2130" | ||
| // https://www.analog.com/en/resources/app-notes/an-026.html | ||
| uint8_t tmc2130_calc_constant_torque_value(uint8_t i, uint8_t va, float fac, float tcorr, | ||
| float& prev_theoretical_value) { | ||
| constexpr uint8_t SIN0 = 0; | ||
| constexpr uint8_t AMP = 248; // Amplitude limit as per AN-026 recommendation | ||
| constexpr float TARGET_MAGNITUDE_SQUARED = (float)AMP * AMP + (float)SIN0 * SIN0; | ||
|
|
||
| // Theoretical constant torque value at microstep position i | ||
| float theoretical_value; | ||
|
|
||
| if (i < 128) { | ||
| // Phase 1 (positions 0-127): Power-corrected sine curve | ||
| // Calculate theoretical value using sine function with power factor | ||
| // correction and tcorr adjustment for midpoint matching | ||
| float sin_val = sin(M_PI * (float)i / 512.0f); | ||
|
Collaborator
There was a problem hiding this comment. I suspect the compiler doesnt calculate If I'm right this will consume less memory and drop one division operation: constexpr float sin_gain = M_PI / 512.f;
float sin_val = sin(sin_gain * i);I may be wrong but I think I have seen this happen before.
Author
There was a problem hiding this comment. As this is only calculated upon startup or changing of the value, I did not spend too much time optimizing, but I can certainly investigate further. |
||
| theoretical_value = (AMP - SIN0) * pow(sin_val, fac) * tcorr + SIN0; | ||
| } else { | ||
| // Phase 2 (positions 128-255): Constant torque constraint solving | ||
| // For constant torque: |A(i)|² + |B(i)|² = TARGET_MAGNITUDE_SQUARED | ||
| // Since B(i) = A(255-i), solve: |A(i)|² + |A(255-i)|² = TARGET_MAGNITUDE_SQUARED | ||
| // Therefore: A(i) = sqrt(TARGET_MAGNITUDE_SQUARED - A(255-i)²) | ||
|
|
||
| // Calculate mirror position value from Phase 1 curve | ||
| uint8_t mirror_i = 255 - i; | ||
| float sin_val = sin(M_PI * (float)mirror_i / 512.0f); | ||
| float mirror_theoretical = (AMP - SIN0) * pow(sin_val, fac) * tcorr + SIN0; | ||
|
|
||
| // Apply constant torque constraint | ||
| theoretical_value = sqrt(TARGET_MAGNITUDE_SQUARED - mirror_theoretical * mirror_theoretical); | ||
| } | ||
|
|
||
| // Step 1: Initial quantization using simple rounding | ||
| uint8_t candidate_value = (uint8_t)(theoretical_value + 0.5); | ||
|
|
||
| // Step 2: Slope-based delta limiting for TMC2130 compression | ||
| // Calculate slope between current and previous theoretical values | ||
| float slope = theoretical_value - prev_theoretical_value; | ||
|
|
||
| // Determine allowed delta range | ||
| // This ensures delta ranges match slope ranges for optimal compression: | ||
| // slope ∈ [0,1) → deltas ∈ [0,1], slope ∈ [1,2) → deltas ∈ [1,2], etc. | ||
| int8_t min_delta = (int8_t)floor(slope); | ||
|
Collaborator
There was a problem hiding this comment. Is the floor function required? It treats positive and negative numbers differently. Would it work for |
||
|
|
||
| // Clamp to TMC2130 hardware delta limits [-1, 3] | ||
| // Max delta is 2 because we need range [min_delta, min_delta+1] | ||
| if (min_delta < -1) { | ||
| min_delta = -1; | ||
| } else if (min_delta > 2) { | ||
| min_delta = 2; | ||
|
Collaborator
There was a problem hiding this comment. Sometimes we can save some memory by using the constrain macro. https://docs.arduino.cc/language-reference/en/functions/math/constrain/ min_delta = constrain(min_delta, -1, 2);It wouldnt save much but perhaps some. |
||
| } | ||
|
|
||
| // Enforce delta limits: constrain actual delta to [min_delta, min_delta+1] | ||
| int8_t delta = candidate_value - va; | ||
| if (delta < min_delta) { | ||
| candidate_value = va + min_delta; | ||
| } else if (delta > min_delta + 1) { | ||
| candidate_value = va + min_delta + 1; | ||
| } | ||
|
|
||
| // Step 3: Final amplitude clamping to valid TMC2130 range | ||
| if (candidate_value < SIN0) { | ||
| candidate_value = SIN0; | ||
| } else if (candidate_value > AMP) { | ||
| candidate_value = AMP; | ||
| } | ||
|
|
||
| // Update previous theoretical value for next slope calculation | ||
| prev_theoretical_value = theoretical_value; | ||
|
|
||
| return candidate_value; | ||
| } | ||
|
|
||
| // TMC2130 Wave Generation with Algorithm Selection | ||
| // | ||
| // Algorithm Overview: | ||
| // - Original: Simple sine wave with power factor adjustment | ||
| // - Constant Torque: Maintains |A|² + |B|² = constant throughout microstep cycle | ||
| void tmc2130_set_wave(uint8_t axis, uint8_t amp, uint8_t fac1000) | ||
| { | ||
| // TMC2130 wave compression algorithm | ||
| // optimized for minimal memory requirements | ||
| // printf_P(PSTR("tmc2130_set_wave %d %d\n"), axis, fac1000); | ||
| if (fac1000 < TMC2130_WAVE_FAC1000_MIN) fac1000 = 0; | ||
| if (fac1000 > TMC2130_WAVE_FAC1000_MAX) fac1000 = TMC2130_WAVE_FAC1000_MAX; | ||
| float fac = 1; | ||
| if (fac1000) fac = ((float)((uint16_t)fac1000 + 1000) / 1000); //correction factor | ||
| // printf_P(PSTR(" factor: %s\n"), ftostr43(fac)); | ||
| uint8_t vA = 0; //value of currentA | ||
|
|
@@ -904,16 +1007,40 @@ void tmc2130_set_wave(uint8_t axis, uint8_t amp, uint8_t fac1000) | |
| int8_t dA; //delta value | ||
| uint8_t i = 0; //microstep index | ||
| uint32_t reg = 0; //tmc2130 register | ||
|
|
||
| // Constant torque algorithm parameters (only used if use_constant_torque is true) | ||
| float prev_theoretical_value = 0.0; // Cache previous theoretical value for slope calculation (initialized with SIN0) | ||
| float tcorr = 1.0; // Pre-calculated correction factor for constant torque algorithm | ||
|
|
||
| uint8_t algorithm = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_WAVE_ALGORITHM); | ||
| bool use_constant_torque = (algorithm == TMC2130_WAVE_ALGORITHM_CONSTANT_TORQUE); | ||
|
|
||
| // Pre-calculate tcorr for constant torque algorithm to avoid redundant calculations | ||
| if (use_constant_torque) { | ||
| constexpr uint8_t SIN0 = 0; | ||
| constexpr uint8_t AMP = 248; // Amplitude limit as per AD recommendation | ||
| constexpr float MIDPOINT_VALUE = 175.362481734263781f; // sqrt((AMP² + SIN0²) / 2) | ||
| constexpr float SIN_127_5 = 0.704934080375905f; // sin(M_PI * 127.5f / 512.0f) | ||
|
|
||
| tcorr = (MIDPOINT_VALUE - SIN0) / ((AMP - SIN0) * pow(SIN_127_5, fac)); | ||
| tmc2130_wr_MSLUTSTART(axis, SIN0, AMP); | ||
| } else { | ||
| tmc2130_wr_MSLUTSTART(axis, 0, amp); | ||
| } | ||
| do | ||
| { | ||
| if ((i & 0x1f) == 0) | ||
| reg = 0; | ||
|
|
||
| if (use_constant_torque) { | ||
| vA = tmc2130_calc_constant_torque_value(i, va, fac, tcorr, prev_theoretical_value); | ||
| } else { | ||
| // calculate value | ||
| if (fac == 1) // default TMC wave | ||
| vA = (uint8_t)((amp+1) * sin((2*PI*i + PI)/1024) + 0.5) - 1; | ||
| else // corrected wave | ||
| vA = (uint8_t)(amp * pow(sin(2*PI*i/1024), fac) + 0.5); | ||
| } | ||
| dA = vA - va; // calculate delta | ||
| va = vA; | ||
| b = -1; | ||
|
|
||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
The new variable should also be documented in the large table near the top of this file. Between lines 100-400 :)