From 2e726c46c87ea1707203b9019d14cf82872be9e8 Mon Sep 17 00:00:00 2001
From: MangaValk <patrickvalkmanga@hotmail.com>
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
+ );
}
- void swt_init() { swt_lock(); }
+ #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
+ }
+
+ #include <bitset>
+
+ 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