Permalink
Browse files

Detect Estop in PRU and abort motion for arc and bezier moves.

Signed-off-by: H Hartley Sweeten <hsweeten@visionengravers.com>
  • Loading branch information...
bigguiness authored and hzeller committed Oct 16, 2018
1 parent 31451dc commit 3c66fc55976e830207c8338eec9ad5b85a3b8edb
Showing with 32 additions and 32 deletions.
  1. +20 −20 src/gcode-parser/arc-gen.cc
  2. +5 −5 src/gcode-parser/gcode-parser.cc
  3. +2 −2 src/gcode-parser/gcode-parser.h
  4. +5 −5 src/gcode2ps.cc
@@ -45,20 +45,20 @@
#define MM_PER_ARC_SEGMENT 0.1
// Generate an arc. Input is the
static void arc_gen(enum GCodeParserAxis normal_axis, // Normal axis
static bool arc_gen(enum GCodeParserAxis normal_axis, // Normal axis
bool is_cw, // 0 CCW, 1 CW
AxesRegister *position_out, // start position. Will be updated.
const AxesRegister &center, // Offset to center.
const AxesRegister &target, // Target position.
std::function<void(const AxesRegister&)> segment_output) {
std::function<bool(const AxesRegister&)> segment_output) {
// Depending on the normal vector, pre-calc plane
enum GCodeParserAxis plane[3];
switch (normal_axis) {
case AXIS_Z: plane[0] = AXIS_X; plane[1] = AXIS_Y; plane[2] = AXIS_Z; break;
case AXIS_X: plane[0] = AXIS_Y; plane[1] = AXIS_Z; plane[2] = AXIS_X; break;
case AXIS_Y: plane[0] = AXIS_X; plane[1] = AXIS_Z; plane[2] = AXIS_Y; break;
default:
return; // Invalid axis.
return true; // Invalid axis (ignore move).
}
// Alias reference for readable use with [] operator
@@ -99,7 +99,7 @@ static void arc_gen(enum GCodeParserAxis normal_axis, // Normal axis
// We don't care about non-XYZ moves (e.g. extruder)
if (mm_of_travel < 0.00001)
return;
return true; // (ignore move)
// Figure out how many segments for this gcode
const int segments = floorf(mm_of_travel / MM_PER_ARC_SEGMENT);
@@ -119,14 +119,14 @@ static void arc_gen(enum GCodeParserAxis normal_axis, // Normal axis
position[plane[2]] += linear_per_segment;
// Emit
segment_output(position);
if (!segment_output(position)) return false;
}
// Ensure last segment arrives at target location.
for (int axis = AXIS_X; axis <= AXIS_Z; axis++) {
position[(GCodeParserAxis)axis] = target[(GCodeParserAxis)axis];
}
segment_output(position);
return segment_output(position);
}
static AxesRegister calc_bezier_point(float t,
@@ -152,11 +152,11 @@ static AxesRegister calc_bezier_point(float t,
return p;
}
static void spline_gen(const AxesRegister &start,
static bool spline_gen(const AxesRegister &start,
const AxesRegister &cp1,
const AxesRegister &cp2,
const AxesRegister &target,
std::function<void (const AxesRegister& pos)> segment_output) {
std::function<bool(const AxesRegister& pos)> segment_output) {
#if 0
Log_debug("spline_gen: start:%.3f,%.3f cp1:%.3f,%.3f cp2:%.3f,%.3f end:%.3f,%.3f\n",
position[AXIS_X], position[AXIS_Y],
@@ -166,31 +166,31 @@ static void spline_gen(const AxesRegister &start,
#endif
for (float t = 0; t < 1; t += 0.01f) {
segment_output(calc_bezier_point(t, start, cp1, cp2, target));
if (!segment_output(calc_bezier_point(t, start, cp1, cp2, target))) return false;
}
segment_output(target);
return segment_output(target);
}
void GCodeParser::EventReceiver::arc_move(float feed_mm_p_sec,
bool GCodeParser::EventReceiver::arc_move(float feed_mm_p_sec,
GCodeParserAxis normal_axis,
bool clockwise,
const AxesRegister &start,
const AxesRegister &center,
const AxesRegister &end) {
AxesRegister position = start;
arc_gen(normal_axis, clockwise, &position,
center, end, [this, feed_mm_p_sec](const AxesRegister &pos) {
coordinated_move(feed_mm_p_sec, pos);
});
return arc_gen(normal_axis, clockwise, &position,
center, end, [this, feed_mm_p_sec](const AxesRegister &pos) {
return coordinated_move(feed_mm_p_sec, pos);
});
}
void GCodeParser::EventReceiver::spline_move(float feed_mm_p_sec,
bool GCodeParser::EventReceiver::spline_move(float feed_mm_p_sec,
const AxesRegister &start,
const AxesRegister &cp1,
const AxesRegister &cp2,
const AxesRegister &end) {
spline_gen(start, cp1, cp2, end,
[this, feed_mm_p_sec](const AxesRegister &pos) {
coordinated_move(feed_mm_p_sec, pos);
});
return spline_gen(start, cp1, cp2, end,
[this, feed_mm_p_sec](const AxesRegister &pos) {
return coordinated_move(feed_mm_p_sec, pos);
});
}
@@ -1639,9 +1639,9 @@ const char *GCodeParser::Impl::handle_arc(const char *line, bool is_cw) {
absolute_center[AXIS_X] += offset[AXIS_X];
absolute_center[AXIS_Y] += offset[AXIS_Y];
absolute_center[AXIS_Z] += offset[AXIS_Z];
callbacks()->arc_move(feedrate, arc_normal_, is_cw,
axes_pos_, absolute_center, target);
axes_pos_ = target;
if (callbacks()->arc_move(feedrate, arc_normal_, is_cw,
axes_pos_, absolute_center, target))
axes_pos_ = target;
return line;
}
@@ -1791,8 +1791,8 @@ const char *GCodeParser::Impl::handle_spline(float sub_command, const char *line
cp2 = _cp2;
}
callbacks()->spline_move(-1, axes_pos_, cp1, cp2, target);
axes_pos_ = target;
if (callbacks()->spline_move(-1, axes_pos_, cp1, cp2, target))
axes_pos_ = target;
return line;
}
@@ -272,7 +272,7 @@ class GCodeParser::EventReceiver {
// TODO(hzeller): We could probably generalize this by having a
// 'normal vector' instead of normal_axis + clockwise. This would allow for
// arbitrarily placed arcs in space (but there is no GCode for it).
virtual void arc_move(float feed_mm_p_sec,
virtual bool arc_move(float feed_mm_p_sec,
GCodeParserAxis normal_axis, bool clockwise,
const AxesRegister &start,
const AxesRegister &center,
@@ -283,7 +283,7 @@ class GCodeParser::EventReceiver {
// control points "cp1" and "cp2".
// The default implementation linearlizes curve and calls coordinated_move()
// with the segments.
virtual void spline_move(float feed_mm_p_sec,
virtual bool spline_move(float feed_mm_p_sec,
const AxesRegister &start,
const AxesRegister &cp1, const AxesRegister &cp2,
const AxesRegister &end);
@@ -229,7 +229,7 @@ class GCodePrintVisualizer : public GCodeParser::EventReceiver {
return true;
}
void arc_move(float feed_mm_p_sec,
bool arc_move(float feed_mm_p_sec,
GCodeParserAxis normal_axis, bool clockwise,
const AxesRegister &start,
const AxesRegister &center,
@@ -248,11 +248,11 @@ class GCodePrintVisualizer : public GCodeParser::EventReceiver {
end[AXIS_X], end[AXIS_Y], end[AXIS_Z]);
}
// Let the standard arc generation happen.
EventReceiver::arc_move(feed_mm_p_sec, normal_axis, clockwise,
start, center, end);
return EventReceiver::arc_move(feed_mm_p_sec, normal_axis, clockwise,
start, center, end);
}
void spline_move(float feed_mm_p_sec,
bool spline_move(float feed_mm_p_sec,
const AxesRegister &start,
const AxesRegister &cp1, const AxesRegister &cp2,
const AxesRegister &end) final {
@@ -273,7 +273,7 @@ class GCodePrintVisualizer : public GCodeParser::EventReceiver {
cp2[AXIS_X], cp2[AXIS_Y], start[AXIS_Z],
end[AXIS_X], end[AXIS_Y], end[AXIS_Z]);
}
EventReceiver::spline_move(feed_mm_p_sec, start, cp1, cp2, end);
return EventReceiver::spline_move(feed_mm_p_sec, start, cp1, cp2, end);
}
void gcode_command_done(char letter, float val) final {

0 comments on commit 3c66fc5

Please sign in to comment.