122 lines
3.2 KiB
C++
122 lines
3.2 KiB
C++
#include "gcode_commands.h"
|
|
#include "glm/geometric.hpp"
|
|
|
|
glm::vec<3, double> grbl::roll_components(glm::vec<3, double> v, int turns) {
|
|
glm::vec<3, double> roll{};
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
roll[i] = v[(i - turns + 300) % 3];
|
|
}
|
|
|
|
return roll;
|
|
}
|
|
|
|
grbl::mcode_cmd::mcode_cmd(int code, int line)
|
|
: code(code) {
|
|
command::line_number = line;
|
|
}
|
|
|
|
grbl::spindle_cmd::spindle_cmd(double s, int line)
|
|
: speed(s) {
|
|
command::line_number = line;
|
|
}
|
|
|
|
grbl::dwell_cmd::dwell_cmd(double s, int line)
|
|
: seconds(s) {
|
|
command::line_number = line;
|
|
}
|
|
|
|
double grbl::line_motion_cmd::length() const {
|
|
if (!start_valid || !position_valid[0] || !position_valid[1] || !position_valid[2])
|
|
return 0;
|
|
|
|
return glm::length(delta());
|
|
}
|
|
|
|
glm::vec<3, double> grbl::line_motion_cmd::interpolate(double ratio) {
|
|
return start + delta() * ratio;
|
|
}
|
|
|
|
std::vector<std::shared_ptr<grbl::motion_cmd>> 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<grbl::motion_cmd>(this)};
|
|
}
|
|
|
|
std::vector<std::shared_ptr<motion_cmd>> result;
|
|
int divisions = (int) std::ceil(length() / lngth);
|
|
|
|
if (divisions < 1) {
|
|
divisions = 1;
|
|
}
|
|
|
|
glm::vec3 last_end = start;
|
|
|
|
for (int i = 1; i <= divisions; i++) {
|
|
glm::vec3 end = interpolate(((double) i) / divisions);
|
|
|
|
auto immediate = new line_motion_cmd;
|
|
immediate->start = last_end;
|
|
immediate->end = end;
|
|
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<line_motion_cmd>(immediate));
|
|
|
|
last_end = end;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
double grbl::arc_motion_cmd::length() const {
|
|
return abs(angle_span() * radius());
|
|
}
|
|
|
|
glm::vec<3, double> grbl::arc_motion_cmd::interpolate(double ratio) {
|
|
double angle = start_angle() + angle_span() * ratio;
|
|
|
|
glm::vec<3, double> onPlane = {
|
|
u + (radius() * cos(angle)),
|
|
v + (radius() * sin(angle)),
|
|
0
|
|
};
|
|
|
|
auto d = delta() * ratio;
|
|
double helix = grbl::roll_components(start + d, -(int) plane).z;
|
|
|
|
onPlane.z = helix;
|
|
|
|
glm::vec3 interpolation = roll_components(onPlane, (int) plane);
|
|
return interpolation;
|
|
}
|
|
|
|
std::vector<std::shared_ptr<grbl::motion_cmd>> grbl::arc_motion_cmd::split(double lngth) {
|
|
int divisions = (int) ceil(length() / lngth);
|
|
|
|
if (divisions < 1)
|
|
divisions = 1;
|
|
|
|
glm::vec3 lastEnd = start;
|
|
|
|
|
|
std::vector<std::shared_ptr<grbl::motion_cmd>> result;
|
|
|
|
for (int i = 1; i <= divisions; i++) {
|
|
glm::vec<3, double> end = interpolate(((double) i) / divisions);
|
|
|
|
auto immediate = new arc_motion_cmd;
|
|
immediate->start = lastEnd;
|
|
immediate->end = end;
|
|
immediate->feed = feed;
|
|
immediate->direction = direction;
|
|
immediate->plane = plane;
|
|
immediate->u = u;
|
|
immediate->v = v;
|
|
|
|
result.push_back(std::shared_ptr<arc_motion_cmd>(immediate));
|
|
|
|
lastEnd = end;
|
|
}
|
|
return result;
|
|
}
|