Basic automated ATC implementations (Draft/WIP)

FluidNC/src/ToolChangers/atc_basic.cpp

@@ -0,0 +1,151 @@
+// Copyright (c) 2024 -	Bart Dring
+// Use of this source code is governed by a GPLv3 license that can be found in the LICENSE file.
+
+#include "atc_basic.h"
+#include "../Machine/MachineConfig.h"
+#include <cstdio>
+#include <iostream>
+
+namespace ATCs {
+    void Basic_ATC::init() {
+        log_info("ATC:" << name());
+    }
+
+    void Basic_ATC::probe_notification() {}
+
+    bool Basic_ATC::tool_change(uint8_t new_tool, bool pre_select, bool set_tool) {
+        protocol_buffer_synchronize();  // wait for all motion to complete
+        _macro.erase();                 // clear previous gcode
+
+        if (pre_select) {  // not implemented
+            return true;
+        }
+
+        // save current location, so we can return after the tool change.
+        _macro.addf("#<start_x >= #<_x>");
+        _macro.addf("#<start_y >= #<_y>");
+        _macro.addf("#<start_z >= #<_z>");
+
+        //save machine states
+        Macro set_state;
+        Macro restore_state;
+        set_state.addf("M9");  // Disable coolant
+        if (gc_state.modal.coolant.Mist) {
+            restore_state.addf("M7");
+        }
+        if (gc_state.modal.coolant.Flood) {
+            restore_state.addf("M8");
+        }
+        set_state.addf("G21");  // become Metric
+        if (gc_state.modal.units == Units::Inches) {
+            restore_state.addf("G20");
+        }
+        set_state.addf("G90");  // become absolute
+        if (gc_state.modal.distance != Distance::Absolute) {
+            restore_state.addf("G91");
+        }
+        set_state.addf("M5");  // turn off the spindle
+        if (gc_state.modal.spindle != SpindleState::Disable) {
+            restore_state.addf("M3");
+        }
+
+        _macro.addf(set_state._gcode.c_str());
+
+        // set_tool is used to update the current tool and reset the TLO to 0
+        // if we dont have_tool_setter_offset then we also do an initial probe
+        if (set_tool) {
+            _prev_tool = new_tool;
+            _macro.addf("G4P0 0.0");
+            if (!_have_tool_setter_offset) {
+                get_ets_offset();
+            }
+            _macro.addf(restore_state._gcode.c_str());
+            move_to_start_position();
+            _macro.run(nullptr);
+            return true;
+        }
+
+        try {
+            if (_prev_tool > 0) {
+                // return tool
+                move_to_tool_position(_prev_tool);
+                _macro.addf(_toolreturn_macro._gcode.c_str()); // use macro with G91 movements or the _tc_tool_* variables to to return tool, operating the ATC using M62 & M63
+                _macro.addf(set_state._gcode.c_str()); // ensure the previous user macro didn't change modes
+            }
+
+            if (new_tool > 0) {
+                //pickup tool, if this is the 1st pickup ever, we also probe the tool_setter_offset
+                move_to_tool_position(_prev_tool);
+                _macro.addf(_toolpickup_macro._gcode.c_str()); // use macro with G91 movements or the _tc_tool_* variables to to pickup tool, operating the ATC using M62 & M63
+                _macro.addf(set_state._gcode.c_str()); // ensure the previous user macro didn't change modes
+                if (!_have_tool_setter_offset) {
+                    get_ets_offset();
+                }
+            }
+
+            // probe the new tool
+            ets_probe();
+            // TLO is simply the difference between the tool1 probe and the new tool probe.
+            _macro.addf("#<_my_tlo_z >=[#5063 - #<_ets_tool1_z>]");
+            _macro.addf("G43.1Z#<_my_tlo_z>");
+
+            move_to_start_position();
+
+            _macro.addf(restore_state._gcode.c_str());
+            _macro.run(nullptr);
+
+            return true;
+        } catch (...) { log_info("Exception caught"); }
+
+        return false;
+    }
+
+    void  Basic_ATC::move_to_start_position(){
+        // return to location before the tool change
+        move_to_safe_z();
+        _macro.addf("G0 X#<start_x>Y#<start_y>");
+        _macro.addf("G0 Z#<start_z>");
+    }
+
+    void Basic_ATC::move_to_tool_position(uint8_t tool_index) {
+        tool_index -= 1;
+        move_to_safe_z();
+        _macro.addf("G53 G0 X%0.3f Y%0.3f", _tool_mpos[tool_index][X_AXIS], _tool_mpos[tool_index][Y_AXIS]);
+        _macro.addf("#<_tc_tool_x >=%0.3f", _tool_mpos[tool_index][X_AXIS]);
+        _macro.addf("#<_tc_tool_y >=%0.3f", _tool_mpos[tool_index][Y_AXIS]);
+        _macro.addf("#<_tc_tool_z >=%0.3f", _tool_mpos[tool_index][Z_AXIS]);
+    }
+
+    void Basic_ATC::move_to_safe_z() {
+        _macro.addf("G53 G0 Z%0.3f", _safe_z);
+    }
+
+    void Basic_ATC::move_over_toolsetter() {
+        move_to_safe_z();
+        _macro.addf("G53 G0 X%0.3fY%0.3f", _ets_mpos[0], _ets_mpos[1]);
+    }
+
+    void Basic_ATC::get_ets_offset() {
+        ets_probe();
+        _macro.addf("#<_ets_tool1_z>=[#5063]");  // save the value of the tool1 ETS Z
+        _have_tool_setter_offset = true;
+    }
+
+    void Basic_ATC::ets_probe() {
+        move_to_safe_z();
+        move_over_toolsetter();
+        _macro.addf("G53 G0 Z #</ atc_manual / ets_rapid_z_mpos_mm>");  // rapid down
+
+        // do a fast probe if there is a seek that is faster than feed
+        if (_probe_seek_rate > _probe_feed_rate) {
+            _macro.addf("G53 G38.2 Z%0.3f F%0.3f", _ets_mpos[2], _probe_seek_rate);
+            _macro.addf("G0 Z[#<_z> + 5]");  // retract before next probe
+        }
+        // do the feed rate probe
+        _macro.addf("G53 G38.2 Z%0.3f F%0.3f", _ets_mpos[2], _probe_feed_rate);
+    }
+
+    namespace {
+        ATCFactory::InstanceBuilder<Basic_ATC> registration("atc_basic");
+    }
+}

FluidNC/src/ToolChangers/atc_basic.h

@@ -0,0 +1,79 @@
+// Copyright (c) 2024 -	Bart Dring
+// Use of this source code is governed by a GPLv3 license that can be found in the LICENSE file.
+
+#pragma once
+
+#include "src/Config.h"
+
+#include "src/Configuration/Configurable.h"
+
+#include "src/Channel.h"
+#include "src/Module.h"
+#include "atc.h"
+#include "../Machine/Macros.h"
+
+namespace ATCs {
+    const int TOOL_COUNT = 8;
+
+    class Basic_ATC : public ATC {
+    public:
+        Basic_ATC(const char* name) : ATC(name) {}
+
+        Basic_ATC(const Basic_ATC&)            = delete;
+        Basic_ATC(Basic_ATC&&)                 = delete;
+        Basic_ATC& operator=(const Basic_ATC&) = delete;
+        Basic_ATC& operator=(Basic_ATC&&)      = delete;
+
+        virtual ~Basic_ATC() = default;
+
+    private:
+        // config items
+        float              _safe_z           = 50.0;
+        float              _probe_seek_rate  = 200.0;
+        float              _probe_feed_rate  = 80.0;
+        std::vector<float> _ets_mpos         = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
+        float              _ets_rapid_z_mpos = 0;
+        std::vector<float> _tool_mpos[TOOL_COUNT];
+
+        uint8_t _prev_tool               = 0;  // TODO This could be a NV setting
+        bool    _have_tool_setter_offset = false;
+        float   _tool_setter_offset      = 0.0;  // have we established an offset.
+        float   _tool_setter_position[MAX_N_AXIS];
+
+        void  move_to_safe_z();
+        void  move_over_toolsetter();
+        void  ets_probe();
+        void  get_ets_offset();
+        void  move_to_tool_position(uint8_t tool_index);
+        void  move_to_start_position();
+
+        Macro _macro;
+        Macro _toolreturn_macro;
+        Macro _toolpickup_macro;
+
+    public:
+        void init() override;
+        void probe_notification() override;
+        bool tool_change(uint8_t value, bool pre_select, bool set_tool) override;
+
+        void validate() override {}
+
+        void group(Configuration::HandlerBase& handler) override {
+            handler.item("safe_z_mpos_mm", _safe_z, -100000, 100000);
+            handler.item("probe_seek_rate_mm_per_min", _probe_seek_rate, 1, 10000);
+            handler.item("probe_feed_rate_mm_per_min", _probe_feed_rate, 1, 10000);
+            handler.item("ets_mpos_mm", _ets_mpos);
+            handler.item("ets_rapid_z_mpos_mm", _ets_rapid_z_mpos);
+            handler.item("toolreturn_macro", _toolreturn_macro);
+            handler.item("toolpickup_macro", _toolpickup_macro);
+            handler.item("tool1_mpos_mm", _tool_mpos[0]);
+            handler.item("tool2_mpos_mm", _tool_mpos[1]);
+            handler.item("tool3_mpos_mm", _tool_mpos[2]);
+            handler.item("tool4_mpos_mm", _tool_mpos[3]);
+            handler.item("tool5_mpos_mm", _tool_mpos[4]);
+            handler.item("tool6_mpos_mm", _tool_mpos[5]);
+            handler.item("tool7_mpos_mm", _tool_mpos[6]);
+            handler.item("tool8_mpos_mm", _tool_mpos[7]);
+        }
+    };
+}