From af8be0316503974cb5a7017e52830b6db181c454 Mon Sep 17 00:00:00 2001 From: InsanityAutomation Date: Sun, 9 Aug 2020 16:43:08 -0400 Subject: [PATCH] DIGIPOTSS in motter current array with PWM, stored to eeprom, and G34 tested --- Marlin/src/gcode/calibrate/G34_M422.cpp | 143 +++++++++++++----------- Marlin/src/module/stepper.cpp | 61 +++++----- Marlin/src/module/stepper.h | 47 ++++---- 3 files changed, 136 insertions(+), 115 deletions(-) diff --git a/Marlin/src/gcode/calibrate/G34_M422.cpp b/Marlin/src/gcode/calibrate/G34_M422.cpp index 68b5b1b7b0..31555ff0bf 100644 --- a/Marlin/src/gcode/calibrate/G34_M422.cpp +++ b/Marlin/src/gcode/calibrate/G34_M422.cpp @@ -22,31 +22,32 @@ #include "../../inc/MarlinConfig.h" -#if ENABLED(Z_STEPPER_AUTO_ALIGN) - -#include "../../feature/z_stepper_align.h" - #include "../gcode.h" #include "../../module/planner.h" #include "../../module/stepper.h" #include "../../module/motion.h" #include "../../module/probe.h" - -#if HAS_MULTI_HOTEND - #include "../../module/tool_change.h" -#endif +#include "../../module/endstops.h" #if HAS_LEVELING #include "../../feature/bedlevel/bedlevel.h" #endif +#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) +#include "../../core/debug_out.h" + +#if ENABLED(Z_STEPPER_AUTO_ALIGN) + +#include "../../feature/z_stepper_align.h" + +#if HAS_MULTI_HOTEND + #include "../../module/tool_change.h" +#endif + #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) #include "../../libs/least_squares_fit.h" #endif -#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) -#include "../../core/debug_out.h" - /** * G34: Z-Stepper automatic alignment * @@ -491,59 +492,25 @@ void GcodeSuite::M422() { TEMPORARY_SOFT_ENDSTOP_STATE(false); TEMPORARY_BED_LEVELING_STATE(false); + TemporaryGlobalEndstopsState unlock_z(false); #ifdef GANTRY_CALIBRATION_COMMANDS_PRE gcode.process_subcommands_now_P(PSTR(GANTRY_CALIBRATION_COMMANDS_PRE)); #endif - // Store current motor settings, then apply reduced value - #if DAC_STEPPER_CURRENT - const float target_current = parserfloatval('S', GANTRY_CALIBRATION_CURRENT); - const float previous_current = dac_amps(Z_AXIS, target_current); - else - const int16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT); - #endif - - #if HAS_DIGIPOTSS - const uint32_t previous_current = motor_current_setting[Z_AXIS]; - stepper.digipot_current(Z_AXIS, target_current); - #elif HAS_MOTOR_CURRENT_PWM - const uint32_t previous_current = motor_current_setting[Z_AXIS]; - stepper.digipot_current(1, target_current); - #if DAC_STEPPER_CURRENT - dac_current_raw(Z_AXIS, target_current); - #elif ENABLED(HAS_I2C_DIGIPOT) - previous_current = dac_amps(Z_AXIS); - digipot_i2c_set_current(Z_AXIS, target_current) - #elif HAS_TRINAMIC_CONFIG - static uint16_t previous_current_arr[NUM_Z_STEPPER_DRIVERS]; - #if AXIS_IS_TMC(Z) - previous_current_arr[0] = stepperZ.getMilliamps(); - stepperZ.rms_current(target_current); - #endif - #if AXIS_IS_TMC(Z2) - previous_current_arr[1] = stepperZ2.getMilliamps(); - stepperZ2.rms_current(target_current); - #endif - #if AXIS_IS_TMC(Z3) - previous_current_arr[2] = stepperZ3.getMilliamps(); - stepperZ3.rms_current(target_current); - #endif - #if AXIS_IS_TMC(Z4) - previous_current_arr[3] = stepperZ4.getMilliamps(); - stepperZ4.rms_current(target_current); - #endif - #endif + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Sub Commands Processed"); + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Parking XY"); // Move XY to safe position #ifdef GANTRY_CALIBRATION_SAFE_POSITION xy_pos_t safe_pos = GANTRY_CALIBRATION_SAFE_POSITION; current_position[X_AXIS] = safe_pos[X_AXIS]; current_position[Y_AXIS] = safe_pos[Y_AXIS]; - do_blocking_move_to(current_position, MMM_TO_MMS(GANTRY_CALIBRATION_XY_PARK_FEEDRATE); + do_blocking_move_to(current_position, MMM_TO_MMS(GANTRY_CALIBRATION_XY_PARK_FEEDRATE)); planner.synchronize(); #endif + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Setting Z Pounce"); const uint16_t move_distance = parser.intval('Z', GANTRY_CALIBRATION_EXTRA_HEIGHT); // Move Z to pounce position @@ -553,9 +520,49 @@ void GcodeSuite::M422() { current_position[Z_AXIS] = (Z_MIN_POS + move_distance); #endif - do_blocking_move_to(current_position, MMM_TO_MMS(HOMING_FEEDRATE_Z); + do_blocking_move_to(current_position, MMM_TO_MMS(HOMING_FEEDRATE_Z)); planner.synchronize(); + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Reducing Current"); + // Store current motor settings, then apply reduced value + #if HAS_DIGIPOTSS + const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT); + const uint32_t previous_current = stepper.motor_current_setting[Z_AXIS]; + stepper.digipot_current(Z_AXIS, target_current); + #elif HAS_MOTOR_CURRENT_PWM + const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT); + const uint32_t previous_current = stepper.motor_current_setting[Z_AXIS]; + stepper.digipot_current(1, target_current); + #elif DAC_STEPPER_CURRENT + const float target_current = parser.floatval('S', GANTRY_CALIBRATION_CURRENT); + const float previous_current = dac_amps(Z_AXIS, target_current); + dac_current_raw(Z_AXIS, target_current); + #elif ENABLED(HAS_I2C_DIGIPOT) + const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT); + previous_current = dac_amps(Z_AXIS); + digipot_i2c_set_current(Z_AXIS, target_current) + #elif HAS_TRINAMIC_CONFIG + const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT); + static uint16_t previous_current_arr[NUM_Z_STEPPER_DRIVERS]; + #if AXIS_IS_TMC(Z) + previous_current_arr[0] = stepperZ.getMilliamps(); + stepperZ.rms_current(target_current); + #endif + #if AXIS_IS_TMC(Z2) + previous_current_arr[1] = stepperZ2.getMilliamps(); + stepperZ2.rms_current(target_current); + #endif + #if AXIS_IS_TMC(Z3) + previous_current_arr[2] = stepperZ3.getMilliamps(); + stepperZ3.rms_current(target_current); + #endif + #if AXIS_IS_TMC(Z4) + previous_current_arr[3] = stepperZ4.getMilliamps(); + stepperZ4.rms_current(target_current); + #endif + #endif + + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Final Z Move"); // Do Final Z move to adjust #if GANTRY_CALIBRATION_DIRECTION == 1 current_position[Z_AXIS] = (Z_MAX_POS + move_distance); @@ -563,9 +570,11 @@ void GcodeSuite::M422() { current_position[Z_AXIS] = (Z_MIN_POS - move_distance); #endif - do_blocking_move_to(current_position, MMM_TO_MMS(GANTRY_CALIBRATION_FEEDRATE); + do_blocking_move_to(current_position, MMM_TO_MMS(GANTRY_CALIBRATION_FEEDRATE)); planner.synchronize(); + + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Z Backoff"); // Back off end plate, back to normal motion range #if GANTRY_CALIBRATION_DIRECTION == 1 current_position[Z_AXIS] = (Z_MAX_POS - move_distance); @@ -573,34 +582,38 @@ void GcodeSuite::M422() { current_position[Z_AXIS] = (Z_MIN_POS + move_distance); #endif - do_blocking_move_to(current_position, MMM_TO_MMS(GANTRY_CALIBRATION_FEEDRATE); + do_blocking_move_to(current_position, MMM_TO_MMS(GANTRY_CALIBRATION_FEEDRATE)); planner.synchronize(); + + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Restore Current"); // Reset current to original values #if HAS_DIGIPOTSS stepper.digipot_current(Z_AXIS, previous_current); #elif HAS_MOTOR_CURRENT_PWM stepper.digipot_current(1, previous_current); - #if DAC_STEPPER_CURRENT + #elif DAC_STEPPER_CURRENT dac_current_raw(Z_AXIS, previous_current); #elif ENABLED(HAS_I2C_DIGIPOT) digipot_i2c_set_current(Z_AXIS, previous_current) #elif HAS_TRINAMIC_CONFIG static uint16_t previous_current_arr[NUM_Z_STEPPER_DRIVERS]; #if AXIS_IS_TMC(Z) - stepperZ.rms_current(previous_current_arr[0]); - #endif - #if AXIS_IS_TMC(Z2) - stepperZ2.rms_current(previous_current_arr[1]); - #endif - #if AXIS_IS_TMC(Z3) - stepperZ3.rms_current(previous_current_arr[2]); - #endif - #if AXIS_IS_TMC(Z4) - stepperZ4.rms_current(previous_current_arr[3]); - #endif + stepperZ.rms_current(previous_current_arr[0]); + #endif + #if AXIS_IS_TMC(Z2) + stepperZ2.rms_current(previous_current_arr[1]); + #endif + #if AXIS_IS_TMC(Z3) + stepperZ3.rms_current(previous_current_arr[2]); + #endif + #if AXIS_IS_TMC(Z4) + stepperZ4.rms_current(previous_current_arr[3]); + #endif #endif + + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Running Post Commands"); #ifdef GANTRY_CALIBRATION_COMMANDS_POST gcode.process_subcommands_now_P(PSTR(GANTRY_CALIBRATION_COMMANDS_POST)); #endif diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 8a5de4b608..0e59549b81 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -83,6 +83,10 @@ Stepper stepper; // Singleton #define BABYSTEPPING_EXTRA_DIR_WAIT +#if HAS_MOTOR_CURRENT_PWM || HAS_DIGIPOTSS + bool Stepper::initialized; // = false +#endif + #ifdef __AVR__ #include "speed_lookuptable.h" #endif @@ -106,7 +110,7 @@ Stepper stepper; // Singleton #include "../feature/dac/dac_dac084s085.h" #endif -#if HAS_MOTOR_CURRENT_SPI +#if HAS_DIGIPOTSS #include #endif @@ -138,12 +142,11 @@ Stepper stepper; // Singleton bool Stepper::separate_multi_axis = false; #endif -#if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM - bool Stepper::initialized; // = false - uint32_t Stepper::motor_current_setting[MOTOR_CURRENT_COUNT]; // Initialized by settings.load() - #if HAS_MOTOR_CURRENT_SPI - constexpr uint32_t Stepper::digipot_count[]; - #endif +#if HAS_MOTOR_CURRENT_PWM + uint32_t Stepper::motor_current_setting[3]; // Initialized by settings.load() +#elif HAS_DIGIPOTSS + constexpr uint32_t digipot_count[] = DIGIPOT_MOTOR_CURRENT; + uint32_t Stepper::motor_current_setting[COUNT(digipot_count)]; // Initialized by settings.load() #endif // private: @@ -155,7 +158,7 @@ uint8_t Stepper::last_direction_bits, // = 0 bool Stepper::abort_current_block; -#if DISABLED(MIXING_EXTRUDER) && HAS_MULTI_EXTRUDER +#if DISABLED(MIXING_EXTRUDER) && EXTRUDERS > 1 uint8_t Stepper::last_moved_extruder = 0xFF; #endif @@ -191,7 +194,7 @@ uint32_t Stepper::advance_divisor = 0, Stepper::decelerate_after, // The count at which to start decelerating Stepper::step_event_count; // The total event count for the current block -#if EITHER(HAS_MULTI_EXTRUDER, MIXING_EXTRUDER) +#if EXTRUDERS > 1 || ENABLED(MIXING_EXTRUDER) uint8_t Stepper::stepper_extruder; #else constexpr uint8_t Stepper::stepper_extruder; @@ -357,11 +360,11 @@ xyze_int8_t Stepper::count_direction{0}; #elif ENABLED(DUAL_X_CARRIAGE) #define X_APPLY_DIR(v,ALWAYS) do{ \ if (extruder_duplication_enabled || ALWAYS) { X_DIR_WRITE(v); X2_DIR_WRITE(mirrored_duplication_mode ? !(v) : v); } \ - else if (last_moved_extruder) X2_DIR_WRITE(v); else X_DIR_WRITE(v); \ + else if (movement_extruder()) X2_DIR_WRITE(v); else X_DIR_WRITE(v); \ }while(0) #define X_APPLY_STEP(v,ALWAYS) do{ \ if (extruder_duplication_enabled || ALWAYS) { X_STEP_WRITE(v); X2_STEP_WRITE(v); } \ - else if (last_moved_extruder) X2_STEP_WRITE(v); else X_STEP_WRITE(v); \ + else if (movement_extruder()) X2_STEP_WRITE(v); else X_STEP_WRITE(v); \ }while(0) #else #define X_APPLY_DIR(v,Q) X_DIR_WRITE(v) @@ -2131,7 +2134,7 @@ uint32_t Stepper::block_phase_isr() { MIXER_STEPPER_SETUP(); #endif - #if HAS_MULTI_EXTRUDER + #if EXTRUDERS > 1 stepper_extruder = current_block->extruder; #endif @@ -2156,7 +2159,7 @@ uint32_t Stepper::block_phase_isr() { || TERN(MIXING_EXTRUDER, false, stepper_extruder != last_moved_extruder) ) { last_direction_bits = current_block->direction_bits; - #if HAS_MULTI_EXTRUDER + #if EXTRUDERS > 1 last_moved_extruder = stepper_extruder; #endif @@ -2590,7 +2593,7 @@ void Stepper::init() { set_directions(); - #if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM + #if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM initialized = true; digipot_init(); #endif @@ -2930,10 +2933,10 @@ void Stepper::report_positions() { * Software-controlled Stepper Motor Current */ -#if HAS_MOTOR_CURRENT_SPI +#if HAS_DIGIPOTSS // From Arduino DigitalPotControl example - void Stepper::set_digipot_value_spi(const int16_t address, const int16_t value) { + void Stepper::digitalPotWrite(const int16_t address, const int16_t value) { WRITE(DIGIPOTSS_PIN, LOW); // Take the SS pin low to select the chip SPI.transfer(address); // Send the address and value via SPI SPI.transfer(value); @@ -2941,7 +2944,7 @@ void Stepper::report_positions() { //delay(10); } -#endif // HAS_MOTOR_CURRENT_SPI +#endif // HAS_DIGIPOTSS #if HAS_MOTOR_CURRENT_PWM @@ -2958,7 +2961,7 @@ void Stepper::report_positions() { #if ANY_PIN(MOTOR_CURRENT_PWM_E, MOTOR_CURRENT_PWM_E0, MOTOR_CURRENT_PWM_E1) case 2: #endif - set_digipot_current(i, motor_current_setting[i]); + digipot_current(i, motor_current_setting[i]); default: break; } } @@ -2968,23 +2971,23 @@ void Stepper::report_positions() { #if !MB(PRINTRBOARD_G2) - #if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM + #if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM - void Stepper::set_digipot_current(const uint8_t driver, const int16_t current) { + void Stepper::digipot_current(const uint8_t driver, const int16_t current) { if (WITHIN(driver, 0, COUNT(motor_current_setting) - 1)) motor_current_setting[driver] = current; // update motor_current_setting if (!initialized) return; - #if HAS_MOTOR_CURRENT_SPI - - //SERIAL_ECHOLNPAIR("Digipotss current ", current); - + #if HAS_DIGIPOTSS + SERIAL_ECHOLNPAIR("Digipotss current ", current); const uint8_t digipot_ch[] = DIGIPOT_CHANNELS; - set_digipot_value_spi(digipot_ch[driver], current); + digitalPotWrite(digipot_ch[driver], current); #elif HAS_MOTOR_CURRENT_PWM + + #define _WRITE_CURRENT_PWM(P) analogWrite(pin_t(MOTOR_CURRENT_PWM_## P ##_PIN), 255L * current / (MOTOR_CURRENT_PWM_RANGE)) switch (driver) { case 0: @@ -3020,13 +3023,15 @@ void Stepper::report_positions() { void Stepper::digipot_init() { - #if HAS_MOTOR_CURRENT_SPI + #if HAS_DIGIPOTSS SPI.begin(); SET_OUTPUT(DIGIPOTSS_PIN); - LOOP_L_N(i, COUNT(motor_current_setting)) - set_digipot_current(i, motor_current_setting[i]); + LOOP_L_N(i, COUNT(motor_current_setting)) { + //digitalPotWrite(digipot_ch[i], digipot_motor_current[i]); + digipot_current(i, motor_current_setting[i]); + } #elif HAS_MOTOR_CURRENT_PWM diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 572c3f3f9b..51f7c489d7 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -245,25 +245,16 @@ class Stepper { static bool separate_multi_axis; #endif - #if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM - #if HAS_MOTOR_CURRENT_PWM - #ifndef PWM_MOTOR_CURRENT - #define PWM_MOTOR_CURRENT DEFAULT_PWM_MOTOR_CURRENT - #endif - #define MOTOR_CURRENT_COUNT 3 - #elif HAS_MOTOR_CURRENT_SPI - static constexpr uint32_t digipot_count[] = DIGIPOT_MOTOR_CURRENT; - #define MOTOR_CURRENT_COUNT COUNT(Stepper::digipot_count) + #if HAS_MOTOR_CURRENT_PWM + #ifndef PWM_MOTOR_CURRENT + #define PWM_MOTOR_CURRENT DEFAULT_PWM_MOTOR_CURRENT #endif static bool initialized; - static uint32_t motor_current_setting[MOTOR_CURRENT_COUNT]; // Initialized by settings.load() - #endif - - // Last-moved extruder, as set when the last movement was fetched from planner - #if HAS_MULTI_EXTRUDER - static uint8_t last_moved_extruder; - #else - static constexpr uint8_t last_moved_extruder = 0; + static uint32_t motor_current_setting[3]; + #elif HAS_DIGIPOTSS + static bool initialized; + static constexpr uint32_t digipot_count[] = DIGIPOT_MOTOR_CURRENT; + static uint32_t motor_current_setting[COUNT(digipot_count)]; // Initialized by settings.load() #endif private: @@ -275,6 +266,13 @@ class Stepper { static bool abort_current_block; // Signals to the stepper that current block should be aborted + // Last-moved extruder, as set when the last movement was fetched from planner + #if EXTRUDERS < 2 + static constexpr uint8_t last_moved_extruder = 0; + #elif DISABLED(MIXING_EXTRUDER) + static uint8_t last_moved_extruder; + #endif + #if ENABLED(X_DUAL_ENDSTOPS) static bool locked_X_motor, locked_X2_motor; #endif @@ -310,7 +308,7 @@ class Stepper { decelerate_after, // The point from where we need to start decelerating step_event_count; // The total event count for the current block - #if EITHER(HAS_MULTI_EXTRUDER, MIXING_EXTRUDER) + #if EXTRUDERS > 1 || ENABLED(MIXING_EXTRUDER) static uint8_t stepper_extruder; #else static constexpr uint8_t stepper_extruder = 0; @@ -457,15 +455,20 @@ class Stepper { // The last movement direction was not null on the specified axis. Note that motor direction is not necessarily the same. FORCE_INLINE static bool axis_is_moving(const AxisEnum axis) { return TEST(axis_did_move, axis); } + // The extruder associated to the last movement + FORCE_INLINE static uint8_t movement_extruder() { + return (EXTRUDERS > 1 && DISABLED(MIXING_EXTRUDER)) ? last_moved_extruder : 0; + } + // Handle a triggered endstop static void endstop_triggered(const AxisEnum axis); // Triggered position of an axis in steps static int32_t triggered_position(const AxisEnum axis); - #if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM - static void set_digipot_value_spi(const int16_t address, const int16_t value); - static void set_digipot_current(const uint8_t driver, const int16_t current); + #if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM + static void digitalPotWrite(const int16_t address, const int16_t value); + static void digipot_current(const uint8_t driver, const int16_t current); #endif #if HAS_MICROSTEPS @@ -588,7 +591,7 @@ class Stepper { static int32_t _eval_bezier_curve(const uint32_t curr_step); #endif - #if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM + #if HAS_MOTOR_CURRENT_PWM || HAS_DIGIPOTSS static void digipot_init(); #endif