From 553487cc8ab348939c6033910c0aa11d230b2ad6 Mon Sep 17 00:00:00 2001 From: MangaValk Date: Fri, 16 Apr 2021 15:02:31 +0200 Subject: [PATCH] Tool sensors (#17239) --- Marlin/Configuration_adv.h | 6 + Marlin/src/MarlinCore.cpp | 7 +- Marlin/src/lcd/menu/menu_configuration.cpp | 1 + Marlin/src/module/tool_change.cpp | 156 +++++++++++++++++--- Marlin/src/module/tool_change.h | 9 +- Marlin/src/pins/stm32f4/pins_BTT_GTR_V1_0.h | 16 +- buildroot/tests/BIGTREE_GTR_V1_0 | 7 + 7 files changed, 174 insertions(+), 28 deletions(-) diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 578bf144fc..02a5ff0eeb 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -2198,6 +2198,12 @@ //#define EVENT_GCODE_AFTER_TOOLCHANGE "G12X" // Extra G-code to run after tool-change #endif + /** + * Tool Sensors detect when tools have been picked up or dropped. + * Requires the pins TOOL_SENSOR1_PIN, TOOL_SENSOR2_PIN, etc. + */ + //#define TOOL_SENSOR + /** * Retract and prime filament on tool-change to reduce * ooze and stringing and to get cleaner transitions. diff --git a/Marlin/src/MarlinCore.cpp b/Marlin/src/MarlinCore.cpp index 842429a407..2e1eab86de 100644 --- a/Marlin/src/MarlinCore.cpp +++ b/Marlin/src/MarlinCore.cpp @@ -210,9 +210,7 @@ #include "feature/fanmux.h" #endif -#if DO_SWITCH_EXTRUDER || ANY(SWITCHING_NOZZLE, PARKING_EXTRUDER, MAGNETIC_PARKING_EXTRUDER, ELECTROMAGNETIC_SWITCHING_TOOLHEAD, SWITCHING_TOOLHEAD) - #include "module/tool_change.h" -#endif +#include "module/tool_change.h" #if ENABLED(USE_CONTROLLER_FAN) #include "feature/controllerfan.h" @@ -731,6 +729,9 @@ void idle(TERN_(ADVANCED_PAUSE_FEATURE, bool no_stepper_sleep/*=false*/)) { // Return if setup() isn't completed if (marlin_state == MF_INITIALIZING) goto IDLE_DONE; + // TODO: Still causing errors + (void)check_tool_sensor_stats(active_extruder, true); + // Handle filament runout sensors TERN_(HAS_FILAMENT_SENSOR, runout.run()); diff --git a/Marlin/src/lcd/menu/menu_configuration.cpp b/Marlin/src/lcd/menu/menu_configuration.cpp index 7ef49f0cf8..7ea355b795 100644 --- a/Marlin/src/lcd/menu/menu_configuration.cpp +++ b/Marlin/src/lcd/menu/menu_configuration.cpp @@ -130,6 +130,7 @@ void menu_advanced_settings(); #if ENABLED(TOOLCHANGE_MIGRATION_FEATURE) #include "../../module/motion.h" // for active_extruder + #include "../../gcode/queue.h" void menu_toolchange_migration() { PGM_P const msg_migrate = GET_TEXT(MSG_TOOL_MIGRATION_SWAP); diff --git a/Marlin/src/module/tool_change.cpp b/Marlin/src/module/tool_change.cpp index 080c1e629b..2b8a434172 100644 --- a/Marlin/src/module/tool_change.cpp +++ b/Marlin/src/module/tool_change.cpp @@ -49,10 +49,14 @@ bool toolchange_extruder_ready[EXTRUDERS]; #endif -#if ENABLED(MAGNETIC_PARKING_EXTRUDER) || defined(EVENT_GCODE_AFTER_TOOLCHANGE) || (ENABLED(PARKING_EXTRUDER) && PARKING_EXTRUDER_SOLENOIDS_DELAY > 0) +#if EITHER(MAGNETIC_PARKING_EXTRUDER, TOOL_SENSOR) || defined(EVENT_GCODE_AFTER_TOOLCHANGE) || (ENABLED(PARKING_EXTRUDER) && PARKING_EXTRUDER_SOLENOIDS_DELAY > 0) #include "../gcode/gcode.h" #endif +#if ENABLED(TOOL_SENSOR) + #include "../lcd/marlinui.h" +#endif + #if ENABLED(DUAL_X_CARRIAGE) #include "stepper.h" #endif @@ -147,11 +151,11 @@ #endif // SWITCHING_NOZZLE -inline void _line_to_current(const AxisEnum fr_axis, const float fscale=1) { +void _line_to_current(const AxisEnum fr_axis, const float fscale=1) { line_to_current_position(planner.settings.max_feedrate_mm_s[fr_axis] * fscale); } -inline void slow_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_axis, 0.5f); } -inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_axis); } +void slow_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_axis, 0.2f); } +void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_axis, 0.5f); } #if ENABLED(MAGNETIC_PARKING_EXTRUDER) @@ -370,7 +374,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a DEBUG_POS("PE Tool-Change done.", current_position); parking_extruder_set_parked(false); } - else if (do_solenoid_activation) { // && nomove == true + else if (do_solenoid_activation) { // Deactivate current extruder solenoid pe_solenoid_set_pin_state(active_extruder, !PARKING_EXTRUDER_SOLENOIDS_PINS_ACTIVE); // Engage new extruder magnetic field @@ -384,12 +388,117 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a #if ENABLED(SWITCHING_TOOLHEAD) - inline void swt_lock(const bool locked=true) { - const uint16_t swt_angles[2] = SWITCHING_TOOLHEAD_SERVO_ANGLES; - MOVE_SERVO(SWITCHING_TOOLHEAD_SERVO_NR, swt_angles[locked ? 0 : 1]); + // Return a bitmask of tool sensor states + inline uint8_t poll_tool_sensor_pins() { + return (0 + #if ENABLED(TOOL_SENSOR) + #if PIN_EXISTS(TOOL_SENSOR1) + | (READ(TOOL_SENSOR1_PIN) << 0) + #endif + #if PIN_EXISTS(TOOL_SENSOR2) + | (READ(TOOL_SENSOR2_PIN) << 1) + #endif + #if PIN_EXISTS(TOOL_SENSOR3) + | (READ(TOOL_SENSOR3_PIN) << 2) + #endif + #if PIN_EXISTS(TOOL_SENSOR4) + | (READ(TOOL_SENSOR4_PIN) << 3) + #endif + #if PIN_EXISTS(TOOL_SENSOR5) + | (READ(TOOL_SENSOR5_PIN) << 4) + #endif + #if PIN_EXISTS(TOOL_SENSOR6) + | (READ(TOOL_SENSOR6_PIN) << 5) + #endif + #if PIN_EXISTS(TOOL_SENSOR7) + | (READ(TOOL_SENSOR7_PIN) << 6) + #endif + #if PIN_EXISTS(TOOL_SENSOR8) + | (READ(TOOL_SENSOR8_PIN) << 7) + #endif + #endif + ); + } + + #if ENABLED(TOOL_SENSOR) + + bool tool_sensor_disabled; // = false + + uint8_t check_tool_sensor_stats(const uint8_t tool_index, const bool kill_on_error/*=false*/, const bool disable/*=false*/) { + static uint8_t sensor_tries; // = 0 + for (;;) { + if (poll_tool_sensor_pins() == _BV(tool_index)) { + sensor_tries = 0; + return tool_index; + } + else if (kill_on_error && (!tool_sensor_disabled || disable)) { + sensor_tries++; + if (sensor_tries > 10) kill(PSTR("Tool Sensor error")); + safe_delay(5); + } + else { + sensor_tries++; + if (sensor_tries > 10) return -1; + safe_delay(5); + } + } + } + + #endif + + inline void switching_toolhead_lock(const bool locked) { + #ifdef SWITCHING_TOOLHEAD_SERVO_ANGLES + const uint16_t swt_angles[2] = SWITCHING_TOOLHEAD_SERVO_ANGLES; + MOVE_SERVO(SWITCHING_TOOLHEAD_SERVO_NR, swt_angles[locked ? 0 : 1]); + #elif PIN_EXISTS(SWT_SOLENOID) + OUT_WRITE(SWT_SOLENOID_PIN, locked); + gcode.dwell(10); + #else + #error "No toolhead locking mechanism configured." + #endif } - void swt_init() { swt_lock(); } + #include + + void swt_init() { + switching_toolhead_lock(true); + + #if ENABLED(TOOL_SENSOR) + // Init tool sensors + #if PIN_EXISTS(TOOL_SENSOR1) + SET_INPUT_PULLUP(TOOL_SENSOR1_PIN); + #endif + #if PIN_EXISTS(TOOL_SENSOR2) + SET_INPUT_PULLUP(TOOL_SENSOR2_PIN); + #endif + #if PIN_EXISTS(TOOL_SENSOR3) + SET_INPUT_PULLUP(TOOL_SENSOR3_PIN); + #endif + #if PIN_EXISTS(TOOL_SENSOR4) + SET_INPUT_PULLUP(TOOL_SENSOR4_PIN); + #endif + #if PIN_EXISTS(TOOL_SENSOR5) + SET_INPUT_PULLUP(TOOL_SENSOR5_PIN); + #endif + #if PIN_EXISTS(TOOL_SENSOR6) + SET_INPUT_PULLUP(TOOL_SENSOR6_PIN); + #endif + #if PIN_EXISTS(TOOL_SENSOR7) + SET_INPUT_PULLUP(TOOL_SENSOR7_PIN); + #endif + #if PIN_EXISTS(TOOL_SENSOR8) + SET_INPUT_PULLUP(TOOL_SENSOR8_PIN); + #endif + + if (check_tool_sensor_stats(0)) { + ui.set_status_P("TC error"); + switching_toolhead_lock(false); + while (check_tool_sensor_stats(0)) { /* nada */ } + switching_toolhead_lock(true); + } + ui.set_status_P("TC Success"); + #endif + } inline void switching_toolhead_tool_change(const uint8_t new_tool, bool no_move/*=false*/) { if (no_move) return; @@ -398,6 +507,8 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a const float placexpos = toolheadposx[active_extruder], grabxpos = toolheadposx[new_tool]; + (void)check_tool_sensor_stats(active_extruder, true); + /** * 1. Move to switch position of current toolhead * 2. Unlock tool and drop it in the dock @@ -421,13 +532,14 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a DEBUG_SYNCHRONIZE(); DEBUG_POS("Move Y SwitchPos + Security", current_position); - fast_line_to_current(Y_AXIS); + slow_line_to_current(Y_AXIS); // 2. Unlock tool and drop it in the dock + TERN_(TOOL_SENSOR, tool_sensor_disabled = true); planner.synchronize(); DEBUG_ECHOLNPGM("(2) Unlock and Place Toolhead"); - swt_lock(false); + switching_toolhead_lock(false); safe_delay(500); current_position.y = SWITCHING_TOOLHEAD_Y_POS; @@ -440,7 +552,9 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a current_position.y -= SWITCHING_TOOLHEAD_Y_CLEAR; DEBUG_POS("Move back Y clear", current_position); - fast_line_to_current(Y_AXIS); // move away from docked toolhead + slow_line_to_current(Y_AXIS); // move away from docked toolhead + + (void)check_tool_sensor_stats(active_extruder); // 3. Move to the new toolhead @@ -457,7 +571,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a DEBUG_SYNCHRONIZE(); DEBUG_POS("Move Y SwitchPos + Security", current_position); - fast_line_to_current(Y_AXIS); + slow_line_to_current(Y_AXIS); // 4. Grab and lock the new toolhead @@ -472,14 +586,19 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a // Wait for move to finish, pause 0.2s, move servo, pause 0.5s planner.synchronize(); safe_delay(200); - swt_lock(); + + (void)check_tool_sensor_stats(new_tool, true, true); + + switching_toolhead_lock(true); safe_delay(500); current_position.y -= SWITCHING_TOOLHEAD_Y_CLEAR; DEBUG_POS("Move back Y clear", current_position); - fast_line_to_current(Y_AXIS); // Move away from docked toolhead + slow_line_to_current(Y_AXIS); // Move away from docked toolhead planner.synchronize(); // Always sync the final move + (void)check_tool_sensor_stats(new_tool, true, true); + DEBUG_POS("ST Tool-Change done.", current_position); } @@ -1053,8 +1172,11 @@ void tool_change(const uint8_t new_tool, bool no_move/*=false*/) { move_nozzle_servo(new_tool); #endif - // Set the new active extruder - if (DISABLED(DUAL_X_CARRIAGE)) active_extruder = new_tool; + IF_DISABLED(DUAL_X_CARRIAGE, active_extruder = new_tool); // Set the new active extruder + + TERN_(TOOL_SENSOR, tool_sensor_disabled = false); + + (void)check_tool_sensor_stats(active_extruder, true); // The newly-selected extruder XYZ is actually at... DEBUG_ECHOLNPAIR("Offset Tool XYZ by { ", diff.x, ", ", diff.y, ", ", diff.z, " }"); diff --git a/Marlin/src/module/tool_change.h b/Marlin/src/module/tool_change.h index b79ec676a0..bbdc0b6862 100644 --- a/Marlin/src/module/tool_change.h +++ b/Marlin/src/module/tool_change.h @@ -79,10 +79,9 @@ #if ENABLED(PARKING_EXTRUDER) - #define PE_MAGNET_ON_STATE TERN_(PARKING_EXTRUDER_SOLENOIDS_INVERT, !)PARKING_EXTRUDER_SOLENOIDS_PINS_ACTIVE - void pe_solenoid_set_pin_state(const uint8_t extruder_num, const uint8_t state); + #define PE_MAGNET_ON_STATE TERN_(PARKING_EXTRUDER_SOLENOIDS_INVERT, !)PARKING_EXTRUDER_SOLENOIDS_PINS_ACTIVE inline void pe_solenoid_magnet_on(const uint8_t extruder_num) { pe_solenoid_set_pin_state(extruder_num, PE_MAGNET_ON_STATE); } inline void pe_solenoid_magnet_off(const uint8_t extruder_num) { pe_solenoid_set_pin_state(extruder_num, !PE_MAGNET_ON_STATE); } @@ -115,6 +114,12 @@ void swt_init(); #endif +#if ENABLED(TOOL_SENSOR) + uint8_t check_tool_sensor_stats(const uint8_t active_tool, const bool kill_on_error=false, const bool disable=false); +#else + inline uint8_t check_tool_sensor_stats(const uint8_t, const bool=false, const bool=false) { return 0; } +#endif + /** * Perform a tool-change, which may result in moving the * previous tool out of the way and the new tool into place. diff --git a/Marlin/src/pins/stm32f4/pins_BTT_GTR_V1_0.h b/Marlin/src/pins/stm32f4/pins_BTT_GTR_V1_0.h index 6c59f27c5e..cae1261a81 100644 --- a/Marlin/src/pins/stm32f4/pins_BTT_GTR_V1_0.h +++ b/Marlin/src/pins/stm32f4/pins_BTT_GTR_V1_0.h @@ -38,17 +38,21 @@ // USB Flash Drive support #define HAS_OTG_USB_HOST_SUPPORT -#define TP // Enable to define servo and probe pins #define M5_EXTENDER // The M5 extender is attached // // Servos // -#if ENABLED(TP) - #define SERVO0_PIN PB11 -#endif +#define SERVO0_PIN PB11 // BLTOUCH +#define SOL0_PIN PC7 // Toolchanger -#define PS_ON_PIN PH6 +#if ENABLED(TOOL_SENSOR) + #define TOOL_SENSOR1_PIN PH6 + #define TOOL_SENSOR2_PIN PI4 + //#define TOOL_SENSOR3_PIN PF4 +#else + #define PS_ON_PIN PH6 +#endif // // Trinamic Stallguard pins @@ -110,7 +114,7 @@ #define Z4_STOP_PIN PF6 // M5 M3_STOP #endif -#if ENABLED(TP) && !defined(Z_MIN_PROBE_PIN) +#ifndef Z_MIN_PROBE_PIN #define Z_MIN_PROBE_PIN PH11 // Z Probe must be PH11 #endif diff --git a/buildroot/tests/BIGTREE_GTR_V1_0 b/buildroot/tests/BIGTREE_GTR_V1_0 index 95a1e0acac..24293a4932 100755 --- a/buildroot/tests/BIGTREE_GTR_V1_0 +++ b/buildroot/tests/BIGTREE_GTR_V1_0 @@ -26,5 +26,12 @@ opt_set MOTHERBOARD BOARD_BTT_GTR_V1_0 SERIAL_PORT -1 \ opt_enable TOOLCHANGE_FILAMENT_SWAP TOOLCHANGE_MIGRATION_FEATURE TOOLCHANGE_FS_INIT_BEFORE_SWAP TOOLCHANGE_FS_PRIME_FIRST_USED PID_PARAMS_PER_HOTEND exec_test $1 $2 "BigTreeTech GTR | 6 Extruders | Triple Z" "$3" +restore_configs +opt_set MOTHERBOARD BOARD_BTT_GTR_V1_0 SERIAL_PORT -1 \ + EXTRUDERS 3 TEMP_SENSOR_1 1 TEMP_SENSOR_2 1 \ + SERVO_DELAY '{ 300, 300, 300 }' +opt_enable SWITCHING_TOOLHEAD TOOL_SENSOR +exec_test $1 $2 "BigTreeTech GTR | Switching Toolhead | Tool Sensors" "$3" + # clean up restore_configs