From 61a3fa99f5b0fb48230188f63be6a4c5b6d3565b Mon Sep 17 00:00:00 2001 From: Jim Morris Date: Sun, 13 Dec 2015 20:23:15 -0800 Subject: [PATCH] Make G92 work as expected based on current WCS position --- src/modules/robot/Robot.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp index 1629271e..3d5de934 100644 --- a/src/modules/robot/Robot.cpp +++ b/src/modules/robot/Robot.cpp @@ -395,12 +395,19 @@ void Robot::on_gcode_received(void *argument) g92_offset = wcs_t(0, 0, 0); } else { - // standard setting of the g92 offsets, making current machine position whatever the coordinate arguments are + // standard setting of the g92 offsets, making current WCS position whatever the coordinate arguments are float x, y, z; std::tie(x, y, z) = g92_offset; - if(gcode->has_letter('X')) x = to_millimeters(gcode->get_value('X')) - last_milestone[X_AXIS]; - if(gcode->has_letter('Y')) y = to_millimeters(gcode->get_value('Y')) - last_milestone[Y_AXIS]; - if(gcode->has_letter('Z')) z = to_millimeters(gcode->get_value('Z')) - last_milestone[Z_AXIS]; + // get current position in WCS + wcs_t pos= mcs2wcs(last_milestone); + + // adjust g92 offset to make the current wpos == the value requested + if(gcode->has_letter('X')){ + x += to_millimeters(gcode->get_value('X')) - std::get(pos); + } + + if(gcode->has_letter('Y')) y = to_millimeters(gcode->get_value('Y')) - last_milestone[Y_AXIS] - std::get(wcs_offsets[current_wcs]) - std::get(tool_offset); + if(gcode->has_letter('Z')) z = to_millimeters(gcode->get_value('Z')) - last_milestone[Z_AXIS] - std::get(wcs_offsets[current_wcs]) - std::get(tool_offset); g92_offset = wcs_t(x, y, z); } -- 2.20.1