Tool sensors (MarlinFirmware#17239)

CR30-Users · Apr 16, 2021 · 553487c · 553487c
1 parent 73f6426
commit 553487c
Show file tree

Hide file tree

Showing 7 changed files with 174 additions and 28 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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 <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
@@ -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
@@ -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
@@ -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