meswork/wasm-simulator
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial wasm simulator checkpoint

Browse Source
This commit is contained in:
CNC Local
2026-05-22 04:43:21 +08:00
commit 55d9fff9f0
54 changed files with 5523 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

242
core/src/canon_event_sink.cpp Normal file
View File

@@ -0,0 +1,242 @@
#include "canon_event_sink.h"
#include "rtcp_kinematics.h"
void CanonEventSink::reset() {
position_ = {};
plane_ = 17;
unit_scale_ = 1.0;
feed_ = 0.0;
spindle_ = 0.0;
selected_tool_ = 0;
callback_status_ = 0;
kinematics_type_ = 1;
}
void CanonEventSink::set_callback(CncSimEventCallback callback, void *user_data) {
callback_ = callback;
user_data_ = user_data;
callback_status_ = 0;
}
void CanonEventSink::clear_callback_status() {
callback_status_ = 0;
}
bool CanonEventSink::callback_aborted() const {
return callback_status_ != 0;
}
void CanonEventSink::configure_rtcp(bool enabled, double tool_length) {
rtcp_enabled_ = enabled;
default_rtcp_tool_length_ = tool_length;
rtcp_tool_length_ = tool_length;
}
void CanonEventSink::set_tool_length(int h_code, double tool_length) {
if (h_code <= 0) {
default_rtcp_tool_length_ = tool_length;
if (rtcp_h_code_ == 0) {
rtcp_tool_length_ = tool_length;
}
return;
}
tool_lengths_[h_code] = tool_length;
if (rtcp_h_code_ == h_code) {
rtcp_tool_length_ = tool_length;
}
}
void CanonEventSink::set_rtcp_state(bool enabled, int h_code, int line) {
rtcp_enabled_ = enabled;
rtcp_h_code_ = enabled ? h_code : 0;
if (enabled) {
const auto tool = tool_lengths_.find(h_code);
rtcp_tool_length_ = tool != tool_lengths_.end() ? tool->second : default_rtcp_tool_length_;
}
CncSimEvent event = base_event(CNC_SIM_EVENT_RTCP_STATE, line);
event.feed = rtcp_enabled_ ? 1.0 : 0.0;
event.tool = rtcp_h_code_;
event.dwell_seconds = rtcp_tool_length_;
emit(event);
}
void CanonEventSink::switch_kinematics(int kinematics_type, bool rtcp_enabled, int line) {
kinematics_type_ = kinematics_type;
rtcp_enabled_ = rtcp_enabled;
CncSimEvent event = base_event(CNC_SIM_EVENT_KINEMATICS_SWITCH, line);
event.reserved = kinematics_type_;
event.feed = rtcp_enabled_ ? 1.0 : 0.0;
emit(event);
}
void CanonEventSink::set_g5x_offset(int index, const CncSimPose &offset, int line) {
CncSimEvent event = base_event(CNC_SIM_EVENT_SET_G5X_OFFSET, line);
event.tool = index;
event.start = offset;
emit(event);
}
void CanonEventSink::set_g92_offset(const CncSimPose &offset, int line) {
CncSimEvent event = base_event(CNC_SIM_EVENT_SET_G92_OFFSET, line);
event.start = offset;
emit(event);
}
void CanonEventSink::set_xy_rotation(double angle_degrees, int line) {
CncSimEvent event = base_event(CNC_SIM_EVENT_SET_XY_ROTATION, line);
event.feed = angle_degrees;
emit(event);
}
void CanonEventSink::use_length_units(double scale, int line) {
unit_scale_ = scale;
CncSimEvent event = base_event(CNC_SIM_EVENT_SET_UNITS, line);
event.feed = scale;
emit(event);
}
void CanonEventSink::select_plane(int plane, int line) {
plane_ = plane;
CncSimEvent event = base_event(CNC_SIM_EVENT_SET_PLANE, line);
event.plane = plane_;
emit(event);
}
void CanonEventSink::set_feed_rate(double feed, int line) {
feed_ = feed;
CncSimEvent event = base_event(CNC_SIM_EVENT_SET_FEED, line);
event.feed = feed_;
emit(event);
}
void CanonEventSink::set_spindle_speed(double spindle, int line) {
spindle_ = spindle;
CncSimEvent event = base_event(CNC_SIM_EVENT_SET_SPINDLE, line);
event.spindle = spindle_;
emit(event);
}
void CanonEventSink::select_tool(int tool) {
selected_tool_ = tool;
}
void CanonEventSink::change_tool(int line) {
CncSimEvent event = base_event(CNC_SIM_EVENT_TOOL_CHANGE, line);
event.tool = selected_tool_;
emit(event);
}
void CanonEventSink::straight_traverse(int line, const CncSimPose &end) {
CncSimEvent event = base_event(CNC_SIM_EVENT_RAPID, line);
event.start = position_;
event.end = end;
emit(event);
emit_rtcp_pivot(line, position_, end);
position_ = end;
}
void CanonEventSink::straight_feed(int line, const CncSimPose &end) {
CncSimEvent event = base_event(CNC_SIM_EVENT_LINEAR_FEED, line);
event.start = position_;
event.end = end;
emit(event);
emit_rtcp_pivot(line, position_, end);
position_ = end;
}
void CanonEventSink::arc_feed(int line, const CncSimPose &end, const CncSimPose &center, int turns) {
CncSimEvent event = base_event(CNC_SIM_EVENT_ARC_FEED, line);
event.start = position_;
event.end = end;
event.center = center;
event.arc_turns = turns;
emit(event);
emit_rtcp_pivot(line, position_, end);
position_ = end;
}
void CanonEventSink::dwell(double seconds, int line) {
CncSimEvent event = base_event(CNC_SIM_EVENT_DWELL, line);
event.dwell_seconds = seconds;
emit(event);
}
void CanonEventSink::program_end(int line) {
emit(base_event(CNC_SIM_EVENT_PROGRAM_END, line));
}
void CanonEventSink::emit_raw(CncSimEvent event) {
if (event.version == 0) {
event.version = 1;
}
emit(event);
}
const CncSimPose &CanonEventSink::position() const {
return position_;
}
int CanonEventSink::plane() const {
return plane_;
}
double CanonEventSink::unit_scale() const {
return unit_scale_;
}
double CanonEventSink::feed_rate() const {
return feed_;
}
double CanonEventSink::spindle_speed() const {
return spindle_;
}
int CanonEventSink::selected_tool() const {
return selected_tool_;
}
int CanonEventSink::kinematics_type() const {
return kinematics_type_;
}
bool CanonEventSink::rtcp_enabled() const {
return rtcp_enabled_;
}
double CanonEventSink::rtcp_tool_length() const {
return rtcp_tool_length_;
}
void CanonEventSink::emit(CncSimEvent event) {
if (callback_ && callback_status_ == 0) {
callback_status_ = callback_(&event, user_data_);
}
}
void CanonEventSink::emit_rtcp_pivot(int line, const CncSimPose &start, const CncSimPose &end) {
if (!rtcp_enabled_ || rtcp_tool_length_ == 0.0 || callback_status_ != 0) {
return;
}
CncSimEvent event = base_event(CNC_SIM_EVENT_RTCP_PIVOT, line);
event.start = rtcp_pivot_from_tool_tip(start, rtcp_tool_length_);
event.end = rtcp_pivot_from_tool_tip(end, rtcp_tool_length_);
const RtcpVector tool = rtcp_tool_vector_from_pose(end, rtcp_tool_length_);
event.center.x = tool.x;
event.center.y = tool.y;
event.center.z = tool.z;
emit(event);
}
CncSimEvent CanonEventSink::base_event(CncSimEventType type, int line) const {
CncSimEvent event{};
event.version = 1;
event.type = type;
event.line = line;
event.plane = plane_;
event.feed = feed_;
event.spindle = spindle_;
event.tool = selected_tool_;
return event;
}

64
core/src/canon_event_sink.h Normal file
View File

@@ -0,0 +1,64 @@
#pragma once
#include "cnc_sim_api.h"
#include <unordered_map>
class CanonEventSink {
public:
void reset();
void set_callback(CncSimEventCallback callback, void *user_data);
void clear_callback_status();
bool callback_aborted() const;
void configure_rtcp(bool enabled, double tool_length);
void set_tool_length(int h_code, double tool_length);
void set_rtcp_state(bool enabled, int h_code, int line);
void switch_kinematics(int kinematics_type, bool rtcp_enabled, int line);
void set_g5x_offset(int index, const CncSimPose &offset, int line);
void set_g92_offset(const CncSimPose &offset, int line);
void set_xy_rotation(double angle_degrees, int line);
void use_length_units(double scale, int line);
void select_plane(int plane, int line);
void set_feed_rate(double feed, int line);
void set_spindle_speed(double spindle, int line);
void select_tool(int tool);
void change_tool(int line);
void straight_traverse(int line, const CncSimPose &end);
void straight_feed(int line, const CncSimPose &end);
void arc_feed(int line, const CncSimPose &end, const CncSimPose &center, int turns);
void dwell(double seconds, int line);
void program_end(int line);
void emit_raw(CncSimEvent event);
const CncSimPose &position() const;
int plane() const;
double unit_scale() const;
double feed_rate() const;
double spindle_speed() const;
int selected_tool() const;
int kinematics_type() const;
bool rtcp_enabled() const;
double rtcp_tool_length() const;
private:
void emit(CncSimEvent event);
void emit_rtcp_pivot(int line, const CncSimPose &start, const CncSimPose &end);
CncSimEvent base_event(CncSimEventType type, int line) const;
CncSimEventCallback callback_ = nullptr;
void *user_data_ = nullptr;
int callback_status_ = 0;
CncSimPose position_{};
int plane_ = 17;
double unit_scale_ = 1.0;
double feed_ = 0.0;
double spindle_ = 0.0;
int selected_tool_ = 0;
bool rtcp_enabled_ = false;
double default_rtcp_tool_length_ = 0.0;
double rtcp_tool_length_ = 0.0;
int rtcp_h_code_ = 0;
int kinematics_type_ = 1;
std::unordered_map<int, double> tool_lengths_;
};

235
core/src/cnc_sim_api.cpp Normal file
View File

@@ -0,0 +1,235 @@
#include "cnc_sim_api.h"
#include "canon_event_sink.h"
#include "gcode_backend.h"
#include <algorithm>
#include <cctype>
#include <cstdlib>
#include <string>
struct CncSimHandle {
CncSimDialect dialect = CNC_SIM_DIALECT_LINUXCNC;
GcodeBackendKind backend = GcodeBackendKind::Smoke;
std::string last_error;
CanonEventSink sink;
};
namespace {
void set_error(CncSimHandle *handle, const std::string &message) {
if (handle) {
handle->last_error = message;
}
}
std::string config_text(const char *json, size_t json_len) {
if (!json || json_len == 0) {
return {};
}
return std::string(json, json + json_len);
}
std::string lower_ascii(std::string value) {
std::transform(value.begin(), value.end(), value.begin(), [](unsigned char ch) {
return static_cast<char>(std::tolower(ch));
});
return value;
}
bool contains_backend_value(const std::string &json, const char *value) {
std::string compact;
compact.reserve(json.size());
for (char ch : json) {
if (!std::isspace(static_cast<unsigned char>(ch))) {
compact.push_back(static_cast<char>(std::tolower(static_cast<unsigned char>(ch))));
}
}
const std::string needle = std::string("\"backend\":\"") + value + "\"";
return compact.find(needle) != std::string::npos;
}
std::string compact_lower_json(const std::string &json) {
std::string compact;
compact.reserve(json.size());
for (char ch : json) {
if (!std::isspace(static_cast<unsigned char>(ch))) {
compact.push_back(static_cast<char>(std::tolower(static_cast<unsigned char>(ch))));
}
}
return compact;
}
bool contains_bool_value(const std::string &compact, const char *key, bool *value) {
const std::string true_needle = std::string("\"") + key + "\":true";
if (compact.find(true_needle) != std::string::npos) {
*value = true;
return true;
}
const std::string false_needle = std::string("\"") + key + "\":false";
if (compact.find(false_needle) != std::string::npos) {
*value = false;
return true;
}
return false;
}
bool find_number_value(const std::string &compact, const char *key, double *value) {
const std::string needle = std::string("\"") + key + "\":";
const size_t pos = compact.find(needle);
if (pos == std::string::npos) {
return false;
}
const char *start = compact.c_str() + pos + needle.size();
char *end = nullptr;
const double parsed = std::strtod(start, &end);
if (end == start) {
return false;
}
*value = parsed;
return true;
}
void apply_tool_length_table(CanonEventSink &sink, const std::string &compact) {
const std::string needle = "\"toollengths\":{";
size_t pos = compact.find(needle);
if (pos == std::string::npos) {
return;
}
pos += needle.size();
while (pos < compact.size() && compact[pos] != '}') {
if (compact[pos] == ',') {
++pos;
continue;
}
if (compact[pos] != '"') {
break;
}
++pos;
char *h_end = nullptr;
const long h_code = std::strtol(compact.c_str() + pos, &h_end, 10);
if (h_end == compact.c_str() + pos || *h_end != '"') {
break;
}
pos = static_cast<size_t>(h_end - compact.c_str()) + 1;
if (pos >= compact.size() || compact[pos] != ':') {
break;
}
++pos;
char *length_end = nullptr;
const double tool_length = std::strtod(compact.c_str() + pos, &length_end);
if (length_end == compact.c_str() + pos) {
break;
}
if (h_code > 0) {
sink.set_tool_length(static_cast<int>(h_code), tool_length);
}
pos = static_cast<size_t>(length_end - compact.c_str());
}
}
int apply_config(CncSimHandle *handle, const std::string &json) {
if (json.empty()) {
return 0;
}
const std::string compact = compact_lower_json(json);
if (contains_backend_value(json, "smoke")) {
handle->backend = GcodeBackendKind::Smoke;
} else if (contains_backend_value(json, "linuxcnc-rs274")) {
handle->backend = GcodeBackendKind::LinuxCncRs274;
} else if (compact.find("\"backend\"") != std::string::npos) {
set_error(handle, "unsupported backend in config");
return -1;
}
bool rtcp_enabled = false;
double tool_length = 0.0;
const bool has_rtcp_enabled = contains_bool_value(compact, "rtcp", &rtcp_enabled) ||
contains_bool_value(compact, "enabled", &rtcp_enabled);
const bool has_tool_length = find_number_value(compact, "toollength", &tool_length) ||
find_number_value(compact, "tool_length", &tool_length);
if (has_rtcp_enabled || has_tool_length) {
handle->sink.configure_rtcp(rtcp_enabled, tool_length);
}
apply_tool_length_table(handle->sink, compact);
return 0;
}
} // namespace
extern "C" {
CncSimHandle *cnc_sim_create(void) {
return new CncSimHandle();
}
void cnc_sim_destroy(CncSimHandle *handle) {
delete handle;
}
void cnc_sim_reset(CncSimHandle *handle) {
if (!handle) {
return;
}
handle->last_error.clear();
handle->sink.reset();
}
int cnc_sim_set_dialect(CncSimHandle *handle, CncSimDialect dialect) {
if (!handle) {
return -1;
}
switch (dialect) {
case CNC_SIM_DIALECT_LINUXCNC:
case CNC_SIM_DIALECT_FANUC:
case CNC_SIM_DIALECT_SIEMENS:
handle->dialect = dialect;
return 0;
default:
set_error(handle, "unsupported dialect");
return -1;
}
}
int cnc_sim_set_event_callback(CncSimHandle *handle, CncSimEventCallback callback, void *user_data) {
if (!handle) {
return -1;
}
handle->sink.set_callback(callback, user_data);
return 0;
}
int cnc_sim_load_config_json(CncSimHandle *handle, const char *json, size_t json_len) {
if (!handle) {
return -1;
}
if (!json && json_len != 0) {
set_error(handle, "null config buffer");
return -1;
}
handle->last_error.clear();
return apply_config(handle, config_text(json, json_len));
}
int cnc_sim_parse_program(CncSimHandle *handle, const char *program, size_t program_len) {
if (!handle) {
return -1;
}
handle->last_error.clear();
const int rc = parse_gcode_with_backend(handle->backend, handle->sink, program, program_len, &handle->last_error);
if (rc != 0 && handle->last_error.empty()) {
handle->last_error = "program parse failed";
}
return rc;
}
const char *cnc_sim_last_error(CncSimHandle *handle) {
if (!handle) {
return "null simulator handle";
}
return handle->last_error.c_str();
}
} // extern "C"

44
core/src/gcode_backend.cpp Normal file
View File

@@ -0,0 +1,44 @@
#include "gcode_backend.h"
#ifdef CNC_SIM_ENABLE_LINUXCNC_RS274_BACKEND
#include "linuxcnc_rs274_backend.h"
#endif
#include "smoke_gcode_parser.h"
const char *gcode_backend_name(GcodeBackendKind backend) {
switch (backend) {
case GcodeBackendKind::Smoke:
return "smoke";
case GcodeBackendKind::LinuxCncRs274:
return "linuxcnc-rs274";
default:
return "unknown";
}
}
int parse_gcode_with_backend(GcodeBackendKind backend,
CanonEventSink &sink,
const char *program,
size_t program_len,
std::string *error) {
switch (backend) {
case GcodeBackendKind::Smoke: {
SmokeGcodeParser parser(sink);
return parser.parse(program, program_len, error);
}
case GcodeBackendKind::LinuxCncRs274:
#ifdef CNC_SIM_ENABLE_LINUXCNC_RS274_BACKEND
return parse_linuxcnc_rs274_backend(sink, program, program_len, error);
#else
if (error) {
*error = "linuxcnc-rs274 backend is not compiled into this build";
}
return -1;
#endif
default:
if (error) {
*error = "unknown G-code backend";
}
return -1;
}
}

20
core/src/gcode_backend.h Normal file
View File

@@ -0,0 +1,20 @@
#pragma once
#include "canon_event_sink.h"
#include <cstddef>
#include <string>
enum class GcodeBackendKind {
Smoke,
LinuxCncRs274,
};
const char *gcode_backend_name(GcodeBackendKind backend);
int parse_gcode_with_backend(GcodeBackendKind backend,
CanonEventSink &sink,
const char *program,
size_t program_len,
std::string *error);

703
core/src/linuxcnc_canon_bridge.cpp Normal file
View File

@@ -0,0 +1,703 @@
#include "linuxcnc_canon_bridge.h"
#include "canon_event_sink.h"
#include "canon.hh"
#include <cstdarg>
#include <cstdio>
#include <cstring>
#include <cstdlib>
#include <string>
#include <vector>
namespace {
CanonEventSink *active_sink = nullptr;
std::string parameter_file_name = "rs274ngc.var";
CncSimPose make_pose(double x, double y, double z,
double a, double b, double c,
double u, double v, double w) {
CncSimPose pose{};
pose.x = x;
pose.y = y;
pose.z = z;
pose.a = a;
pose.b = b;
pose.c = c;
pose.u = u;
pose.v = v;
pose.w = w;
return pose;
}
int plane_to_g_code(CANON_PLANE plane) {
switch (plane) {
case CANON_PLANE::XY:
return 17;
case CANON_PLANE::XZ:
return 18;
case CANON_PLANE::YZ:
return 19;
default:
return 17;
}
}
double units_to_scale(CANON_UNITS units) {
switch (units) {
case CANON_UNITS_INCHES:
return 25.4;
case CANON_UNITS_CM:
return 10.0;
case CANON_UNITS_MM:
default:
return 1.0;
}
}
CANON_PLANE g_code_to_plane(int plane) {
switch (plane) {
case 18:
return CANON_PLANE::XZ;
case 19:
return CANON_PLANE::YZ;
case 17:
default:
return CANON_PLANE::XY;
}
}
const CncSimPose &current_position() {
static CncSimPose zero{};
return active_sink ? active_sink->position() : zero;
}
void trace_call(const char *name) {
if (std::getenv("CNC_SIM_TRACE_CANON")) {
std::fprintf(stderr, "canon:%s\n", name);
std::fflush(stderr);
}
}
} // namespace
void cnc_sim_linuxcnc_set_canon_sink(CanonEventSink *sink) {
trace_call("cnc_sim_linuxcnc_set_canon_sink");
active_sink = sink;
}
CanonEventSink *cnc_sim_linuxcnc_get_canon_sink() {
return active_sink;
}
void INIT_CANON() {
trace_call("INIT_CANON");
if (active_sink) {
active_sink->reset();
}
}
void USE_LENGTH_UNITS(CANON_UNITS units) {
trace_call("USE_LENGTH_UNITS");
if (active_sink) {
active_sink->use_length_units(units_to_scale(units), 0);
}
}
void SELECT_PLANE(CANON_PLANE plane) {
trace_call("SELECT_PLANE");
if (active_sink) {
active_sink->select_plane(plane_to_g_code(plane), 0);
}
}
void SET_FEED_RATE(double rate) {
trace_call("SET_FEED_RATE");
if (active_sink) {
active_sink->set_feed_rate(rate, 0);
}
}
void SET_SPINDLE_SPEED(int, double speed) {
trace_call("SET_SPINDLE_SPEED");
if (active_sink) {
active_sink->set_spindle_speed(speed, 0);
}
}
void SELECT_TOOL(int tool) {
trace_call("SELECT_TOOL");
if (active_sink) {
active_sink->select_tool(tool);
}
}
void CHANGE_TOOL() {
trace_call("CHANGE_TOOL");
if (active_sink) {
active_sink->change_tool(0);
}
}
void STRAIGHT_TRAVERSE(int lineno,
double x, double y, double z,
double a, double b, double c,
double u, double v, double w) {
trace_call("STRAIGHT_TRAVERSE");
if (active_sink) {
active_sink->straight_traverse(lineno, make_pose(x, y, z, a, b, c, u, v, w));
}
}
void STRAIGHT_FEED(int lineno,
double x, double y, double z,
double a, double b, double c,
double u, double v, double w) {
trace_call("STRAIGHT_FEED");
if (active_sink) {
active_sink->straight_feed(lineno, make_pose(x, y, z, a, b, c, u, v, w));
}
}
void ARC_FEED(int lineno,
double first_end, double second_end,
double first_axis, double second_axis, int rotation,
double axis_end_point,
double a, double b, double c,
double u, double v, double w) {
trace_call("ARC_FEED");
if (!active_sink) {
return;
}
CncSimPose end = active_sink->position();
CncSimPose center = active_sink->position();
switch (active_sink->plane()) {
case 17:
end.x = first_end;
end.y = second_end;
end.z = axis_end_point;
center.x = first_axis;
center.y = second_axis;
break;
case 18:
end.z = first_end;
end.x = second_end;
end.y = axis_end_point;
center.z = first_axis;
center.x = second_axis;
break;
case 19:
end.y = first_end;
end.z = second_end;
end.x = axis_end_point;
center.y = first_axis;
center.z = second_axis;
break;
default:
break;
}
end.a = a;
end.b = b;
end.c = c;
end.u = u;
end.v = v;
end.w = w;
active_sink->arc_feed(lineno, end, center, rotation);
}
void DWELL(double seconds) {
trace_call("DWELL");
if (active_sink) {
active_sink->dwell(seconds, 0);
}
}
void PROGRAM_END() {
trace_call("PROGRAM_END");
if (active_sink) {
active_sink->program_end(0);
}
}
void FINISH(void) {
trace_call("FINISH");
}
void SET_G5X_OFFSET(int index,
double x, double y, double z,
double a, double b, double c,
double u, double v, double w) {
trace_call("SET_G5X_OFFSET");
if (active_sink) {
active_sink->set_g5x_offset(index, make_pose(x, y, z, a, b, c, u, v, w), 0);
}
}
void SET_G92_OFFSET(double x, double y, double z,
double a, double b, double c,
double u, double v, double w) {
trace_call("SET_G92_OFFSET");
if (active_sink) {
active_sink->set_g92_offset(make_pose(x, y, z, a, b, c, u, v, w), 0);
}
}
void SET_XY_ROTATION(double angle) {
trace_call("SET_XY_ROTATION");
if (active_sink) {
active_sink->set_xy_rotation(angle, 0);
}
}
void CANON_UPDATE_END_POINT(double x, double y, double z,
double a, double b, double c,
double u, double v, double w) {
trace_call("CANON_UPDATE_END_POINT");
if (active_sink) {
active_sink->straight_traverse(0, make_pose(x, y, z, a, b, c, u, v, w));
}
}
void SET_TRAVERSE_RATE(double) {
}
void SET_FEED_REFERENCE(CANON_FEED_REFERENCE) {
}
void SET_FEED_MODE(int, int) {
}
void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE, double) {
}
void SET_NAIVECAM_TOLERANCE(double) {
}
void SET_CUTTER_RADIUS_COMPENSATION(double) {
}
void START_CUTTER_RADIUS_COMPENSATION(int) {
}
void STOP_CUTTER_RADIUS_COMPENSATION() {
}
void START_SPEED_FEED_SYNCH(int, double, bool) {
}
void STOP_SPEED_FEED_SYNCH() {
}
void NURBS_G5_FEED(int lineno, const std::vector<NURBS_CONTROL_POINT> &points, unsigned int, CANON_PLANE) {
if (!active_sink) {
return;
}
for (const auto &point : points) {
CncSimPose end = active_sink->position();
end.x = point.NURBS_X;
end.y = point.NURBS_Y;
active_sink->straight_feed(lineno, end);
}
}
void NURBS_G6_FEED(int lineno, const std::vector<NURBS_G6_CONTROL_POINT> &points, unsigned int, double, int, CANON_PLANE) {
if (!active_sink) {
return;
}
for (const auto &point : points) {
CncSimPose end = active_sink->position();
end.x = point.NURBS_X;
end.y = point.NURBS_Y;
active_sink->straight_feed(lineno, end);
}
}
void RIGID_TAP(int lineno, double x, double y, double z, double) {
if (active_sink) {
CncSimPose end = active_sink->position();
end.x = x;
end.y = y;
end.z = z;
active_sink->straight_feed(lineno, end);
}
}
void STRAIGHT_PROBE(int lineno,
double x, double y, double z,
double a, double b, double c,
double u, double v, double w,
unsigned char) {
if (active_sink) {
active_sink->straight_feed(lineno, make_pose(x, y, z, a, b, c, u, v, w));
}
}
void STOP() {
}
void SET_SPINDLE_MODE(int, double) {
}
void SPINDLE_RETRACT_TRAVERSE() {
}
void START_SPINDLE_CLOCKWISE(int, int) {
}
void START_SPINDLE_COUNTERCLOCKWISE(int, int) {
}
void STOP_SPINDLE_TURNING(int) {
if (active_sink) {
active_sink->set_spindle_speed(0.0, 0);
}
}
void SPINDLE_RETRACT() {
}
void ORIENT_SPINDLE(int, double, int) {
}
void WAIT_SPINDLE_ORIENT_COMPLETE(int, double) {
}
void LOCK_SPINDLE_Z() {
}
void USE_SPINDLE_FORCE() {
}
void USE_NO_SPINDLE_FORCE() {
}
void SET_TOOL_TABLE_ENTRY(int, int, const EmcPose &, double, double, double, int) {
}
void USE_TOOL_LENGTH_OFFSET(const EmcPose &) {
}
void CHANGE_TOOL_NUMBER(int number) {
if (active_sink) {
active_sink->select_tool(number);
active_sink->change_tool(0);
}
}
void RELOAD_TOOLDATA(void) {
}
void CLAMP_AXIS(CANON_AXIS) {
}
void COMMENT(const char *) {
if (active_sink) {
CncSimEvent event{};
event.version = 1;
event.type = CNC_SIM_EVENT_COMMENT;
active_sink->emit_raw(event);
}
}
void DISABLE_ADAPTIVE_FEED() {
}
void ENABLE_ADAPTIVE_FEED() {
}
void DISABLE_FEED_OVERRIDE() {
}
void ENABLE_FEED_OVERRIDE() {
}
void DISABLE_SPEED_OVERRIDE(int) {
}
void ENABLE_SPEED_OVERRIDE(int) {
}
void DISABLE_FEED_HOLD() {
}
void ENABLE_FEED_HOLD() {
}
void FLOOD_OFF() {
}
void FLOOD_ON() {
}
void MESSAGE(char *) {
}
void LOG(char *) {
}
void LOGOPEN(char *) {
}
void LOGAPPEND(char *) {
}
void LOGCLOSE() {
}
void MIST_OFF() {
}
void MIST_ON() {
}
void PALLET_SHUTTLE() {
}
void TURN_PROBE_OFF() {
}
void TURN_PROBE_ON() {
}
void UNCLAMP_AXIS(CANON_AXIS) {
}
void NURB_KNOT_VECTOR() {
}
void NURB_CONTROL_POINT(int, double, double, double, double) {
}
void NURB_FEED(double, double) {
}
void SET_BLOCK_DELETE(bool) {
}
bool GET_BLOCK_DELETE(void) {
return false;
}
void OPTIONAL_PROGRAM_STOP() {
}
void SET_OPTIONAL_PROGRAM_STOP(bool) {
}
bool GET_OPTIONAL_PROGRAM_STOP() {
return false;
}
void PROGRAM_STOP() {
}
void SET_MOTION_OUTPUT_BIT(int) {
}
void CLEAR_MOTION_OUTPUT_BIT(int) {
}
void SET_AUX_OUTPUT_BIT(int) {
}
void CLEAR_AUX_OUTPUT_BIT(int) {
}
void SET_MOTION_OUTPUT_VALUE(int, double) {
}
void SET_AUX_OUTPUT_VALUE(int, double) {
}
int WAIT(int, int, int wait_type, double) {
return wait_type;
}
int UNLOCK_ROTARY(int, int) {
return 0;
}
int LOCK_ROTARY(int, int) {
return 0;
}
double GET_EXTERNAL_FEED_RATE() {
return active_sink ? active_sink->feed_rate() : 0.0;
}
int GET_EXTERNAL_FLOOD() {
return 0;
}
CANON_UNITS GET_EXTERNAL_LENGTH_UNIT_TYPE() {
return active_sink && active_sink->unit_scale() == 25.4 ? CANON_UNITS_INCHES : CANON_UNITS_MM;
}
double GET_EXTERNAL_LENGTH_UNITS() {
return active_sink ? active_sink->unit_scale() : 1.0;
}
double GET_EXTERNAL_ANGLE_UNITS() {
return 1.0;
}
int GET_EXTERNAL_MIST() {
return 0;
}
CANON_MOTION_MODE GET_EXTERNAL_MOTION_CONTROL_MODE() {
return CANON_EXACT_STOP;
}
double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE() {
return 0.0;
}
double GET_EXTERNAL_MOTION_CONTROL_NAIVECAM_TOLERANCE() {
return 0.0;
}
void GET_EXTERNAL_PARAMETER_FILE_NAME(char *filename, int max_size) {
trace_call("GET_EXTERNAL_PARAMETER_FILE_NAME");
std::snprintf(filename, max_size, "%s", parameter_file_name.c_str());
}
void SET_PARAMETER_FILE_NAME(const char *filename) {
trace_call("SET_PARAMETER_FILE_NAME");
parameter_file_name = filename ? filename : "rs274ngc.var";
}
CANON_PLANE GET_EXTERNAL_PLANE() {
return active_sink ? g_code_to_plane(active_sink->plane()) : CANON_PLANE::XY;
}
double GET_EXTERNAL_POSITION_A() { return current_position().a; }
double GET_EXTERNAL_POSITION_B() { return current_position().b; }
double GET_EXTERNAL_POSITION_C() { return current_position().c; }
double GET_EXTERNAL_POSITION_X() { return current_position().x; }
double GET_EXTERNAL_POSITION_Y() { return current_position().y; }
double GET_EXTERNAL_POSITION_Z() { return current_position().z; }
double GET_EXTERNAL_POSITION_U() { return current_position().u; }
double GET_EXTERNAL_POSITION_V() { return current_position().v; }
double GET_EXTERNAL_POSITION_W() { return current_position().w; }
double GET_EXTERNAL_PROBE_POSITION_A() { return current_position().a; }
double GET_EXTERNAL_PROBE_POSITION_B() { return current_position().b; }
double GET_EXTERNAL_PROBE_POSITION_C() { return current_position().c; }
double GET_EXTERNAL_PROBE_POSITION_X() { return current_position().x; }
double GET_EXTERNAL_PROBE_POSITION_Y() { return current_position().y; }
double GET_EXTERNAL_PROBE_POSITION_Z() { return current_position().z; }
double GET_EXTERNAL_PROBE_POSITION_U() { return current_position().u; }
double GET_EXTERNAL_PROBE_POSITION_V() { return current_position().v; }
double GET_EXTERNAL_PROBE_POSITION_W() { return current_position().w; }
double GET_EXTERNAL_PROBE_VALUE() {
return 0.0;
}
int GET_EXTERNAL_PROBE_TRIPPED_VALUE() {
return 0;
}
int GET_EXTERNAL_QUEUE_EMPTY() {
return 1;
}
double GET_EXTERNAL_SPEED(int) {
return active_sink ? active_sink->spindle_speed() : 0.0;
}
CANON_DIRECTION GET_EXTERNAL_SPINDLE(int) {
return active_sink && active_sink->spindle_speed() != 0.0 ? CANON_CLOCKWISE : CANON_STOPPED;
}
double GET_EXTERNAL_TOOL_LENGTH_XOFFSET() { return 0.0; }
double GET_EXTERNAL_TOOL_LENGTH_YOFFSET() { return 0.0; }
double GET_EXTERNAL_TOOL_LENGTH_ZOFFSET() { return 0.0; }
double GET_EXTERNAL_TOOL_LENGTH_AOFFSET() { return 0.0; }
double GET_EXTERNAL_TOOL_LENGTH_BOFFSET() { return 0.0; }
double GET_EXTERNAL_TOOL_LENGTH_COFFSET() { return 0.0; }
double GET_EXTERNAL_TOOL_LENGTH_UOFFSET() { return 0.0; }
double GET_EXTERNAL_TOOL_LENGTH_VOFFSET() { return 0.0; }
double GET_EXTERNAL_TOOL_LENGTH_WOFFSET() { return 0.0; }
int GET_EXTERNAL_TOOL_SLOT() {
return active_sink ? active_sink->selected_tool() : 0;
}
int GET_EXTERNAL_SELECTED_TOOL_SLOT() {
return active_sink ? active_sink->selected_tool() : -1;
}
CANON_TOOL_TABLE GET_EXTERNAL_TOOL_TABLE(int) {
CANON_TOOL_TABLE tool{};
std::memset(&tool, 0, sizeof(tool));
return tool;
}
int GET_EXTERNAL_TC_FAULT() {
return 0;
}
int GET_EXTERNAL_TC_REASON() {
return 0;
}
double GET_EXTERNAL_TRAVERSE_RATE() {
return 0.0;
}
int GET_EXTERNAL_FEED_OVERRIDE_ENABLE() {
return 1;
}
int GET_EXTERNAL_SPINDLE_OVERRIDE_ENABLE(int) {
return 1;
}
int GET_EXTERNAL_ADAPTIVE_FEED_ENABLE() {
return 1;
}
int GET_EXTERNAL_FEED_HOLD_ENABLE() {
return 1;
}
int GET_EXTERNAL_DIGITAL_INPUT(int, int def) {
return def;
}
double GET_EXTERNAL_ANALOG_INPUT(int, double def) {
return def;
}
int GET_EXTERNAL_AXIS_MASK() {
return 0x1ff;
}
void ON_RESET(void) {
if (active_sink) {
active_sink->reset();
}
}
void CANON_ERROR(const char *, ...) {
}
void UPDATE_TAG(const StateTag &) {
}
USER_DEFINED_FUNCTION_TYPE USER_DEFINED_FUNCTION[USER_DEFINED_FUNCTION_NUM];
int GET_EXTERNAL_OFFSET_APPLIED() {
return 0;
}
EmcPose GET_EXTERNAL_OFFSETS() {
EmcPose pose;
ZERO_EMC_POSE(pose);
return pose;
}

7
core/src/linuxcnc_canon_bridge.h Normal file
View File

@@ -0,0 +1,7 @@
#pragma once
class CanonEventSink;
void cnc_sim_linuxcnc_set_canon_sink(CanonEventSink *sink);
CanonEventSink *cnc_sim_linuxcnc_get_canon_sink();

179
core/src/linuxcnc_rs274_backend.cpp Normal file
View File

@@ -0,0 +1,179 @@
#include "linuxcnc_rs274_backend.h"
#include <Python.h>
#include "linuxcnc_canon_bridge.h"
#include "simulator_gcode_controls.h"
#include "linuxcnc.h"
#include "nml_intf/canon.hh"
#include "nml_intf/interp_return.hh"
#include "rs274ngc/interp_base.hh"
#include "emc/tooldata/tooldata.hh"
#include <sstream>
#include <string>
#include <vector>
#include <cstdlib>
int _task = 0;
char _parameter_file_name[LINELEN];
extern "C" PyObject *PyInit_interpreter(void);
extern "C" PyObject *PyInit_emccanon(void);
extern "C" struct _inittab builtin_modules[];
struct _inittab builtin_modules[] = {
{"interpreter", PyInit_interpreter},
{"emccanon", PyInit_emccanon},
{nullptr, nullptr},
};
namespace {
void init_minimal_tooldata_once() {
static bool created = false;
if (!created) {
tool_mmap_creator(nullptr, 0);
created = true;
}
tooldata_reset();
CANON_TOOL_TABLE spindle = tooldata_entry_init();
spindle.toolno = 0;
spindle.pocketno = 0;
tooldata_put(spindle, 0);
CANON_TOOL_TABLE tool = tooldata_entry_init();
tool.toolno = 1;
tool.pocketno = 1;
tool.diameter = 6.0;
tooldata_put(tool, 1);
}
bool normal_read_status(int status) {
return status == INTERP_OK ||
status == INTERP_EXECUTE_FINISH ||
status == INTERP_ENDFILE ||
status == INTERP_EXIT;
}
bool normal_execute_status(int status, bool *program_done) {
if (status == INTERP_EXIT || status == INTERP_ENDFILE) {
*program_done = true;
return true;
}
return status == INTERP_OK || status == INTERP_EXECUTE_FINISH;
}
bool stop_if_callback_aborted(CanonEventSink &sink, std::string *error) {
if (!sink.callback_aborted()) {
return false;
}
if (error) {
*error = "event callback aborted parsing";
}
return true;
}
std::string interp_error(InterpBase *interp, int status, const char *stage) {
char message[1024]{};
interp->error_text(status, message, sizeof(message));
std::string result = stage;
result += " failed";
if (message[0]) {
result += ": ";
result += message;
}
return result;
}
} // namespace
int parse_linuxcnc_rs274_backend(CanonEventSink &sink,
const char *program,
size_t program_len,
std::string *error) {
if (!program && program_len != 0) {
if (error) {
*error = "null program buffer";
}
return -1;
}
if (const char *parameter_file = std::getenv("CNC_SIM_RS274_VAR")) {
SET_PARAMETER_FILE_NAME(parameter_file);
}
init_minimal_tooldata_once();
sink.clear_callback_status();
cnc_sim_linuxcnc_set_canon_sink(&sink);
InterpBase *interp = makeInterp();
int status = interp->init();
if (status != INTERP_OK) {
if (error) {
*error = interp_error(interp, status, "init");
}
delete interp;
cnc_sim_linuxcnc_set_canon_sink(nullptr);
return -1;
}
std::string source(program, program + program_len);
std::istringstream input(source);
std::string line;
bool program_done = false;
int line_number = 0;
while (!program_done && std::getline(input, line)) {
++line_number;
std::vector<SimulatorGcodeControlAction> control_actions;
if (parse_simulator_gcode_control_line(line, &control_actions)) {
for (const auto &action : control_actions) {
emit_simulator_gcode_control_action(sink, action, line_number);
if (stop_if_callback_aborted(sink, error)) {
interp->exit();
delete interp;
cnc_sim_linuxcnc_set_canon_sink(nullptr);
return -1;
}
}
continue;
}
status = interp->read(line.c_str());
if (!normal_read_status(status)) {
if (error) {
*error = interp_error(interp, status, "read");
}
interp->exit();
delete interp;
cnc_sim_linuxcnc_set_canon_sink(nullptr);
return -1;
}
if (status == INTERP_EXIT || status == INTERP_ENDFILE) {
break;
}
status = interp->execute();
if (!normal_execute_status(status, &program_done)) {
if (error) {
*error = interp_error(interp, status, "execute");
}
interp->exit();
delete interp;
cnc_sim_linuxcnc_set_canon_sink(nullptr);
return -1;
}
if (stop_if_callback_aborted(sink, error)) {
interp->exit();
delete interp;
cnc_sim_linuxcnc_set_canon_sink(nullptr);
return -1;
}
}
interp->exit();
delete interp;
cnc_sim_linuxcnc_set_canon_sink(nullptr);
return 0;
}

12
core/src/linuxcnc_rs274_backend.h Normal file
View File

@@ -0,0 +1,12 @@
#pragma once
#include "canon_event_sink.h"
#include <cstddef>
#include <string>
int parse_linuxcnc_rs274_backend(CanonEventSink &sink,
const char *program,
size_t program_len,
std::string *error);

72
core/src/rtcp_kinematics.cpp Normal file
View File

@@ -0,0 +1,72 @@
#include "rtcp_kinematics.h"
#include <cmath>
namespace {
constexpr double kPi = 3.141592653589793238462643383279502884;
double radians(double degrees) {
return degrees * kPi / 180.0;
}
RtcpVector rotate_x(RtcpVector vector, double angle) {
const double c = std::cos(angle);
const double s = std::sin(angle);
return {
vector.x,
vector.y * c - vector.z * s,
vector.y * s + vector.z * c,
};
}
RtcpVector rotate_y(RtcpVector vector, double angle) {
const double c = std::cos(angle);
const double s = std::sin(angle);
return {
vector.x * c + vector.z * s,
vector.y,
-vector.x * s + vector.z * c,
};
}
RtcpVector rotate_z(RtcpVector vector, double angle) {
const double c = std::cos(angle);
const double s = std::sin(angle);
return {
vector.x * c - vector.y * s,
vector.x * s + vector.y * c,
vector.z,
};
}
} // namespace
RtcpVector rtcp_rotate_abc_degrees(RtcpVector vector, double a_deg, double b_deg, double c_deg) {
vector = rotate_x(vector, radians(a_deg));
vector = rotate_y(vector, radians(b_deg));
vector = rotate_z(vector, radians(c_deg));
return vector;
}
RtcpVector rtcp_tool_vector_from_pose(const CncSimPose &pose, double tool_length) {
return rtcp_rotate_abc_degrees({0.0, 0.0, -tool_length}, pose.a, pose.b, pose.c);
}
CncSimPose rtcp_pivot_from_tool_tip(const CncSimPose &tool_tip, double tool_length) {
const RtcpVector tool = rtcp_tool_vector_from_pose(tool_tip, tool_length);
CncSimPose pivot = tool_tip;
pivot.x = tool_tip.x - tool.x;
pivot.y = tool_tip.y - tool.y;
pivot.z = tool_tip.z - tool.z;
return pivot;
}
CncSimPose rtcp_tool_tip_from_pivot(const CncSimPose &pivot, double tool_length) {
const RtcpVector tool = rtcp_tool_vector_from_pose(pivot, tool_length);
CncSimPose tool_tip = pivot;
tool_tip.x = pivot.x + tool.x;
tool_tip.y = pivot.y + tool.y;
tool_tip.z = pivot.z + tool.z;
return tool_tip;
}

17
core/src/rtcp_kinematics.h Normal file
View File

@@ -0,0 +1,17 @@
#pragma once
#include "cnc_sim_api.h"
struct RtcpVector {
double x;
double y;
double z;
};
// Rotation order is intrinsic tool orientation A then B then C, represented as
// Rz(C) * Ry(B) * Rx(A) applied to a local tool vector.
RtcpVector rtcp_rotate_abc_degrees(RtcpVector vector, double a_deg, double b_deg, double c_deg);
RtcpVector rtcp_tool_vector_from_pose(const CncSimPose &pose, double tool_length);
CncSimPose rtcp_pivot_from_tool_tip(const CncSimPose &tool_tip, double tool_length);
CncSimPose rtcp_tool_tip_from_pivot(const CncSimPose &pivot, double tool_length);

145
core/src/simulator_gcode_controls.cpp Normal file
View File

@@ -0,0 +1,145 @@
#include "simulator_gcode_controls.h"
#include <cctype>
#include <cmath>
#include <cstdlib>
namespace {
std::string strip_line_comment(const std::string &line) {
std::string out;
bool in_paren = false;
for (char raw : line) {
const char ch = static_cast<char>(std::toupper(static_cast<unsigned char>(raw)));
if (in_paren) {
if (ch == ')') {
in_paren = false;
}
continue;
}
if (ch == '(') {
in_paren = true;
continue;
}
if (ch == ';') {
break;
}
out.push_back(ch);
}
return out;
}
bool g_code_is(double actual, double expected) {
return std::fabs(actual - expected) < 0.0001;
}
bool parse_m_control_line(const std::string &stripped,
std::vector<SimulatorGcodeControlAction> *actions) {
const char *text = stripped.c_str();
bool saw_code = false;
for (size_t i = 0; text[i] != '\0';) {
if (std::isspace(static_cast<unsigned char>(text[i]))) {
++i;
continue;
}
if (text[i] != 'M') {
return false;
}
++i;
char *end = nullptr;
const long value = std::strtol(text + i, &end, 10);
if (end == text + i) {
return false;
}
SimulatorGcodeControlAction action{};
action.kind = SimulatorGcodeControlKind::KinematicsSwitch;
if (value == 428) {
action.value = 1;
action.rtcp_enabled = true;
} else if (value == 429) {
action.value = 0;
action.rtcp_enabled = false;
} else if (value == 430) {
action.value = 2;
action.rtcp_enabled = true;
} else {
return false;
}
actions->push_back(action);
saw_code = true;
i = static_cast<size_t>(end - text);
}
return saw_code;
}
bool parse_rtcp_control_line(const std::string &stripped,
std::vector<SimulatorGcodeControlAction> *actions) {
const char *text = stripped.c_str();
std::vector<SimulatorGcodeControlAction> parsed;
int h_code = 0;
bool saw_rtcp_code = false;
for (size_t i = 0; text[i] != '\0';) {
if (std::isspace(static_cast<unsigned char>(text[i]))) {
++i;
continue;
}
const char letter = text[i];
if (letter != 'G' && letter != 'H') {
return false;
}
++i;
char *end = nullptr;
const double value = std::strtod(text + i, &end);
if (end == text + i) {
return false;
}
if (letter == 'H') {
h_code = static_cast<int>(std::lround(value));
} else if (g_code_is(value, 43.4) || g_code_is(value, 43.5) || g_code_is(value, 49.0)) {
SimulatorGcodeControlAction action{};
action.kind = SimulatorGcodeControlKind::RtcpState;
action.value = static_cast<int>(std::lround(value * 10.0));
action.rtcp_enabled = g_code_is(value, 43.4) || g_code_is(value, 43.5);
parsed.push_back(action);
saw_rtcp_code = true;
} else {
return false;
}
i = static_cast<size_t>(end - text);
}
for (auto &action : parsed) {
action.h_code = action.rtcp_enabled ? h_code : 0;
actions->push_back(action);
}
return saw_rtcp_code;
}
} // namespace
bool parse_simulator_gcode_control_line(const std::string &line,
std::vector<SimulatorGcodeControlAction> *actions) {
actions->clear();
const std::string stripped = strip_line_comment(line);
if (parse_m_control_line(stripped, actions)) {
return true;
}
actions->clear();
if (parse_rtcp_control_line(stripped, actions)) {
return true;
}
actions->clear();
return false;
}
void emit_simulator_gcode_control_action(CanonEventSink &sink,
const SimulatorGcodeControlAction &action,
int line) {
if (action.kind == SimulatorGcodeControlKind::KinematicsSwitch) {
sink.switch_kinematics(action.value, action.rtcp_enabled, line);
} else if (action.kind == SimulatorGcodeControlKind::RtcpState) {
sink.set_rtcp_state(action.rtcp_enabled, action.h_code, line);
}
}

24
core/src/simulator_gcode_controls.h Normal file
View File

@@ -0,0 +1,24 @@
#pragma once
#include "canon_event_sink.h"
#include <string>
#include <vector>
enum class SimulatorGcodeControlKind {
KinematicsSwitch,
RtcpState,
};
struct SimulatorGcodeControlAction {
SimulatorGcodeControlKind kind;
int value;
bool rtcp_enabled;
int h_code;
};
bool parse_simulator_gcode_control_line(const std::string &line,
std::vector<SimulatorGcodeControlAction> *actions);
void emit_simulator_gcode_control_action(CanonEventSink &sink,
const SimulatorGcodeControlAction &action,
int line);

399
core/src/smoke_gcode_parser.cpp Normal file
View File

@@ -0,0 +1,399 @@
#include "smoke_gcode_parser.h"
#include <algorithm>
#include <cctype>
#include <cmath>
#include <cstdlib>
#include <sstream>
#include <string>
#include <unordered_map>
#include <vector>
namespace {
struct Word {
char letter;
double value;
};
std::string trim(const std::string &value) {
auto first = std::find_if_not(value.begin(), value.end(), [](unsigned char ch) { return std::isspace(ch); });
auto last = std::find_if_not(value.rbegin(), value.rend(), [](unsigned char ch) { return std::isspace(ch); }).base();
if (first >= last) {
return {};
}
return std::string(first, last);
}
std::string strip_comments(const std::string &line, std::string *comment) {
std::string out;
bool in_paren = false;
std::string paren;
for (char raw : line) {
char ch = static_cast<char>(std::toupper(static_cast<unsigned char>(raw)));
if (in_paren) {
if (ch == ')') {
in_paren = false;
if (comment && !paren.empty()) {
if (!comment->empty()) {
*comment += " ";
}
*comment += trim(paren);
}
paren.clear();
} else {
paren.push_back(raw);
}
continue;
}
if (ch == '(') {
in_paren = true;
continue;
}
if (ch == ';') {
break;
}
out.push_back(ch);
}
return out;
}
std::vector<Word> parse_word_list(const std::string &line) {
std::vector<Word> words;
const char *text = line.c_str();
char *end = nullptr;
for (size_t i = 0; text[i] != '\0';) {
if (std::isspace(static_cast<unsigned char>(text[i]))) {
++i;
continue;
}
char letter = text[i];
if (!std::isalpha(static_cast<unsigned char>(letter))) {
++i;
continue;
}
++i;
double value = std::strtod(text + i, &end);
if (end == text + i) {
continue;
}
words.push_back({letter, value});
i = static_cast<size_t>(end - text);
}
return words;
}
std::unordered_map<char, double> last_words_by_letter(const std::vector<Word> &word_list) {
std::unordered_map<char, double> words;
for (const Word &word : word_list) {
words[word.letter] = word.value;
}
return words;
}
bool has_axis_word(const std::unordered_map<char, double> &words) {
static constexpr char axes[] = {'X', 'Y', 'Z', 'A', 'B', 'C', 'U', 'V', 'W'};
for (char axis : axes) {
if (words.find(axis) != words.end()) {
return true;
}
}
return false;
}
void apply_axis(CncSimPose *pose, const std::unordered_map<char, double> &words, char axis, double CncSimPose::*member, bool absolute, double scale) {
auto it = words.find(axis);
if (it == words.end()) {
return;
}
const double value = it->second * scale;
if (absolute) {
pose->*member = value;
} else {
pose->*member += value;
}
}
void apply_axes(CncSimPose *pose, const std::unordered_map<char, double> &words, bool absolute, double scale) {
apply_axis(pose, words, 'X', &CncSimPose::x, absolute, scale);
apply_axis(pose, words, 'Y', &CncSimPose::y, absolute, scale);
apply_axis(pose, words, 'Z', &CncSimPose::z, absolute, scale);
apply_axis(pose, words, 'A', &CncSimPose::a, absolute, 1.0);
apply_axis(pose, words, 'B', &CncSimPose::b, absolute, 1.0);
apply_axis(pose, words, 'C', &CncSimPose::c, absolute, 1.0);
apply_axis(pose, words, 'U', &CncSimPose::u, absolute, scale);
apply_axis(pose, words, 'V', &CncSimPose::v, absolute, scale);
apply_axis(pose, words, 'W', &CncSimPose::w, absolute, scale);
}
void set_arc_center(CncSimEvent *event, const std::unordered_map<char, double> &words, int plane, double scale) {
event->center = event->start;
if (words.count('R')) {
const double radius = words.at('R') * scale;
double start_first = 0.0;
double start_second = 0.0;
double end_first = 0.0;
double end_second = 0.0;
if (plane == 17) {
start_first = event->start.x;
start_second = event->start.y;
end_first = event->end.x;
end_second = event->end.y;
} else if (plane == 18) {
start_first = event->start.x;
start_second = event->start.z;
end_first = event->end.x;
end_second = event->end.z;
} else if (plane == 19) {
start_first = event->start.y;
start_second = event->start.z;
end_first = event->end.y;
end_second = event->end.z;
}
const double dx = end_first - start_first;
const double dy = end_second - start_second;
const double chord = std::hypot(dx, dy);
if (chord > 0.0 && std::fabs(radius) >= chord * 0.5) {
const double mid_first = (start_first + end_first) * 0.5;
const double mid_second = (start_second + end_second) * 0.5;
double h = std::sqrt(std::max(0.0, radius * radius - chord * chord * 0.25));
if (radius < 0.0) {
h = -h;
}
const double direction = event->arc_turns >= 0 ? 1.0 : -1.0;
const double center_first = mid_first + (-dy / chord) * h * direction;
const double center_second = mid_second + (dx / chord) * h * direction;
if (plane == 17) {
event->center.x = center_first;
event->center.y = center_second;
} else if (plane == 18) {
event->center.x = center_first;
event->center.z = center_second;
} else if (plane == 19) {
event->center.y = center_first;
event->center.z = center_second;
}
return;
}
}
const double i = words.count('I') ? words.at('I') * scale : 0.0;
const double j = words.count('J') ? words.at('J') * scale : 0.0;
const double k = words.count('K') ? words.at('K') * scale : 0.0;
if (plane == 17) {
event->center.x = event->start.x + i;
event->center.y = event->start.y + j;
} else if (plane == 18) {
event->center.x = event->start.x + i;
event->center.z = event->start.z + k;
} else if (plane == 19) {
event->center.y = event->start.y + j;
event->center.z = event->start.z + k;
}
}
int rounded_word(const std::unordered_map<char, double> &words, char letter, int fallback) {
auto it = words.find(letter);
if (it == words.end()) {
return fallback;
}
return static_cast<int>(std::lround(it->second));
}
int rounded_value(double value) {
return static_cast<int>(std::lround(value));
}
bool g_code_is(double actual, double expected) {
return std::fabs(actual - expected) < 0.0001;
}
} // namespace
SmokeGcodeParser::SmokeGcodeParser(CanonEventSink &sink)
: sink_(sink) {
}
namespace {
bool stop_if_callback_aborted(CanonEventSink &sink, std::string *error) {
if (!sink.callback_aborted()) {
return false;
}
if (error) {
*error = "event callback aborted parsing";
}
return true;
}
} // namespace
int SmokeGcodeParser::parse(const char *program, size_t program_len, std::string *error) {
if (!program && program_len != 0) {
if (error) {
*error = "null program buffer";
}
return -1;
}
sink_.reset();
sink_.clear_callback_status();
absolute_ = true;
std::string source(program, program + program_len);
std::istringstream input(source);
std::string raw_line;
int line_number = 0;
int modal_motion = -1;
while (std::getline(input, raw_line)) {
++line_number;
std::string comment;
std::string line = strip_comments(raw_line, &comment);
if (!comment.empty()) {
CncSimEvent event{};
event.version = 1;
event.type = CNC_SIM_EVENT_COMMENT;
event.line = line_number;
sink_.emit_raw(event);
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
}
auto word_list = parse_word_list(line);
if (word_list.empty()) {
continue;
}
auto words = last_words_by_letter(word_list);
std::vector<int> m_codes;
for (const Word &word : word_list) {
if (word.letter == 'G') {
const int g = rounded_value(word.value);
if (g_code_is(word.value, 43.4) || g_code_is(word.value, 43.5)) {
sink_.set_rtcp_state(true, rounded_word(words, 'H', 0), line_number);
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
} else if (g == 49) {
sink_.set_rtcp_state(false, 0, line_number);
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
} else if (g == 17 || g == 18 || g == 19) {
sink_.select_plane(g, line_number);
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
} else if (g == 20 || g == 70) {
sink_.use_length_units(25.4, line_number);
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
} else if (g == 21 || g == 71) {
sink_.use_length_units(1.0, line_number);
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
} else if (g == 90) {
absolute_ = true;
} else if (g == 91) {
absolute_ = false;
} else if (g == 0 || g == 1 || g == 2 || g == 3) {
modal_motion = g;
} else if (g == 4) {
sink_.dwell(words.count('P') ? words['P'] : 0.0, line_number);
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
}
} else if (word.letter == 'M') {
m_codes.push_back(rounded_value(word.value));
}
}
if (words.count('F')) {
sink_.set_feed_rate(words['F'] * sink_.unit_scale(), line_number);
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
}
if (words.count('S')) {
sink_.set_spindle_speed(words['S'], line_number);
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
}
if (words.count('T')) {
sink_.select_tool(rounded_word(words, 'T', sink_.selected_tool()));
}
bool program_end = false;
for (int m : m_codes) {
if (m == 3 || m == 4 || m == 5) {
sink_.set_spindle_speed(m == 5 ? 0.0 : sink_.spindle_speed(), line_number);
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
} else if (m == 6) {
sink_.change_tool(line_number);
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
} else if (m == 428) {
sink_.switch_kinematics(1, true, line_number);
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
} else if (m == 429) {
sink_.switch_kinematics(0, false, line_number);
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
} else if (m == 430) {
sink_.switch_kinematics(2, true, line_number);
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
} else if (m == 2 || m == 30) {
sink_.program_end(line_number);
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
program_end = true;
}
}
if (program_end) {
break;
}
if (has_axis_word(words) && (modal_motion == 0 || modal_motion == 1 || modal_motion == 2 || modal_motion == 3)) {
CncSimEvent event{};
event.start = sink_.position();
event.end = sink_.position();
apply_axes(&event.end, words, absolute_, sink_.unit_scale());
if (modal_motion == 2 || modal_motion == 3) {
event.arc_turns = modal_motion == 2 ? -1 : 1;
set_arc_center(&event, words, sink_.plane(), sink_.unit_scale());
}
if (modal_motion == 0) {
sink_.straight_traverse(line_number, event.end);
} else if (modal_motion == 1) {
sink_.straight_feed(line_number, event.end);
} else {
sink_.arc_feed(line_number, event.end, event.center, event.arc_turns);
}
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
}
}
return 0;
}

18
core/src/smoke_gcode_parser.h Normal file
View File

@@ -0,0 +1,18 @@
#pragma once
#include "canon_event_sink.h"
#include <cstddef>
#include <string>
class SmokeGcodeParser {
public:
explicit SmokeGcodeParser(CanonEventSink &sink);
int parse(const char *program, size_t program_len, std::string *error);
private:
CanonEventSink &sink_;
bool absolute_ = true;
};