From 3f9eb049b795953fe11b231317b5b03657c45bcd Mon Sep 17 00:00:00 2001
From: Adrian Scripca <benishor@gmail.com>
Date: Fri, 5 May 2023 15:42:30 +0300
Subject: [PATCH] Got rid of segfault when uploading VBOs. Added go-to-zero
 function. Reworked how work pos is obtained.

---
 grbl_machine.cpp | 51 +++++++++++++++++++++++++++++++++++++++++++-----
 grbl_machine.h   | 18 ++++++++++++++---
 main.cpp         | 21 ++++++++++----------
 render.cpp       | 21 +++++++++++++-------
 render.h         |  1 +
 5 files changed, 86 insertions(+), 26 deletions(-)

diff --git a/grbl_machine.cpp b/grbl_machine.cpp
index e2e9fac..d613260 100644
--- a/grbl_machine.cpp
+++ b/grbl_machine.cpp
@@ -1,5 +1,7 @@
 #include <iostream>
 #include <sstream>
+#include <utility>
+#include <array>
 #include "grbl_machine.h"
 #include "string_utils.h"
 
@@ -96,15 +98,29 @@ void grbl::machine::check_program(const grbl::program& pgm) {
     switch_to_state(grbl_machine_state::check_program);
 }
 
-void grbl::machine::run_program(const grbl::program& pgm, std::string work_offset) {
+void grbl::machine::set_work_offset(std::string work_offset) {
+    std::cout << "Setting work offsset " << work_offset << std::endl;
+//    if (state != grbl_machine_state::disconnected) {
+    pipe->send(work_offset);
+//    awaiting_responses++;
+//    while (awaiting_responses > 0);
+
+    current_work_offset = work_offset;
+
+    auto pieces = split_string(parameters[work_offset], ",");
+    current_work_offset_values[0] = std::stof(pieces[0]);
+    current_work_offset_values[1] = std::stof(pieces[1]);
+    current_work_offset_values[2] = std::stof(pieces[2]);
+//    }
+
+}
+
+void grbl::machine::run_program(const grbl::program& pgm, const std::string& work_offset) {
     running_program = pgm;
     std::cout << "running program (" << running_program.filename << ") with " << running_program.number_of_instructions() << " instructions"
               << " on work offset " << work_offset << std::endl;
 
-    // set desired offset
-    while (awaiting_responses > 0);
-    pipe->send(work_offset);
-    while (awaiting_responses > 0);
+    set_work_offset(work_offset);
 
     switch_to_state(grbl_machine_state::run_program);
 }
@@ -576,6 +592,31 @@ void grbl::machine::zero_offset_axis(int offset_index, int axis) {
     listener->on_parameters_reloaded();
 }
 
+void grbl::machine::go_to_zero(bool x, bool y, bool z) {
+    std::string command = "G0";
+    if (x) command += "X0";
+    if (y) command += "Y0";
+    if (z) command += "Z0";
+
+    while (awaiting_responses > 0);
+    pipe->send(command);
+    awaiting_responses++;
+    while (awaiting_responses > 0);
+}
+
+const float *grbl::machine::get_current_work_offset_values() const {
+    return current_work_offset_values;
+}
+
+std::array<float, 3> grbl::machine::get_work_pos() const {
+    std::array<float, 3> result = {
+            last_report.machine_pos[0] - current_work_offset_values[0],
+            last_report.machine_pos[1] - current_work_offset_values[1],
+            last_report.machine_pos[2] - current_work_offset_values[2],
+    };
+    return result;
+}
+
 bool grbl::jog_state::no_jogging() const {
     return !(up_pressed || down_pressed || left_pressed || right_pressed || z_up_pressed || z_down_pressed);
 }
diff --git a/grbl_machine.h b/grbl_machine.h
index 0c9fb9f..6493830 100644
--- a/grbl_machine.h
+++ b/grbl_machine.h
@@ -165,7 +165,7 @@ struct machine_state_init : public machine_state {
     init_stage init_state = init_stage::start;
     void move_to_next_init_stage();
 
-    machine* cnc;
+    machine *cnc;
 };
 
 struct machine_state_idle : public machine_state {
@@ -203,7 +203,7 @@ struct machine_state_run_program : public machine_state {
     bool continue_program();
     void move_to_next_run_stage();
 
-    machine* cnc;
+    machine *cnc;
     run_stage stage = run_stage::start;
     bool run_failed;
     size_t run_error;
@@ -228,7 +228,11 @@ struct machine : public transport_callbacks {
 
     void set_listener(grbl::machine_listener *listener);
     void connect();
-    void run_program(const grbl::program& pgm, std::string work_offset);
+
+    void set_work_offset(std::string work_offset);
+    std::array<float, 3> get_work_pos() const;
+
+    void run_program(const grbl::program& pgm, const std::string& work_offset);
     void check_program(const grbl::program& pgm);
     void request_jog(jog_state jog) const;
     void cancel_jog() const;
@@ -246,6 +250,8 @@ struct machine : public transport_callbacks {
     // 0 = G54, 1 = G55, etc
     void zero_offset(int which);
 
+    void go_to_zero(bool x, bool y, bool z);
+
     // 0 = G54, 1 = G55, etc axis 0=X, 1=Y, 2=Z
     void zero_offset_axis(int offset_index, int axis);
 
@@ -266,6 +272,12 @@ protected:
     std::map<std::string, std::string, settings_cmp> settings;
     std::map<std::string, std::string, parameters_cmp> parameters;
 
+    std::string current_work_offset;
+    float current_work_offset_values[3] = {0};
+public:
+    const float *get_current_work_offset_values() const;
+protected:
+
     volatile size_t awaiting_responses = 0;
     realtime_status_report last_report{};
     machine_listener *listener = nullptr;
diff --git a/main.cpp b/main.cpp
index 7e83359..6660248 100644
--- a/main.cpp
+++ b/main.cpp
@@ -73,9 +73,6 @@ public:
     Widget *parameters_layer;
     TextBox *mpos_x_text, *mpos_y_text, *mpos_z_text;
     ComboBox *cboOffset, *cboTool;
-    float offset_x = 0;
-    float offset_y = 0;
-    float offset_z = 0;
 
     SenderApp() : Screen(Vector2i(1024, 768), "GRBL Sender") {
         inc_ref();
@@ -165,6 +162,11 @@ public:
             cnc.zero_offset(cboOffset->selected_index());
         });
 
+        auto goto_zero_btn = mpos->add<Button>("Goto 0");
+        goto_zero_btn->set_callback([&]() {
+            cnc.go_to_zero(true, true, true);
+        });
+
         // work parameters
         layer->add<Label>("Work parameters", "sans-bold", 20);
         auto x = new Widget(layer);
@@ -262,11 +264,7 @@ public:
 
     void refresh_offset() {
         auto offset_name = "G" + std::to_string(cboOffset->selected_index() + 54);
-        auto offset_values = cnc.get_parameters().at(offset_name);
-        auto offset_pieces = split_string(offset_values, ",");
-        offset_x = std::stof(offset_pieces[0]);
-        offset_y = std::stof(offset_pieces[1]);
-        offset_z = std::stof(offset_pieces[2]);
+        cnc.set_work_offset(offset_name);
     }
 
     void fill_in_parameters() {
@@ -388,9 +386,10 @@ public:
     }
 
     void update_dro() {
-        mpos_x_text->set_value(std::to_string(cnc.get_status().machine_pos[0] - offset_x));
-        mpos_y_text->set_value(std::to_string(cnc.get_status().machine_pos[1] - offset_y));
-        mpos_z_text->set_value(std::to_string(cnc.get_status().machine_pos[2] - offset_z));
+        auto work_pos = cnc.get_work_pos();
+        mpos_x_text->set_value(std::to_string(work_pos[0]));
+        mpos_y_text->set_value(std::to_string(work_pos[1]));
+        mpos_z_text->set_value(std::to_string(work_pos[2]));
     }
 
     void on_banner(std::string line) override {
diff --git a/render.cpp b/render.cpp
index fc2ce4c..418ecc7 100644
--- a/render.cpp
+++ b/render.cpp
@@ -91,8 +91,13 @@ void grbl::program_renderer::update(const grbl::program& pgm, const grbl::machin
     // build vbo and vao
     vertices_count = build_vbo(pgm);
 
-    auto x = cnc.get_status().work_pos;
-    spindle_pos = glm::vec3(x[0], x[1], x[2]);
+    auto work_pos = cnc.get_status().machine_pos;
+    auto offsets = cnc.get_current_work_offset_values();
+    for (int i = 0; i < 3; i++) {
+        work_pos[i] -= offsets[i];
+    }
+
+    spindle_pos = glm::vec3(work_pos[0], work_pos[1], work_pos[2]);
 }
 
 void grbl::program_renderer::initialize_spindle_buffers() {
@@ -148,7 +153,7 @@ void grbl::program_renderer::initialize_spindle_buffers() {
     }
 
     glBindBuffer(GL_ARRAY_BUFFER, spindle_vbo_id);
-    glBufferData(GL_ARRAY_BUFFER, sizeOfVertexInBytes * buffer_data.size(), buffer_data.data(), GL_STATIC_DRAW);
+    glBufferData(GL_ARRAY_BUFFER, sizeof(float) * buffer_data.size(), buffer_data.data(), GL_STATIC_DRAW);
 
     spindle_vertices_count = buffer_data.size() * sizeof(float) / sizeOfVertexInBytes;
 }
@@ -178,7 +183,8 @@ GLsizei grbl::program_renderer::build_vbo(const grbl::program& pgm) {
     static auto movement_re = std::regex(R"(([gG]0*1?\s+|[xXyYzZ]\s*[0-9\.\-]+))");
     bool is_tool_on = false;
 
-    std::vector<float> buffer_data;
+
+    buffer_data.clear();
 
     glm::vec3 tool_pos;
     min_pos = max_pos = tool_pos = glm::vec3(0);
@@ -270,10 +276,11 @@ GLsizei grbl::program_renderer::build_vbo(const grbl::program& pgm) {
     }
 
     const GLsizei sizeOfVertexInBytes = 28;
-    glBindBuffer(GL_ARRAY_BUFFER, vbo_id);
-    glBufferData(GL_ARRAY_BUFFER, sizeOfVertexInBytes * buffer_data.size(), buffer_data.data(), GL_DYNAMIC_DRAW);
-
     auto number_of_vertices = buffer_data.size() * sizeof(float) / sizeOfVertexInBytes;
+
+    glBindBuffer(GL_ARRAY_BUFFER, vbo_id);
+    glBufferData(GL_ARRAY_BUFFER, sizeof(float) * buffer_data.size(), buffer_data.data(), GL_STATIC_DRAW);
+
     return number_of_vertices;
 }
 
diff --git a/render.h b/render.h
index 2788780..df2316c 100644
--- a/render.h
+++ b/render.h
@@ -27,6 +27,7 @@ private:
     GLuint vbo_id;
     GLuint vao_id;
 
+    std::vector<float> buffer_data;
     shader_program *shader = nullptr;
 
     bool initialized = false;