88#include " language.h"
99#include " spi.h"
1010#include " Timer.h"
11+ #include " eeprom.h"
12+ #include < math.h>
1113
1214#define TMC2130_GCONF_NORMAL 0x00000000 // spreadCycle
1315#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)
883885 tmc2130_set_pwr (axis, pwr);
884886}
885887
888+ // Calculate constant torque value for a given microstep positionAdd commentMore actions
889+ //
890+ // Maintains |A|² + |B|² = constant throughout the microstep cycle, where A and B are the two motor phases.
891+ // Uses a two-phase approach: positions 0-127 follow a power-corrected sine curve,
892+ // while positions 128-255 are calculated using the constant torque constraint equation.
893+ //
894+ // Creates only deltas achievable by TMC2130 in at most four segments. It relies on the fact,
895+ // that for power corrected sine curve with fac [1, 1.2] the slope stays in the range [0, 2]
896+ // and fits at most three segments.
897+ //
898+ // Parameters:
899+ // - i: microstep position (0-255)
900+ // - va: previous amplitude value for delta calculation
901+ // - fac: power factor for linearity correction
902+ // - tcorr: pre-calculated torque correction factor
903+ // - carry: quantization error carry-forward (modified by reference)
904+ // - prev_theoretical_value: previous theoretical value for slope calculation (modified by reference)
905+ //
906+ // Returns: 8-bit amplitude value clamped to [SIN0, AMP] range
907+ //
908+ // References:
909+ // - Prusa Forum: "TMC2130 constant torque algorithm" discussion
910+ // 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/
911+ // - Analog Devices AN-026: "Stepper Motor Control Using TMC2130"
912+ // https://www.analog.com/en/resources/app-notes/an-026.html
913+ uint8_t tmc2130_calc_constant_torque_value (uint8_t i, uint8_t va, float fac, float tcorr,
914+ float & prev_theoretical_value) {
915+ constexpr uint8_t SIN0 = 0 ;
916+ constexpr uint8_t AMP = 248 ; // Amplitude limit as per AN-026 recommendation
917+ constexpr float TARGET_MAGNITUDE_SQUARED = (float )AMP * AMP + (float )SIN0 * SIN0;
918+
919+ // Theoretical constant torque value at microstep position i
920+ float theoretical_value;
921+
922+ if (i < 128 ) {
923+ // Phase 1 (positions 0-127): Power-corrected sine curve
924+ // Calculate theoretical value using sine function with power factor
925+ // correction and tcorr adjustment for midpoint matching
926+ float sin_val = sin (M_PI * (float )i / 512 .0f );
927+ theoretical_value = (AMP - SIN0) * pow (sin_val, fac) * tcorr + SIN0;
928+ } else {
929+ // Phase 2 (positions 128-255): Constant torque constraint solving
930+ // For constant torque: |A(i)|² + |B(i)|² = TARGET_MAGNITUDE_SQUARED
931+ // Since B(i) = A(255-i), solve: |A(i)|² + |A(255-i)|² = TARGET_MAGNITUDE_SQUARED
932+ // Therefore: A(i) = sqrt(TARGET_MAGNITUDE_SQUARED - A(255-i)²)
933+
934+ // Calculate mirror position value from Phase 1 curve
935+ uint8_t mirror_i = 255 - i;
936+ float sin_val = sin (M_PI * (float )mirror_i / 512 .0f );
937+ float mirror_theoretical = (AMP - SIN0) * pow (sin_val, fac) * tcorr + SIN0;
938+
939+ // Apply constant torque constraint
940+ theoretical_value = sqrt (TARGET_MAGNITUDE_SQUARED - mirror_theoretical * mirror_theoretical);
941+ }
942+
943+ // Step 1: Initial quantization using simple rounding
944+ uint8_t candidate_value = (uint8_t )(theoretical_value + 0.5 );
945+
946+ // Step 2: Slope-based delta limiting for TMC2130 compression
947+ // Calculate slope between current and previous theoretical values
948+ float slope = theoretical_value - prev_theoretical_value;
949+
950+ // Determine allowed delta range
951+ // This ensures delta ranges match slope ranges for optimal compression:
952+ // slope ∈ [0,1) → deltas ∈ [0,1], slope ∈ [1,2) → deltas ∈ [1,2], etc.
953+ int8_t min_delta = (int8_t )floor (slope);
954+
955+ // Clamp to TMC2130 hardware delta limits [-1, 3]
956+ // Max delta is 2 because we need range [min_delta, min_delta+1]
957+ if (min_delta < -1 ) {
958+ min_delta = -1 ;
959+ } else if (min_delta > 2 ) {
960+ min_delta = 2 ;
961+ }
962+
963+ // Enforce delta limits: constrain actual delta to [min_delta, min_delta+1]
964+ int8_t delta = candidate_value - va;
965+ if (delta < min_delta) {
966+ candidate_value = va + min_delta;
967+ } else if (delta > min_delta + 1 ) {
968+ candidate_value = va + min_delta + 1 ;
969+ }
970+
971+ // Step 3: Final amplitude clamping to valid TMC2130 range
972+ if (candidate_value < SIN0) {
973+ candidate_value = SIN0;
974+ } else if (candidate_value > AMP) {
975+ candidate_value = AMP;
976+ }
977+
978+ // Update previous theoretical value for next slope calculation
979+ prev_theoretical_value = theoretical_value;
980+
981+ return candidate_value;
982+ }
983+
984+ // TMC2130 Wave Generation with Algorithm Selection
985+ //
986+ // Algorithm Overview:
987+ // - Original: Simple sine wave with power factor adjustment
988+ // - Constant Torque: Maintains |A|² + |B|² = constant throughout microstep cycle
886989void tmc2130_set_wave (uint8_t axis, uint8_t amp, uint8_t fac1000)
887990{
888991// TMC2130 wave compression algorithm
889992// optimized for minimal memory requirements
890993// printf_P(PSTR("tmc2130_set_wave %d %d\n"), axis, fac1000);
891994 if (fac1000 < TMC2130_WAVE_FAC1000_MIN) fac1000 = 0 ;
892995 if (fac1000 > TMC2130_WAVE_FAC1000_MAX) fac1000 = TMC2130_WAVE_FAC1000_MAX;
893- float fac = 0 ;
996+ float fac = 1 ;
894997 if (fac1000) fac = ((float )((uint16_t )fac1000 + 1000 ) / 1000 ); // correction factor
895998// printf_P(PSTR(" factor: %s\n"), ftostr43(fac));
896999 uint8_t vA = 0 ; // value of currentA
@@ -904,16 +1007,40 @@ void tmc2130_set_wave(uint8_t axis, uint8_t amp, uint8_t fac1000)
9041007 int8_t dA; // delta value
9051008 uint8_t i = 0 ; // microstep index
9061009 uint32_t reg = 0 ; // tmc2130 register
907- tmc2130_wr_MSLUTSTART (axis, 0 , amp);
1010+
1011+ // Constant torque algorithm parameters (only used if use_constant_torque is true)
1012+ float prev_theoretical_value = 0.0 ; // Cache previous theoretical value for slope calculation (initialized with SIN0)
1013+ float tcorr = 1.0 ; // Pre-calculated correction factor for constant torque algorithm
1014+
1015+ uint8_t algorithm = eeprom_read_byte ((uint8_t *)EEPROM_TMC2130_WAVE_ALGORITHM);
1016+ bool use_constant_torque = (algorithm == TMC2130_WAVE_ALGORITHM_CONSTANT_TORQUE);
1017+
1018+ // Pre-calculate tcorr for constant torque algorithm to avoid redundant calculations
1019+ if (use_constant_torque) {
1020+ constexpr uint8_t SIN0 = 0 ;
1021+ constexpr uint8_t AMP = 248 ; // Amplitude limit as per AD recommendation
1022+ constexpr float MIDPOINT_VALUE = 175 .362481734263781f ; // sqrt((AMP² + SIN0²) / 2)
1023+ constexpr float SIN_127_5 = 0 .704934080375905f ; // sin(M_PI * 127.5f / 512.0f)
1024+
1025+ tcorr = (MIDPOINT_VALUE - SIN0) / ((AMP - SIN0) * pow (SIN_127_5, fac));
1026+ tmc2130_wr_MSLUTSTART (axis, SIN0, AMP);
1027+ } else {
1028+ tmc2130_wr_MSLUTSTART (axis, 0 , amp);
1029+ }
9081030 do
9091031 {
9101032 if ((i & 0x1f ) == 0 )
9111033 reg = 0 ;
912- // calculate value
913- if (fac == 0 ) // default TMC wave
914- vA = (uint8_t )((amp+1 ) * sin ((2 *PI*i + PI)/1024 ) + 0.5 ) - 1 ;
915- else // corrected wave
916- vA = (uint8_t )(amp * pow (sin (2 *PI*i/1024 ), fac) + 0.5 );
1034+
1035+ if (use_constant_torque) {
1036+ vA = tmc2130_calc_constant_torque_value (i, va, fac, tcorr, prev_theoretical_value);
1037+ } else {
1038+ // calculate value
1039+ if (fac == 1 ) // default TMC wave
1040+ vA = (uint8_t )((amp+1 ) * sin ((2 *PI*i + PI)/1024 ) + 0.5 ) - 1 ;
1041+ else // corrected wave
1042+ vA = (uint8_t )(amp * pow (sin (2 *PI*i/1024 ), fac) + 0.5 );
1043+ }
9171044 dA = vA - va; // calculate delta
9181045 va = vA;
9191046 b = -1 ;
0 commit comments