Tool sensors (#17239)

This commit is contained in:
MangaValk 2021-04-16 15:02:31 +02:00 committed by Scott Lahteine
parent 791edd0cec
commit 2e726c46c8
7 changed files with 174 additions and 28 deletions

View file

@ -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.

View file

@ -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());

View file

@ -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);

View file

@ -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, " }");

View file

@ -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.

View file

@ -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

View file

@ -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