From 4e7e059e22bb4b3badd67e83952577ff5efa5de2 Mon Sep 17 00:00:00 2001 From: Adrian Scripca Date: Thu, 18 May 2023 07:24:05 +0300 Subject: [PATCH] Work on applying the probed heightmap. --- CMakeLists.txt | 2 +- gcode_commands.cpp | 14 ++-- gcode_commands.h | 6 +- gcode_file.cpp | 157 +++++++++++++++++++++++++++++++++++++++++++++ gcode_file.h | 18 ++++++ gcode_parser.cpp | 10 +-- gcode_parser.h | 2 +- grbl.cpp | 121 ++++++++++++++++++++++++++++++++-- grbl.h | 5 +- grbl_test.cpp | 4 +- render.cpp | 13 ++-- sender_app.cpp | 9 ++- sender_app.h | 10 ++- 13 files changed, 334 insertions(+), 37 deletions(-) create mode 100644 gcode_file.cpp create mode 100644 gcode_file.h diff --git a/CMakeLists.txt b/CMakeLists.txt index b3fb063..2442b3f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,7 +26,7 @@ set(CMAKE_CXX_VISIBILITY_PRESET hidden) set(TESTS grbl_test.cpp heightmap_test.cpp) set(TERJE terje/gcode.cpp terje/gcode.h terje/gcode_parser.h terje/gcode_parser.cpp terje/machine.h terje/machine.cpp terje/grbl.h terje/grbl.cpp terje/comms.h terje/comms.cpp terje/telnet.h terje/telnet.cpp terje/measure_view_model.h terje/measure_view_model.cpp) -set(SENDER_GRBL_SRC grbl.h grbl.cpp grbl_communication.h grbl_communication.cpp grbl_machine.h grbl_machine.cpp string_utils.h render.h render.cpp heightmap.h heightmap.cpp gcode_parser.h gcode_parser.cpp gcode_commands.h gcode_commands.cpp sender_app.h sender_app.cpp) +set(SENDER_GRBL_SRC grbl.h grbl.cpp grbl_communication.h grbl_communication.cpp grbl_machine.h grbl_machine.cpp string_utils.h render.h render.cpp heightmap.h heightmap.cpp gcode_parser.h gcode_parser.cpp gcode_commands.h gcode_commands.cpp sender_app.h sender_app.cpp gcode_file.h gcode_file.cpp) add_executable(sender main.cpp ${TESTS} ${TERJE} ${SENDER_GRBL_SRC}) target_link_libraries(sender nanogui GL gtest gtest_main) \ No newline at end of file diff --git a/gcode_commands.cpp b/gcode_commands.cpp index 512bd08..571db95 100644 --- a/gcode_commands.cpp +++ b/gcode_commands.cpp @@ -37,13 +37,13 @@ glm::vec<3, double> grbl::line_motion_cmd::interpolate(double ratio) { return start + delta() * ratio; } -std::vector> grbl::line_motion_cmd::split(double lngth) { +std::vector grbl::line_motion_cmd::split(double lngth) { //don't split up rapid or not fully defined motions if (rapid || !start_valid || !position_valid[0] || !position_valid[1] || !position_valid[2]) { - return {std::shared_ptr(this)}; + return {this}; } - std::vector> result; + std::vector result; int divisions = (int) std::ceil(length() / lngth); if (divisions < 1) { @@ -61,7 +61,7 @@ std::vector> grbl::line_motion_cmd::split(doub immediate->feed = feed; immediate->position_valid[0] = immediate->position_valid[1] = immediate->position_valid[2] = true; immediate->start_valid = true; - result.push_back(std::shared_ptr(immediate)); + result.push_back(immediate); last_end = end; } @@ -90,7 +90,7 @@ glm::vec<3, double> grbl::arc_motion_cmd::interpolate(double ratio) { return interpolation; } -std::vector> grbl::arc_motion_cmd::split(double lngth) { +std::vector grbl::arc_motion_cmd::split(double lngth) { int divisions = (int) ceil(length() / lngth); if (divisions < 1) @@ -99,7 +99,7 @@ std::vector> grbl::arc_motion_cmd::split(doubl glm::vec3 lastEnd = start; - std::vector> result; + std::vector result; for (int i = 1; i <= divisions; i++) { glm::vec<3, double> end = interpolate(((double) i) / divisions); @@ -113,7 +113,7 @@ std::vector> grbl::arc_motion_cmd::split(doubl immediate->u = u; immediate->v = v; - result.push_back(std::shared_ptr(immediate)); + result.push_back(immediate); lastEnd = end; } diff --git a/gcode_commands.h b/gcode_commands.h index 106a624..bb0abe1 100644 --- a/gcode_commands.h +++ b/gcode_commands.h @@ -52,7 +52,7 @@ namespace grbl { // Total travel distance of tool virtual double length() const = 0; virtual glm::vec<3, double> interpolate(double ratio) = 0; - virtual std::vector> split(double length) = 0; + virtual std::vector split(double length) = 0; }; struct line_motion_cmd : public motion_cmd { @@ -65,7 +65,7 @@ namespace grbl { double length() const override; glm::vec<3, double> interpolate(double ratio) override; - std::vector> split(double length) override; + std::vector split(double length) override; }; struct arc_motion_cmd : public motion_cmd { @@ -112,7 +112,7 @@ namespace grbl { } glm::vec<3, double> interpolate(double ratio) override; - std::vector> split(double length) override; + std::vector split(double length) override; double length() const override; diff --git a/gcode_file.cpp b/gcode_file.cpp new file mode 100644 index 0000000..ec80a0d --- /dev/null +++ b/gcode_file.cpp @@ -0,0 +1,157 @@ +#include "gcode_file.h" +#include "gcode_parser.h" +#include +#include +#include + +grbl::grbl_file grbl::grbl_file::load(std::string path) { + grbl::grbl_parser parser; + + grbl::grbl_file result{}; + + std::ifstream in_file{path}; + if (in_file) { + parser.parse(in_file); + result = grbl_file(parser.commands); + } + + return result; +} + +grbl::grbl_file::grbl_file(std::vector cmd) + : commands(cmd) { + +} + +std::vector grbl::grbl_file::get_gcode() { + std::vector result; + + result.emplace_back("G90 G91.1 G21 G17"); + + grbl::parser_state state; + auto xyz = "XYZ"; + + std::stringstream ss; + ss << std::fixed << std::setprecision(3); + + for (auto &c: commands) { + auto *motion = dynamic_cast(c); + bool is_motion = motion != nullptr; + + auto *line = dynamic_cast(c); + auto *arc = dynamic_cast(c); + auto *mcode = dynamic_cast(c); + auto *spindle = dynamic_cast(c); + auto *dwell = dynamic_cast(c); + + if (is_motion) { + if (motion->feed != state.feed) { + ss.str(""); + ss << "F" << motion->feed; + result.push_back(ss.str()); + state.feed = motion->feed; + } + } + + if (line != nullptr) { + std::string code = line->rapid ? "G0" : "G1"; + + for (int i = 0; i < 3; i++) { + if (!line->position_valid[i]) + continue; + if (!line->start_valid || state.position[i] != line->end[i]) { + ss.str(""); + ss << xyz[i] << line->end[i]; + code += ss.str(); + } + } + + result.push_back(code); + + state.position = line->end; + continue; + } + + if (arc != nullptr) { + if (state.plane != arc->plane) { + switch (arc->plane) { + case arc_plane::xy: + result.emplace_back("G17"); + break; + case arc_plane::yz: + result.emplace_back("G19"); + break; + case arc_plane::zx: + result.emplace_back("G18"); + break; + } + state.plane = arc->plane; + } + + std::string code = arc->direction == arc_direction::cw ? "G2" : "G3"; + if (state.position.x != arc->end.x) { + ss.str(""); + ss << "X" << arc->end.x; + code += ss.str(); + } + if (state.position.y != arc->end.y) { + ss.str(""); + ss << "Y" << arc->end.y; + code += ss.str(); + } + if (state.position.z != arc->end.z) { + ss.str(""); + ss << "Z" << arc->end.z; + code += ss.str(); + } + + glm::vec3 center = roll_components({arc->u, arc->v, 0}, (int) arc->plane) - state.position; + + if (center.x != 0 && arc->plane != arc_plane::yz) { + ss.str(""); + ss << " I" << center.x; + code += ss.str(); + } + if (center.y != 0 && arc->plane != arc_plane::zx) { + ss.str(""); + ss << " J" << center.y; + code += ss.str(); + } + if (center.z != 0 && arc->plane != arc_plane::xy) { + ss.str(""); + ss << " K" << center.z; + code += ss.str(); + } + + result.push_back(code); + state.position = arc->end; + continue; + } + + if (mcode != nullptr) { + int code = mcode->code; + if (code == 2 || code == 30) + continue; + + result.push_back("M" + std::to_string(code)); + continue; + } + + if (spindle != nullptr) { + ss.str(""); + ss << "S" << spindle->speed; + result.push_back(ss.str()); + continue; + } + + if (dwell != nullptr) { + ss.str(""); + ss << "G4 P" << dwell->seconds; + result.push_back(ss.str()); + continue; + } + } + + return result; +} + diff --git a/gcode_file.h b/gcode_file.h new file mode 100644 index 0000000..ab40df4 --- /dev/null +++ b/gcode_file.h @@ -0,0 +1,18 @@ +#pragma once + +#include +#include "gcode_commands.h" + +namespace grbl { + + struct grbl_file { + + grbl_file() = default; + grbl_file(std::vector commands); + static grbl_file load(std::string path); + + std::vector get_gcode(); + + std::vector commands; + }; +} \ No newline at end of file diff --git a/gcode_parser.cpp b/gcode_parser.cpp index d4c1d6f..b6d11ae 100644 --- a/gcode_parser.cpp +++ b/gcode_parser.cpp @@ -121,7 +121,7 @@ void grbl::grbl_parser::parse(std::string line, int line_number) { throw parse_exception("M code can only have positive integer parameters", line_number); } - commands.push_back(std::make_shared(param, line_number)); + commands.push_back(new mcode_cmd{param, line_number}); it = words.erase(it); continue; } @@ -132,7 +132,7 @@ void grbl::grbl_parser::parse(std::string line, int line_number) { warnings.push_back("spindle speed must be positive. (line " + std::to_string(line_number) + ")"); } - commands.push_back(std::make_shared(std::abs(param), line_number)); + commands.push_back(new spindle_cmd{std::abs(param), line_number}); it = words.erase(it); continue; } @@ -203,7 +203,7 @@ void grbl::grbl_parser::parse(std::string line, int line_number) { warnings.push_back("dwell time must be positive. (line " + std::to_string(line_number) + ")"); } - commands.push_back(std::make_shared(std::abs((*next).parameter), line_number)); + commands.push_back(new dwell_cmd{std::abs((*next).parameter), line_number}); it = words.erase(it); it = words.erase(it); continue; @@ -341,7 +341,7 @@ void grbl::grbl_parser::parse(std::string line, int line_number) { std::memcpy(l->position_valid, state.position_valid, sizeof(bool) * 3); - commands.push_back(std::shared_ptr(l)); + commands.push_back(l); state.position = end_pos; return; } @@ -512,7 +512,7 @@ void grbl::grbl_parser::parse(std::string line, int line_number) { a->line_number = line_number; a->plane = state.plane; - commands.push_back(std::shared_ptr(a)); + commands.push_back(a); state.position = end_pos; return; } diff --git a/gcode_parser.h b/gcode_parser.h index db16cb5..3ff920d 100644 --- a/gcode_parser.h +++ b/gcode_parser.h @@ -52,7 +52,7 @@ namespace grbl { struct grbl_parser { parser_state state; - std::vector> commands; + std::vector commands; std::vector warnings; grbl_parser(); diff --git a/grbl.cpp b/grbl.cpp index b03d9c1..e201773 100644 --- a/grbl.cpp +++ b/grbl.cpp @@ -1,5 +1,7 @@ #include "grbl.h" #include "string_utils.h" +#include "gcode_parser.h" +#include "gcode_file.h" #include #include @@ -38,7 +40,7 @@ grbl::program::program(std::string filename) { static auto comment_re = std::regex(R"(^\s*\(([^)]*)\)\s*$)"); static auto gcode_re = std::regex(R"(([a-zA-Z0-9\s.\-]+)\s*(;.*|\(([^)]*)\))?)"); -bool grbl::program::load_from_stream(std::istream& in) { +bool grbl::program::load_from_stream(std::istream &in) { instructions.clear(); is_loaded = true; @@ -74,7 +76,7 @@ bool grbl::program::load_from_stream(std::istream& in) { return is_loaded; } -bool grbl::program::load_from_string(const std::string& content) { +bool grbl::program::load_from_string(const std::string &content) { std::stringstream in_stream; in_stream << content; @@ -95,7 +97,7 @@ bool grbl::program::load_from_file(std::string path) { return is_loaded; } -std::ostream& operator<<(std::ostream& out, const grbl::instruction_type& t) { +std::ostream &operator<<(std::ostream &out, const grbl::instruction_type &t) { switch (t) { case grbl::instruction_type::gcode: out << "gcode"; @@ -110,13 +112,118 @@ std::ostream& operator<<(std::ostream& out, const grbl::instruction_type& t) { return out; } -std::ostream& operator<<(std::ostream& out, const grbl::instruction& i) { - out << "{.line: " << i.line << ", .type: " << i.type << ", .cmd: " << i.command << ", .comment: " << i.comment << " }"; +std::ostream &operator<<(std::ostream &out, const grbl::instruction &i) { + out << "{.line: " << i.line << ", .type: " << i.type << ", .cmd: " << i.command << ", .comment: " << i.comment + << " }"; return out; } -void grbl::program::dump(std::ostream& out) { - for (auto& i: instructions) { +void grbl::program::dump(std::ostream &out) { + for (auto &i: instructions) { out << i << std::endl; } } + +grbl::program grbl::program::apply_heightmap(grbl::heightmap& grid) { + + double segmentLength = grid.resolution; + + grbl::program result; + + std::vector new_commands; + + grbl::grbl_parser parser; + std::ifstream in_file{filename}; + if (in_file) { + parser.parse(in_file); + + for (auto &c: parser.commands) { + auto line = dynamic_cast(c); + auto arc = dynamic_cast(c); + if (line != nullptr) { + + // do not split up or modify any lines that are rapid or not fully defined + if (!line->start_valid || + (!line->position_valid[0] || !line->position_valid[1] || !line->position_valid[2]) || line->rapid) { + new_commands.push_back(line); + continue; + } + + for (auto &m: line->split(segmentLength)) { + m->start.z += grid.get_z_at(m->start.x, m->start.y); + m->end.z += grid.get_z_at(m->end.x, m->end.y); + + new_commands.push_back(m); + } + + } else if (arc != nullptr) { + if (arc->plane != arc_plane::xy) { + throw std::runtime_error( + "GCode contains arcs in YZ or XZ plane (G18/19), can't apply height map. Use 'Arcs to Lines' if you really need this."); + } + + for (auto &m: arc->split(segmentLength)) { + m->start.z += grid.get_z_at(m->start.x, m->start.y); + m->end.z += grid.get_z_at(m->end.x, m->end.y); + + new_commands.push_back(m); + } + + } else { + new_commands.push_back(c); + } + } + + grbl::grbl_file file(new_commands); + auto new_lines = file.get_gcode(); + result.load_from_lines(new_lines); + } + + return result; + +} + +bool grbl::program::load_from_lines(std::vector lines) { + is_loaded = true; + size_t line_number = 0; + for (auto &line: lines) { + line_number++; + if (!line.empty()) { + + std::smatch sm{}; + if (std::regex_match(line, sm, comment_re)) { + auto comment = sm.str(1); + instructions.emplace_back(instruction::new_comment(line_number, comment)); + } else if (std::regex_match(line, sm, gcode_re)) { + auto command = trim(sm.str(1)); + auto comment = trim(sm.str(3)); + instructions.emplace_back(instruction::new_gcode(line_number, command, comment)); + } else { + std::cerr << "Failed to parse line " << line << std::endl; + is_loaded = false; + } + } + } + + return is_loaded; +} + +void grbl::program::save(std::string path) { + std::ofstream out_file{path}; + if (out_file) { + for (auto &i: instructions) { + switch (i.type) { + case instruction_type::gcode: + out_file << i.command; + if (!i.comment.empty()) + out_file << " (" << i.comment << ")"; + out_file << std::endl; + break; + default: + break; + } + + } + } + +} diff --git a/grbl.h b/grbl.h index 797d38d..23f8c94 100644 --- a/grbl.h +++ b/grbl.h @@ -3,6 +3,7 @@ #include #include #include +#include "heightmap.h" namespace grbl { @@ -32,6 +33,7 @@ struct program { bool load_from_file(std::string filename); bool load_from_stream(std::istream& in); bool load_from_string(const std::string& content); + bool load_from_lines(std::vector lines); void dump(std::ostream& out); @@ -42,10 +44,11 @@ struct program { instruction instruction_at(size_t index) { return instructions.at(index); } - bool is_loaded = false; std::string filename; std::vector instructions{}; + grbl::program apply_heightmap(grbl::heightmap& grid); + void save(std::string path); }; } diff --git a/grbl_test.cpp b/grbl_test.cpp index d216d7a..25b0535 100644 --- a/grbl_test.cpp +++ b/grbl_test.cpp @@ -81,14 +81,14 @@ G00 X0. Y0. Z0.25 } for (auto &c: parser.commands) { - auto line = dynamic_cast(c.get()); + auto line = dynamic_cast(c); if (line != nullptr) { std::cout << "Line from " << glm::to_string(line->start) << " to " << glm::to_string(line->end) << std::endl; continue; } - auto arc = dynamic_cast(c.get()); + auto arc = dynamic_cast(c); if (arc != nullptr) { std::cout << "Arc from " << glm::to_string(arc->start) << " to " << glm::to_string(arc->end) << std::endl; continue; diff --git a/render.cpp b/render.cpp index dfdee3d..2fdfc09 100644 --- a/render.cpp +++ b/render.cpp @@ -346,6 +346,9 @@ void grbl::program_renderer::initialize_program_buffers() { GLsizei grbl::program_renderer::update_model_vbo(const grbl::program &pgm) { if (!pgm.is_loaded) return 0; + min_pos = glm::vec3(std::numeric_limits::max()); + max_pos = glm::vec3(std::numeric_limits::min()); + min_pos = max_pos = glm::vec3(0); /* static auto movement_re = std::regex(R"(([gG]0*1?\s+|[xXyYzZ]\s*[0-9\.\-]+))"); @@ -436,19 +439,19 @@ GLsizei grbl::program_renderer::update_model_vbo(const grbl::program &pgm) { auto feed_color = glm::vec4(1.0f, 0, 0, 1); for (auto &c: parser.commands) { - auto line = dynamic_cast(c.get()); - auto arc = dynamic_cast(c.get()); + auto line = dynamic_cast(c); + auto arc = dynamic_cast(c); if (line != nullptr) { - if (line->rapid) + if (line->rapid) { add_line(buffer_data, line->start, rapid_color, line->end, rapid_color); - else + } else { add_line(buffer_data, line->start, feed_color, line->end, feed_color); - if (!line->rapid) { update_model_extents(line->start); update_model_extents(line->end); } + } else if (arc != nullptr) { auto pieces = arc->split(0.1); for (auto &p: pieces) { diff --git a/sender_app.cpp b/sender_app.cpp index c96bc88..e78b3b0 100644 --- a/sender_app.cpp +++ b/sender_app.cpp @@ -14,12 +14,13 @@ SenderApp::SenderApp() tab_widget->set_selected_index(index); }); - info_layer = tab_widget->add(); + info_vscroll = tab_widget->add(); + tab_widget->append_tab("Info", info_vscroll); + + info_layer = info_vscroll->add(); info_layer->set_layout(new nanogui::GroupLayout(10, 20, 30, 0)); info_layer->add(""); - tab_widget->append_tab("Info", info_layer); - auto probe_layer = tab_widget->add(); probe_layer->set_layout(new nanogui::GroupLayout(10, 20, 30, 0)); probe_layer->add(""); @@ -131,6 +132,8 @@ void SenderApp::add_work_parameters_markup() { btn_run_program = work_btn_holder->add("Run"); // btnRunProgram->set_enabled(false); btn_run_program->set_callback([&] { +// auto leveled_pgm = pgm.apply_heightmap(heightmap_grid); +// leveled_pgm.save("output_corrected6.nc"); cnc.run_program(pgm, "G" + std::to_string(cbo_work_offset->selected_index() + 54)); }); btn_run_program->set_tooltip("Execute program"); diff --git a/sender_app.h b/sender_app.h index a8779f1..0243aaf 100644 --- a/sender_app.h +++ b/sender_app.h @@ -87,7 +87,6 @@ private: void draw_contents() override; grbl::machine cnc{}; -private: nanogui::TextBox *txt_min_z = nullptr, *txt_feed = nullptr; nanogui::TextBox *txt_heightmap_from_x = nullptr, *txt_heightmap_from_y = nullptr; @@ -101,13 +100,20 @@ private: nanogui::Color color_green = nanogui::Color(0, 255, 0, 255); nanogui::Button *btn_load_program = nullptr, *btn_check_program = nullptr, *btn_run_program = nullptr; - nanogui::Widget *info_layer = nullptr, *heightmap_layer = nullptr; nanogui::TabWidget *tab_widget = nullptr; + + nanogui::VScrollPanel *info_vscroll = nullptr; + nanogui::Widget *info_layer = nullptr; + + nanogui::Widget *heightmap_layer = nullptr; + + nanogui::VScrollPanel *settings_vscroll = nullptr; nanogui::Widget *settings_layer = nullptr; nanogui::VScrollPanel *parameters_vscroll = nullptr; nanogui::Widget *parameters_layer = nullptr; + nanogui::TextBox *mpos_x_text = nullptr, *mpos_y_text = nullptr, *mpos_z_text = nullptr; nanogui::ComboBox *cbo_work_offset = nullptr, *cbo_tool = nullptr, *cbo_jog_feed_rates = nullptr, *cbo_jog_distance = nullptr;