From fd2341bc84258107b2e43282bc86b3cc8a8712f7 Mon Sep 17 00:00:00 2001 From: Jim Morris Date: Sat, 1 Oct 2016 00:09:03 -0700 Subject: [PATCH] Fix reset_axis_position for homing. we are given the post compensation position sdo use inverse transform to get last milestone Change the way G30 Zxxx works. so it actually just does a G92 Zxxx after the probe --- src/modules/robot/Robot.cpp | 13 +++++++------ src/modules/tools/endstops/Endstops.cpp | 2 ++ src/modules/tools/zprobe/ZProbe.cpp | 8 ++++++-- 3 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp index a0745b1c..3a2a86c3 100644 --- a/src/modules/robot/Robot.cpp +++ b/src/modules/robot/Robot.cpp @@ -866,22 +866,23 @@ void Robot::process_move(Gcode *gcode, enum MOTION_MODE_T motion_mode) } // reset the machine position for all axis. Used for homing. -// after homing we need to set the actuator position at the position they would be at for the compensated XYZ -// So we need to apply the compensation transform to the last_milestone we are given to get the compensated -// last_machine_position which we then convert to actuator position. +// after homing we supply the cartesian coordinates that the head is at when homed, +// however for Z this is the compensated Z position (if enabled) +// So we need to apply the inverse compensation transform to the supplied coordinates to get the correct last milestone // this will make the results from M114 and ? consistent after homing. void Robot::reset_axis_position(float x, float y, float z) { - // these are set to the same as compensation was not used to get to the current position + // set both the same initially last_machine_position[X_AXIS]= last_milestone[X_AXIS] = x; last_machine_position[Y_AXIS]= last_milestone[Y_AXIS] = y; last_machine_position[Z_AXIS]= last_milestone[Z_AXIS] = z; if(compensationTransform) { - compensationTransform(last_machine_position, false); + // apply inverse transform to get last_milestone + compensationTransform(last_milestone, true); } - // now set the actuator positions to match + // now set the actuator positions based on the supplied compensated position ActuatorCoordinates actuator_pos; arm_solution->cartesian_to_actuator(this->last_machine_position, actuator_pos); for (size_t i = X_AXIS; i <= Z_AXIS; i++) diff --git a/src/modules/tools/endstops/Endstops.cpp b/src/modules/tools/endstops/Endstops.cpp index 546d25ee..1193dc8f 100644 --- a/src/modules/tools/endstops/Endstops.cpp +++ b/src/modules/tools/endstops/Endstops.cpp @@ -719,6 +719,8 @@ void Endstops::process_home_command(Gcode* gcode) } else { // Zero the ax(i/e)s position, add in the home offset + // NOTE that if compensation is active the Z will be set based on where XY are, so make sure XY are homed first then Z + // so XY are at a known consistent position. (especially true if using a proximity probe) for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { if (haxis[c]) { // if we requested this axis to home THEROBOT->reset_axis_position(this->homing_position[c] + this->home_offset[c], c); diff --git a/src/modules/tools/zprobe/ZProbe.cpp b/src/modules/tools/zprobe/ZProbe.cpp index e56a87aa..e3bc6ae4 100644 --- a/src/modules/tools/zprobe/ZProbe.cpp +++ b/src/modules/tools/zprobe/ZProbe.cpp @@ -268,8 +268,12 @@ void ZProbe::on_gcode_received(void *argument) gcode->stream->printf("Z:%1.4f\n", mm); if(set_z) { - // set Z to the specified value - THEROBOT->reset_axis_position(gcode->get_value('Z'), Z_AXIS); + // set current Z to the specified value, shortcut for G92 Znnn + char buf[32]; + int n = snprintf(buf, sizeof(buf), "G92 Z%f", gcode->get_value('Z')); + string g(buf, n); + Gcode gc(g, &(StreamOutput::NullStream)); + THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc); } } else { -- 2.20.1