From 71e51c50335fa9cfa88ce466349f9eeb4d6ec713 Mon Sep 17 00:00:00 2001
From: Adrian Scripca <benishor@gmail.com>
Date: Sun, 7 May 2023 11:05:01 +0300
Subject: [PATCH] Implemented showing program extents

---
 docs/todo.txt    | 53 +++++++++++++++++++++++++++++
 grbl_machine.cpp | 18 ++++++++++
 grbl_machine.h   |  3 ++
 main.cpp         | 88 ++++++++++++++++++++++++++++++++++++++++++++++--
 4 files changed, 160 insertions(+), 2 deletions(-)
 create mode 100644 docs/todo.txt

diff --git a/docs/todo.txt b/docs/todo.txt
new file mode 100644
index 0000000..8c8d4a4
--- /dev/null
+++ b/docs/todo.txt
@@ -0,0 +1,53 @@
+Heightmap probing:
+    1. define the grid:
+        - from: x, y
+        - to: x, y
+        - step every: 5mm?
+        - clearance height: Z1.5
+        - start probing at: Z0.5
+        - max negative z: Z-0.5 (when to fail probing)
+        - z final safety height: Z15
+
+        - from and to can be filled from current project bounding box
+
+    2. probing
+        - machine moves at grid start position (x, y)
+            G0X0Y0
+        - gets down for initial probing
+            G38.2 Z-65 F100
+        - step back a bit (1mm, relative)
+            G91 G0 Z1
+        - seek again with lower feed rate
+            G38.2 Z-5 F5
+        - after first probe ever, this Z becomes reference 0
+            G10 L20 P0 Z0
+
+        - all subsequent probes will be relative to this
+        - for each probing location:
+            - G0 Z"clearance height"
+            - G0 XxxYyyy (next point location)
+            - G0 Z"start probing at"
+            - G38.2 Z-5 F5
+            - store Z offset and place it in the heightmap
+
+    3. modify loaded program with heightmap data
+        - foreach line in program
+            - if line contains a Z coordinate, update the Z according to the X and Y
+            - if line does not contain a Z coordinate, add a Z coordinate according to the X and Y
+
+Edge finding
+    - prerequisites
+        - which edge to find? -X/+X/-Y/+Y
+        - bit diameter used
+        - put the  
+
+Corner finding
+    - which corner are we probing: up left, up right, bottom left, bottom right
+    - need to know the bit diameter
+    - use G38.2 to probe X edge and set that as current X - diam/2
+
+
+DONE - Show program extents
+
+Render program extents
+    - show a blue bounding box for the program extents
diff --git a/grbl_machine.cpp b/grbl_machine.cpp
index 88942dc..aae2e8c 100644
--- a/grbl_machine.cpp
+++ b/grbl_machine.cpp
@@ -682,6 +682,12 @@ void grbl::machine::request_jog_fixed(grbl::jog_direction dir, float distance, f
     awaiting_responses++;
 }
 
+void grbl::machine::start_z_probe(float min_z, float feed_rate) {
+    std::string command = "G38.2 Z" + std::to_string(min_z) + "F" + std::to_string(feed_rate);
+    pipe->send(command);
+    awaiting_responses++;
+}
+
 bool grbl::jog_state::no_jogging() const {
     return !(up_pressed || down_pressed || left_pressed || right_pressed || z_up_pressed || z_down_pressed);
 }
@@ -812,6 +818,18 @@ void grbl::machine_state_idle::on_line_received(std::string line) {
         // TODO: some parameters have more than two :
         auto pieces = split_string(line, ":");
         cnc->parameters[pieces[0]] = pieces[1];
+
+        if (starts_with(line, "PRB")) {
+            auto pieces = split_string(line, ":");
+            auto coords_as_string = split_string(pieces[1], ",");
+
+            float probe_coords[3];
+            for (auto i = 0; i < 3; i++) {
+                probe_coords[i] = std::stof(coords_as_string[i]);
+            }
+            bool probe_touched = pieces[2] == "1";
+            cnc->listener->on_probe_result(probe_touched, probe_coords);
+        }
     }
 }
 
diff --git a/grbl_machine.h b/grbl_machine.h
index 407270b..57639c8 100644
--- a/grbl_machine.h
+++ b/grbl_machine.h
@@ -126,6 +126,7 @@ struct machine_listener {
     virtual void on_check_completed(bool success, size_t failed_index, size_t error) = 0;
     virtual void on_settings_reloaded() = 0;
     virtual void on_parameters_reloaded() = 0;
+    virtual void on_probe_result(bool probe_touched, float probe_coords[3]) = 0;
 };
 
 
@@ -277,6 +278,8 @@ struct machine : public transport_callbacks {
     // 0 = G54, 1 = G55, etc axis 0=X, 1=Y, 2=Z
     void zero_offset_axis(int offset_index, int axis);
 
+    void start_z_probe(float min_z, float feed_rate);
+
 protected:
     void on_connected(transport *transport) override;
     void on_disconnected(transport *transport) override;
diff --git a/main.cpp b/main.cpp
index 26c1488..6a7d1e7 100644
--- a/main.cpp
+++ b/main.cpp
@@ -98,16 +98,24 @@ public:
         });
 
         info_layer = tab_widget->add<Widget>();
-        tab_widget->append_tab("Info", info_layer);
-
         info_layer->set_layout(new GroupLayout(10, 20, 30, 0));
         info_layer->add<Label>("");
 
+        tab_widget->append_tab("Info", info_layer);
+
+        auto probe_layer = tab_widget->add<Widget>();
+        probe_layer->set_layout(new GroupLayout(10, 20, 30, 0));
+        probe_layer->add<Label>("");
+
+        tab_widget->append_tab("Probing", info_layer);
+
         add_status_markup();
         add_dro_markup();
+        add_program_extents();
         add_pins_markup();
         add_jogging_markup();
         add_work_parameters_markup();
+        add_z_probe_markup();
 
         perform_layout();
 
@@ -117,6 +125,33 @@ public:
         m_render_pass->set_cull_mode(RenderPass::CullMode::Disabled);
     }
 
+    TextBox *txt_min_z, *txt_feed;
+
+    void add_z_probe_markup() {
+        info_layer->add<Label>("Z-Probe", "sans-bold", 20);
+        auto z_probe_holder = info_layer->add<Widget>();
+        z_probe_holder->set_layout(new BoxLayout(Orientation::Vertical, Alignment::Fill, 2, 2));
+
+        auto another_holder = z_probe_holder->add<Widget>();
+        another_holder->set_layout(new GridLayout(Orientation::Horizontal, 2, Alignment::Fill, 2, 2));
+
+        another_holder->add<Label>("Feed", "sans-bold");
+        txt_feed = another_holder->add<TextBox>("100");
+        txt_feed->set_editable(true);
+        txt_feed->set_fixed_width(100);
+
+        another_holder->add<Label>("Min Z", "sans-bold");
+        txt_min_z = another_holder->add<TextBox>("-65");
+        txt_min_z->set_editable(true);
+        txt_min_z->set_fixed_width(100);
+
+        z_probe_holder->add<Label>("");
+        auto btn_start_probing = z_probe_holder->add<Button>("Start probing");
+        btn_start_probing->set_callback([&]() {
+            cnc.start_z_probe(std::stof(txt_min_z->value()), std::stof(txt_feed->value()));
+        });
+    }
+
     void add_work_parameters_markup() {
         // work parameters
         info_layer->add<Label>("Work parameters", "sans-bold", 20);
@@ -151,6 +186,7 @@ public:
 
             if (pgm.load_from_file(path)) {
                 init_program_geometry();
+                set_program_extents();
             } else {
             }
         });
@@ -383,6 +419,31 @@ public:
         });
     }
 
+    TextBox *extents_min_x, *extents_max_x;
+    TextBox *extents_min_y, *extents_max_y;
+    TextBox *extents_min_z, *extents_max_z;
+
+    void add_program_extents() {
+        dro_ss << std::setprecision(3) << std::fixed;
+
+        // Program extents
+        info_layer->add<Label>("Program extents", "sans-bold", 20);
+        auto extents_holder = info_layer->add<Widget>();
+        extents_holder->set_layout(new GridLayout(Orientation::Horizontal, 3, Alignment::Fill, 4, 4));
+
+        extents_holder->add<Label>("X", "sans-bold", 20);
+        extents_min_x = extents_holder->add<TextBox>("");
+        extents_max_x = extents_holder->add<TextBox>("");
+
+        extents_holder->add<Label>("Y", "sans-bold", 20);
+        extents_min_y = extents_holder->add<TextBox>("");
+        extents_max_y = extents_holder->add<TextBox>("");
+
+        extents_holder->add<Label>("Z", "sans-bold", 20);
+        extents_min_z = extents_holder->add<TextBox>("");
+        extents_max_z = extents_holder->add<TextBox>("");
+    }
+
     void add_pins_markup() {
         // Pins
         info_layer->add<Label>("Pins", "sans-bold", 20);
@@ -501,6 +562,20 @@ public:
         cam_src_rotation = glm::quat(1.0, 0.0, 0.0, 0.0);
     }
 
+    void set_program_extents() {
+        auto min = renderer.get_extents_min();
+        auto max = renderer.get_extents_max();
+
+        extents_min_x->set_value(std::to_string(min.x));
+        extents_max_x->set_value(std::to_string(max.x));
+
+        extents_min_y->set_value(std::to_string(min.y));
+        extents_max_y->set_value(std::to_string(max.y));
+
+        extents_min_z->set_value(std::to_string(min.z));
+        extents_max_z->set_value(std::to_string(max.z));
+    }
+
     bool resize_event(const Vector2i& size) override {
 //        window->set_fixed_height(this->height() / 2);
         tab_widget->set_fixed_height((this->height() - 30));
@@ -589,6 +664,15 @@ public:
         last_alarm = alarm;
     }
 
+    void on_probe_result(bool probe_touched, float *probe_coords) override {
+        std::stringstream ss;
+        ss << "Probing ended. Result: " << std::boolalpha << probe_touched << " at coords: ";
+        for (auto i = 0; i < 3; i++) {
+            ss << probe_coords[i] << ", ";
+        }
+        new MessageDialog(this, MessageDialog::Type::Warning, "Probe result", ss.str());
+    }
+
     void on_check_completed(bool success, size_t failed_index, size_t error) override {
         btn_check_program->set_background_color(success ? colGreen : color_red);
         if (success) {