530 lines
17 KiB
C++
530 lines
17 KiB
C++
#include "gcode_parser.h"
|
|
|
|
grbl::parse_exception::parse_exception(const std::string &reason, size_t lineNumber)
|
|
: reason(reason), line_number(lineNumber) {}
|
|
|
|
const char *grbl::parse_exception::what() const noexcept {
|
|
return reason.c_str();
|
|
}
|
|
|
|
struct word {
|
|
char command;
|
|
double parameter;
|
|
|
|
std::string to_string() const {
|
|
std::string result;
|
|
result += command;
|
|
result += std::to_string(parameter);
|
|
return result;
|
|
}
|
|
};
|
|
|
|
static std::array<double, 4> motion_commands = {0, 1, 2, 3};
|
|
static std::string valid_words = "GMXYZIJKFRSP";
|
|
static std::string ignore_axes = "ABC";
|
|
|
|
std::string grbl::grbl_parser::cleanup_line(std::string line, int line_number) {
|
|
auto comment_index = line.rfind(';');
|
|
if (comment_index != std::string::npos) {
|
|
line = line.substr(0, comment_index);
|
|
}
|
|
|
|
size_t start;
|
|
while ((start = line.find('(')) != std::string::npos) {
|
|
auto end = line.find(')');
|
|
if (end == std::string::npos || end < start)
|
|
throw parse_exception("mismatched parentheses", line_number);
|
|
|
|
line.erase(start, end - start + 1);
|
|
}
|
|
|
|
return line;
|
|
}
|
|
|
|
void grbl::grbl_parser::parse(std::istream &in) {
|
|
int i = 0;
|
|
for (std::string line; std::getline(in, line);) {
|
|
i++;
|
|
line = trim(cleanup_line(line, i));
|
|
if (line.empty()) continue;
|
|
|
|
parse(to_upper(line), i);
|
|
}
|
|
}
|
|
|
|
bool grbl::grbl_parser::is_motion_command(double param) {
|
|
for (auto &p: motion_commands) {
|
|
if (p == param)
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void grbl::grbl_parser::parse(std::string line, int line_number) {
|
|
static auto gcode_re = std::regex(R"([a-zA-Z]\s*\-?\d*\.?\d*)");
|
|
|
|
std::vector<word> words;
|
|
|
|
std::smatch matches;
|
|
std::string::const_iterator start(line.cbegin());
|
|
while (regex_search(start, line.cend(), matches, gcode_re)) {
|
|
auto token = remove_spaces(matches[0]);
|
|
|
|
words.push_back(
|
|
word{
|
|
.command= token[0],
|
|
.parameter = std::stod(token.substr(1))
|
|
}
|
|
);
|
|
|
|
start = matches.suffix().first;
|
|
}
|
|
|
|
auto it = words.begin();
|
|
while (it != words.end()) {
|
|
if ((*it).command == 'N') {
|
|
it = words.erase(it);
|
|
continue;
|
|
}
|
|
|
|
bool ignore_additional_axes = true;
|
|
if (ignore_axes.find((*it).command) != std::string::npos && ignore_additional_axes) {
|
|
it = words.erase(it);
|
|
continue;
|
|
}
|
|
|
|
if (valid_words.find((*it).command) == std::string::npos) {
|
|
warnings.push_back("ignoring unknown word (letter): \"" + (*it).to_string() + "\". (line " +
|
|
std::to_string(line_number) + ")");
|
|
it = words.erase(it);
|
|
continue;
|
|
}
|
|
|
|
if ((*it).command != 'F') {
|
|
it++;
|
|
continue;
|
|
}
|
|
|
|
state.feed = (*it).parameter;
|
|
if (state.unit == parse_unit::imperial)
|
|
state.feed *= 25.4;
|
|
|
|
it = words.erase(it);
|
|
}
|
|
|
|
|
|
it = words.begin();
|
|
while (it != words.end()) {
|
|
if ((*it).command == 'M') {
|
|
int param = (int) (*it).parameter;
|
|
if (param != (*it).parameter || param < 0) {
|
|
throw parse_exception("M code can only have positive integer parameters", line_number);
|
|
}
|
|
|
|
commands.push_back(std::make_shared<mcode_cmd>(param, line_number));
|
|
it = words.erase(it);
|
|
continue;
|
|
}
|
|
|
|
if ((*it).command == 'S') {
|
|
double param = (*it).parameter;
|
|
if (param < 0) {
|
|
warnings.push_back("spindle speed must be positive. (line " + std::to_string(line_number) + ")");
|
|
}
|
|
|
|
commands.push_back(std::make_shared<spindle_cmd>(std::abs(param), line_number));
|
|
it = words.erase(it);
|
|
continue;
|
|
}
|
|
|
|
|
|
if ((*it).command == 'G' && !is_motion_command((*it).parameter)) {
|
|
auto param = (*it).parameter;
|
|
|
|
if (param == 90) {
|
|
state.distance_mode = parse_distance_mode::absolute;
|
|
it = words.erase(it);
|
|
continue;
|
|
}
|
|
|
|
if (param == 91) {
|
|
state.distance_mode = parse_distance_mode::incremental;
|
|
it = words.erase(it);
|
|
continue;
|
|
}
|
|
|
|
if (param == 90.1) {
|
|
state.arc_distance_mode = parse_distance_mode::absolute;
|
|
it = words.erase(it);
|
|
continue;
|
|
}
|
|
|
|
if (param == 91.1) {
|
|
state.arc_distance_mode = parse_distance_mode::incremental;
|
|
it = words.erase(it);
|
|
continue;
|
|
}
|
|
|
|
if (param == 21) {
|
|
state.unit = parse_unit::metric;
|
|
it = words.erase(it);
|
|
continue;
|
|
}
|
|
|
|
if (param == 20) {
|
|
state.unit = parse_unit::imperial;
|
|
it = words.erase(it);
|
|
continue;
|
|
}
|
|
|
|
if (param == 17) {
|
|
state.plane = arc_plane::xy;
|
|
it = words.erase(it);
|
|
continue;
|
|
}
|
|
|
|
if (param == 18) {
|
|
state.plane = arc_plane::zx;
|
|
it = words.erase(it);
|
|
continue;
|
|
}
|
|
|
|
if (param == 19) {
|
|
state.plane = arc_plane::yz;
|
|
it = words.erase(it);
|
|
continue;
|
|
}
|
|
|
|
if (param == 4) {
|
|
auto next = it;
|
|
next++;
|
|
if (next != words.end() && (*next).command == 'P') {
|
|
if ((*next).parameter < 0) {
|
|
warnings.push_back("dwell time must be positive. (line " + std::to_string(line_number) + ")");
|
|
}
|
|
|
|
commands.push_back(std::make_shared<dwell_cmd>(std::abs((*next).parameter), line_number));
|
|
it = words.erase(it);
|
|
it = words.erase(it);
|
|
continue;
|
|
}
|
|
}
|
|
|
|
// we do not support G64, only G61
|
|
if (param == 64) {
|
|
warnings.push_back("G64 not supported. Ignoring. (line " + std::to_string(line_number) + ")");
|
|
auto next = it;
|
|
next++;
|
|
if (next != words.end() && (*next).command == 'P') {
|
|
it = words.erase(it);
|
|
}
|
|
it = words.erase(it);
|
|
continue;
|
|
}
|
|
|
|
if (param == 94 || param == 93 || param == 95) {
|
|
// G93 sets feed rate mode to inverse time
|
|
// G94 sets feed rate mode to units per minute (mm/min or inch/min)
|
|
// G95 sets feed rate mode to units per revolution
|
|
it = words.erase(it);
|
|
continue;
|
|
}
|
|
|
|
// coordinate system offset
|
|
if (param == 54 || param == 55 || param == 56 || param == 57) {
|
|
it = words.erase(it);
|
|
continue;
|
|
}
|
|
|
|
|
|
warnings.push_back(
|
|
"ignoring unknown command " + (*it).to_string() + ". (line " + std::to_string(line_number) + ")");
|
|
it = words.erase(it);
|
|
}
|
|
|
|
if (words.empty())
|
|
return;
|
|
|
|
int MotionMode = state.last_motion_mode;
|
|
if (words.begin()->command == 'G') {
|
|
MotionMode = (int) words.begin()->parameter;
|
|
state.last_motion_mode = MotionMode;
|
|
words.erase(words.begin());
|
|
}
|
|
|
|
if (MotionMode < 0) {
|
|
throw parse_exception("no motion mode active", line_number);
|
|
}
|
|
|
|
double unit_multiplier = (state.unit == parse_unit::metric) ? 1 : 25.4;
|
|
glm::vec<3, double> end_pos = state.position;
|
|
auto StartValid = state.position_valid[0] && state.position_valid[1] && state.position_valid[2];
|
|
if (state.distance_mode == parse_distance_mode::incremental && !StartValid) {
|
|
throw parse_exception(
|
|
"incremental motion is only allowed after an absolute position has been established (eg. with \"G90 G0 X0 Y0 Z5\")",
|
|
line_number);
|
|
}
|
|
|
|
if ((MotionMode == 2 || MotionMode == 3) && !StartValid) {
|
|
throw parse_exception(
|
|
"arcs (G2/G3) are only allowed after an absolute position has been established (eg. with \"G90 G0 X0 Y0 Z5\")",
|
|
line_number);
|
|
}
|
|
|
|
{
|
|
float incremental = (state.distance_mode == parse_distance_mode::incremental) ? 1 : 0;
|
|
|
|
it = words.begin();
|
|
while (it != words.end()) {
|
|
if ((*it).command != 'X') {
|
|
it++;
|
|
continue;
|
|
}
|
|
|
|
end_pos.x = (*it).parameter * unit_multiplier + incremental * end_pos.x;
|
|
it = words.erase(it);
|
|
state.position_valid[0] = true;
|
|
break;
|
|
}
|
|
|
|
it = words.begin();
|
|
while (it != words.end()) {
|
|
if ((*it).command != 'Y') {
|
|
it++;
|
|
continue;
|
|
}
|
|
|
|
end_pos.y = (*it).parameter * unit_multiplier + incremental * end_pos.y;
|
|
it = words.erase(it);
|
|
state.position_valid[1] = true;
|
|
break;
|
|
}
|
|
|
|
it = words.begin();
|
|
while (it != words.end()) {
|
|
if ((*it).command != 'Z') {
|
|
it++;
|
|
continue;
|
|
}
|
|
|
|
end_pos.z = (*it).parameter * unit_multiplier + incremental * end_pos.z;
|
|
it = words.erase(it);
|
|
state.position_valid[2] = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (MotionMode != 0 && state.feed <= 0) {
|
|
throw parse_exception("feed rate undefined", line_number);
|
|
}
|
|
|
|
if (MotionMode == 1 && !StartValid) {
|
|
warnings.push_back(
|
|
"a feed move is used before an absolute position is established, height maps will not be applied to this motion. (line " +
|
|
std::to_string(line_number) + ")");
|
|
}
|
|
|
|
if (MotionMode <= 1) {
|
|
if (!words.empty()) {
|
|
warnings.push_back(
|
|
"motion command must be last in line (ignoring unused words (TOOD: show serialized words)in block). (line " +
|
|
std::to_string(line_number) + ")");
|
|
}
|
|
|
|
auto l = new line_motion_cmd;
|
|
l->start = state.position;
|
|
l->end = end_pos;
|
|
l->feed = state.feed;
|
|
l->rapid = MotionMode == 0;
|
|
l->line_number = line_number;
|
|
l->start_valid = StartValid;
|
|
|
|
std::memcpy(l->position_valid, state.position_valid, sizeof(bool) * 3);
|
|
|
|
commands.push_back(std::shared_ptr<line_motion_cmd>(l));
|
|
state.position = end_pos;
|
|
return;
|
|
}
|
|
|
|
double U, V;
|
|
bool ijk_used = false;
|
|
|
|
switch (state.plane) {
|
|
case arc_plane::yz:
|
|
U = state.position.y;
|
|
V = state.position.z;
|
|
break;
|
|
case arc_plane::zx:
|
|
U = state.position.z;
|
|
V = state.position.x;
|
|
break;
|
|
default:
|
|
U = state.position.x;
|
|
V = state.position.y;
|
|
break;
|
|
}
|
|
|
|
// find IJK
|
|
{
|
|
float arc_incremental = (state.arc_distance_mode == parse_distance_mode::incremental) ? 1 : 0;
|
|
|
|
it = words.begin();
|
|
while (it != words.end()) {
|
|
if ((*it).command != 'I') {
|
|
it++;
|
|
continue;
|
|
}
|
|
|
|
switch (state.plane) {
|
|
case arc_plane::xy:
|
|
U = (*it).parameter * unit_multiplier + arc_incremental * state.position.x;
|
|
break;
|
|
case arc_plane::yz:
|
|
throw parse_exception("current plane is YZ, I word is invalid", line_number);
|
|
case arc_plane::zx:
|
|
V = (*it).parameter * unit_multiplier + arc_incremental * state.position.x;
|
|
break;
|
|
}
|
|
|
|
ijk_used = true;
|
|
it = words.erase(it);
|
|
break;
|
|
}
|
|
|
|
it = words.begin();
|
|
while (it != words.end()) {
|
|
if ((*it).command != 'J') {
|
|
it++;
|
|
continue;
|
|
}
|
|
|
|
switch (state.plane) {
|
|
case arc_plane::xy:
|
|
V = (*it).parameter * unit_multiplier + arc_incremental * state.position.y;
|
|
break;
|
|
case arc_plane::yz:
|
|
U = (*it).parameter * unit_multiplier + arc_incremental * state.position.y;
|
|
break;
|
|
case arc_plane::zx:
|
|
throw parse_exception("current plane is ZX, J word is invalid", line_number);
|
|
}
|
|
|
|
ijk_used = true;
|
|
it = words.erase(it);
|
|
break;
|
|
}
|
|
|
|
it = words.begin();
|
|
while (it != words.end()) {
|
|
if ((*it).command != 'K') {
|
|
it++;
|
|
continue;
|
|
}
|
|
|
|
switch (state.plane) {
|
|
case arc_plane::xy:
|
|
throw parse_exception("current plane is XY, K word is invalid", line_number);
|
|
break;
|
|
case arc_plane::yz:
|
|
V = (*it).parameter * unit_multiplier + arc_incremental * state.position.z;
|
|
break;
|
|
case arc_plane::zx:
|
|
U = (*it).parameter * unit_multiplier + arc_incremental * state.position.z;
|
|
break;
|
|
}
|
|
|
|
ijk_used = true;
|
|
it = words.erase(it);
|
|
break;
|
|
}
|
|
|
|
// resolve radius
|
|
it = words.begin();
|
|
while (it != words.end()) {
|
|
if ((*it).command != 'R') {
|
|
it++;
|
|
continue;
|
|
}
|
|
|
|
if (ijk_used) {
|
|
throw parse_exception("both IJK and R notation used", line_number);
|
|
}
|
|
|
|
if (state.position == end_pos) {
|
|
throw parse_exception("arcs in R-notation must have non-coincident start and end points",
|
|
line_number);
|
|
}
|
|
|
|
double radius = (*it).parameter * unit_multiplier;
|
|
if (radius == 0)
|
|
throw parse_exception("radius can't be zero", line_number);
|
|
|
|
double A, B;
|
|
switch (state.plane) {
|
|
case arc_plane::yz:
|
|
A = end_pos.y;
|
|
B = end_pos.z;
|
|
break;
|
|
case arc_plane::zx:
|
|
A = end_pos.z;
|
|
B = end_pos.x;
|
|
break;
|
|
default:
|
|
A = end_pos.x;
|
|
B = end_pos.y;
|
|
break;
|
|
}
|
|
|
|
A -= U; //(AB) = vector from start to end of arc along the axes of the current plane
|
|
B -= V;
|
|
|
|
// see grbl/gcode.c
|
|
double h_x2_div_d = 4.0 * (radius * radius) - (A * A + B * B);
|
|
if (h_x2_div_d < 0) {
|
|
throw parse_exception("arc radius too small to reach both ends", line_number);
|
|
}
|
|
|
|
h_x2_div_d = -sqrt(h_x2_div_d) / sqrt(A * A + B * B);
|
|
if (MotionMode == 3 ^ radius < 0) {
|
|
h_x2_div_d = -h_x2_div_d;
|
|
}
|
|
|
|
U += 0.5 * (A - (B * h_x2_div_d));
|
|
V += 0.5 * (B + (A * h_x2_div_d));
|
|
|
|
it = words.erase(it);
|
|
break;
|
|
}
|
|
|
|
if (!words.empty()) {
|
|
warnings.push_back(
|
|
"motion command must be last in line (ignoring unused words (add serialized words here) in block). (line " +
|
|
std::to_string(line_number) + ")");
|
|
}
|
|
|
|
auto a = new arc_motion_cmd;
|
|
a->start = state.position;
|
|
a->end = end_pos;
|
|
a->feed = state.feed;
|
|
a->direction = (MotionMode == 2) ? arc_direction::cw : arc_direction::ccw;
|
|
a->u = U;
|
|
a->v = V;
|
|
a->line_number = line_number;
|
|
a->plane = state.plane;
|
|
|
|
commands.push_back(std::shared_ptr<arc_motion_cmd>(a));
|
|
state.position = end_pos;
|
|
return;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
grbl::grbl_parser::grbl_parser() {
|
|
reset();
|
|
}
|
|
|
|
void grbl::grbl_parser::reset() {
|
|
state = parser_state{};
|
|
}
|