X-Git-Url: http://git.hcoop.net/clinton/Smoothieware.git/blobdiff_plain/fd2341bc84258107b2e43282bc86b3cc8a8712f7..8c2f95f9f7a20a41d315a894f2e229e79a125435:/src/modules/tools/zprobe/ZProbe.cpp diff --git a/src/modules/tools/zprobe/ZProbe.cpp b/src/modules/tools/zprobe/ZProbe.cpp index e3bc6ae4..23c6b751 100644 --- a/src/modules/tools/zprobe/ZProbe.cpp +++ b/src/modules/tools/zprobe/ZProbe.cpp @@ -31,6 +31,7 @@ #include "DeltaCalibrationStrategy.h" #include "ThreePointStrategy.h" #include "DeltaGridStrategy.h" +#include "CartGridStrategy.h" #define enable_checksum CHECKSUM("enable") #define probe_pin_checksum CHECKSUM("probe_pin") @@ -40,7 +41,9 @@ #define return_feedrate_checksum CHECKSUM("return_feedrate") #define probe_height_checksum CHECKSUM("probe_height") #define gamma_max_checksum CHECKSUM("gamma_max") +#define max_z_checksum CHECKSUM("max_z") #define reverse_z_direction_checksum CHECKSUM("reverse_z") +#define dwell_before_probing_checksum CHECKSUM("dwell_before_probing") // from endstop section #define delta_homing_checksum CHECKSUM("delta_homing") @@ -54,8 +57,6 @@ #define STEPS_PER_MM(a) (STEPPER[a]->get_steps_per_mm()) #define Z_STEPS_PER_MM STEPS_PER_MM(Z_AXIS) -#define abs(a) ((a<0) ? -a : a) - void ZProbe::on_module_loaded() { // if the module is disabled -> do nothing @@ -70,7 +71,7 @@ void ZProbe::on_module_loaded() // register event-handlers register_for_event(ON_GCODE_RECEIVED); - // we read the probe in this timer, currently only for G38 probes. + // we read the probe in this timer probing= false; THEKERNEL->slow_ticker->attach(1000, this, &ZProbe::read_probe); } @@ -86,25 +87,38 @@ void ZProbe::config_load() for( auto cs : modules ){ if( THEKERNEL->config->value(leveling_strategy_checksum, cs, enable_checksum )->as_bool() ){ bool found= false; + LevelingStrategy *ls= nullptr; + // check with each known strategy and load it if it matches switch(cs) { case delta_calibration_strategy_checksum: - this->strategies.push_back(new DeltaCalibrationStrategy(this)); + ls= new DeltaCalibrationStrategy(this); found= true; break; case three_point_leveling_strategy_checksum: // NOTE this strategy is mutually exclusive with the delta calibration strategy - this->strategies.push_back(new ThreePointStrategy(this)); + ls= new ThreePointStrategy(this); found= true; break; case delta_grid_leveling_strategy_checksum: - this->strategies.push_back(new DeltaGridStrategy(this)); + ls= new DeltaGridStrategy(this); + found= true; + break; + + case cart_grid_leveling_strategy_checksum: + ls= new CartGridStrategy(this); found= true; break; } - if(found) this->strategies.back()->handleConfig(); + if(found) { + if(ls->handleConfig()) { + this->strategies.push_back(ls); + }else{ + delete ls; + } + } } } @@ -126,7 +140,12 @@ void ZProbe::config_load() this->fast_feedrate = THEKERNEL->config->value(zprobe_checksum, fast_feedrate_checksum)->by_default(100)->as_number(); // feedrate in mm/sec this->return_feedrate = THEKERNEL->config->value(zprobe_checksum, return_feedrate_checksum)->by_default(0)->as_number(); // feedrate in mm/sec this->reverse_z = THEKERNEL->config->value(zprobe_checksum, reverse_z_direction_checksum)->by_default(false)->as_bool(); // Z probe moves in reverse direction - this->max_z = THEKERNEL->config->value(gamma_max_checksum)->by_default(500)->as_number(); // maximum zprobe distance + this->max_z = THEKERNEL->config->value(zprobe_checksum, max_z_checksum)->by_default(NAN)->as_number(); // maximum zprobe distance + if(isnan(this->max_z)){ + this->max_z = THEKERNEL->config->value(gamma_max_checksum)->by_default(200)->as_number(); // maximum zprobe distance + } + this->dwell_before_probing = THEKERNEL->config->value(zprobe_checksum, dwell_before_probing_checksum)->by_default(0)->as_number(); // dwell time in seconds before probing + } uint32_t ZProbe::read_probe(uint32_t dummy) @@ -136,7 +155,7 @@ uint32_t ZProbe::read_probe(uint32_t dummy) // we check all axis as it maybe a G38.2 X10 for instance, not just a probe in Z if(STEPPER[X_AXIS]->is_moving() || STEPPER[Y_AXIS]->is_moving() || STEPPER[Z_AXIS]->is_moving()) { // if it is moving then we check the probe, and debounce it - if(this->pin.get()) { + if(this->pin.get() != invert_probe) { if(debounce < debounce_ms) { debounce++; } else { @@ -160,6 +179,13 @@ uint32_t ZProbe::read_probe(uint32_t dummy) // returns boolean value indicating if probe was triggered bool ZProbe::run_probe(float& mm, float feedrate, float max_dist, bool reverse) { + if(dwell_before_probing > .0001F) safe_delay_ms(dwell_before_probing*1000); + + if(this->pin.get()) { + // probe already triggered so abort + return false; + } + float maxz= max_dist < 0 ? this->max_z*2 : max_dist; probing= true; @@ -167,11 +193,7 @@ bool ZProbe::run_probe(float& mm, float feedrate, float max_dist, bool reverse) debounce= 0; // save current actuator position so we can report how far we moved - ActuatorCoordinates start_pos{ - THEROBOT->actuators[X_AXIS]->get_current_position(), - THEROBOT->actuators[Y_AXIS]->get_current_position(), - THEROBOT->actuators[Z_AXIS]->get_current_position() - }; + float z_start_pos= THEROBOT->actuators[Z_AXIS]->get_current_position(); // move Z down bool dir= (!reverse_z != reverse); // xor @@ -184,15 +206,10 @@ bool ZProbe::run_probe(float& mm, float feedrate, float max_dist, bool reverse) // now see how far we moved, get delta in z we moved // NOTE this works for deltas as well as all three actuators move the same amount in Z - mm= start_pos[2] - THEROBOT->actuators[2]->get_current_position(); + mm= z_start_pos - THEROBOT->actuators[2]->get_current_position(); // set the last probe position to the actuator units moved during this home - THEROBOT->set_last_probe_position( - std::make_tuple( - start_pos[0] - THEROBOT->actuators[0]->get_current_position(), - start_pos[1] - THEROBOT->actuators[1]->get_current_position(), - mm, - probe_detected?1:0)); + THEROBOT->set_last_probe_position(std::make_tuple(0, 0, mm, probe_detected?1:0)); probing= false; @@ -207,8 +224,7 @@ bool ZProbe::run_probe(float& mm, float feedrate, float max_dist, bool reverse) // do probe then return to start position bool ZProbe::run_probe_return(float& mm, float feedrate, float max_dist, bool reverse) { - float save_pos[3]; - THEROBOT->get_axis_position(save_pos); + float save_z_pos= THEROBOT->get_axis_position(Z_AXIS); bool ok= run_probe(mm, feedrate, max_dist, reverse); @@ -222,7 +238,7 @@ bool ZProbe::run_probe_return(float& mm, float feedrate, float max_dist, bool re } // absolute move back to saved starting position - coordinated_move(save_pos[0], save_pos[1], save_pos[2], fr, false); + coordinated_move(NAN, NAN, save_z_pos, fr, false); return ok; } @@ -239,12 +255,14 @@ void ZProbe::on_gcode_received(void *argument) Gcode *gcode = static_cast(argument); if( gcode->has_g && gcode->g >= 29 && gcode->g <= 32) { - + + invert_probe = false; // make sure the probe is defined and not already triggered before moving motors if(!this->pin.connected()) { - gcode->stream->printf("ZProbe not connected.\n"); + gcode->stream->printf("ZProbe pin not configured.\n"); return; } + if(this->pin.get()) { gcode->stream->printf("ZProbe triggered before move, aborting command.\n"); return; @@ -265,7 +283,7 @@ void ZProbe::on_gcode_received(void *argument) if(probe_result) { // the result is in actuator coordinates moved - gcode->stream->printf("Z:%1.4f\n", mm); + gcode->stream->printf("Z:%1.4f\n", THEKERNEL->robot->from_millimeters(mm)); if(set_z) { // set current Z to the specified value, shortcut for G92 Znnn @@ -308,8 +326,8 @@ void ZProbe::on_gcode_received(void *argument) } else if(gcode->has_g && gcode->g == 38 ) { // G38.2 Straight Probe with error, G38.3 straight probe without error // linuxcnc/grbl style probe http://www.linuxcnc.org/docs/2.5/html/gcode/gcode.html#sec:G38-probe - if(gcode->subcode != 2 && gcode->subcode != 3) { - gcode->stream->printf("error:Only G38.2 and G38.3 are supported\n"); + if(gcode->subcode < 2 || gcode->subcode > 5) { + gcode->stream->printf("error:Only G38.2 to G38.5 are supported\n"); return; } @@ -319,7 +337,7 @@ void ZProbe::on_gcode_received(void *argument) return; } - if(this->pin.get()) { + if(this->pin.get() ^ (gcode->subcode >= 4)) { gcode->stream->printf("error:ZProbe triggered before move, aborting command.\n"); return; } @@ -327,22 +345,34 @@ void ZProbe::on_gcode_received(void *argument) // first wait for all moves to finish THEKERNEL->conveyor->wait_for_idle(); + float x= NAN, y=NAN, z=NAN; if(gcode->has_letter('X')) { - // probe in the X axis - probe_XYZ(gcode, X_AXIS); + x= gcode->get_value('X'); + } - }else if(gcode->has_letter('Y')) { - // probe in the Y axis - probe_XYZ(gcode, Y_AXIS); + if(gcode->has_letter('Y')) { + y= gcode->get_value('Y'); + } - }else if(gcode->has_letter('Z')) { - // probe in the Z axis - probe_XYZ(gcode, Z_AXIS); + if(gcode->has_letter('Z')) { + z= gcode->get_value('Z'); + } - }else{ + if(isnan(x) && isnan(y) && isnan(z)) { gcode->stream->printf("error:at least one of X Y or Z must be specified\n"); + return; + } + + if(gcode->subcode == 4 || gcode->subcode == 5) { + invert_probe = true; + } else { + invert_probe = false; } + probe_XYZ(gcode, x, y, z); + + invert_probe = false; + return; } else if(gcode->has_m) { @@ -365,12 +395,13 @@ void ZProbe::on_gcode_received(void *argument) invert_override= (gcode->get_value('I') != 0); pin.set_inverting(pin.is_inverting() != invert_override); // XOR so inverted pin is not inverted and vice versa } + if (gcode->has_letter('D')) this->dwell_before_probing = gcode->get_value('D'); break; case 500: // save settings case 503: // print settings - gcode->stream->printf(";Probe feedrates Slow/fast(K)/Return (mm/sec) max_z (mm) height (mm):\nM670 S%1.2f K%1.2f R%1.2f Z%1.2f H%1.2f\n", - this->slow_feedrate, this->fast_feedrate, this->return_feedrate, this->max_z, this->probe_height); + gcode->stream->printf(";Probe feedrates Slow/fast(K)/Return (mm/sec) max_z (mm) height (mm) dwell (s):\nM670 S%1.2f K%1.2f R%1.2f Z%1.2f H%1.2f D%1.2f\n", + this->slow_feedrate, this->fast_feedrate, this->return_feedrate, this->max_z, this->probe_height, this->dwell_before_probing); // fall through is intended so leveling strategies can handle m-codes too @@ -385,22 +416,18 @@ void ZProbe::on_gcode_received(void *argument) } // special way to probe in the X or Y or Z direction using planned moves, should work with any kinematics -void ZProbe::probe_XYZ(Gcode *gcode, int axis) +void ZProbe::probe_XYZ(Gcode *gcode, float x, float y, float z) { // enable the probe checking in the timer probing= true; probe_detected= false; THEROBOT->disable_segmentation= true; // we must disable segmentation as this won't work with it enabled (beware on deltas probing in X or Y) - // get probe feedrate if specified - float rate = (gcode->has_letter('F')) ? gcode->get_value('F')*60 : this->slow_feedrate; + // get probe feedrate in mm/min and convert to mm/sec if specified + float rate = (gcode->has_letter('F')) ? gcode->get_value('F')/60 : this->slow_feedrate; // do a regular move which will stop as soon as the probe is triggered, or the distance is reached - switch(axis) { - case X_AXIS: coordinated_move(gcode->get_value('X'), 0, 0, rate, true); break; - case Y_AXIS: coordinated_move(0, gcode->get_value('Y'), 0, rate, true); break; - case Z_AXIS: coordinated_move(0, 0, gcode->get_value('Z'), rate, true); break; - } + coordinated_move(x, y, z, rate, true); // coordinated_move returns when the move is finished @@ -417,12 +444,12 @@ void ZProbe::probe_XYZ(Gcode *gcode, int axis) uint8_t probeok= this->probe_detected ? 1 : 0; // print results using the GRBL format - gcode->stream->printf("[PRB:%1.3f,%1.3f,%1.3f:%d]\n", pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], probeok); + gcode->stream->printf("[PRB:%1.3f,%1.3f,%1.3f:%d]\n", THEKERNEL->robot->from_millimeters(pos[X_AXIS]), THEKERNEL->robot->from_millimeters(pos[Y_AXIS]), THEKERNEL->robot->from_millimeters(pos[Z_AXIS]), probeok); THEROBOT->set_last_probe_position(std::make_tuple(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], probeok)); - if(probeok == 0 && gcode->subcode == 2) { - // issue error if probe was not triggered and subcode == 2 - gcode->stream->printf("ALARM:Probe fail\n"); + if(probeok == 0 && (gcode->subcode == 2 || gcode->subcode == 4)) { + // issue error if probe was not triggered and subcode is 2 or 4 + gcode->stream->printf("ALARM: Probe fail\n"); THEKERNEL->call_event(ON_HALT, nullptr); } } @@ -432,38 +459,46 @@ void ZProbe::probe_XYZ(Gcode *gcode, int axis) // NOTE must use G53 to force move in machine coordinates and ignore any WCS offsets void ZProbe::coordinated_move(float x, float y, float z, float feedrate, bool relative) { - char buf[32]; - char cmd[64]; + #define CMDLEN 128 + char *cmd= new char[CMDLEN]; // use heap here to reduce stack usage if(relative) strcpy(cmd, "G91 G0 "); else strcpy(cmd, "G53 G0 "); // G53 forces movement in machine coordinate system if(!isnan(x)) { - int n = snprintf(buf, sizeof(buf), " X%1.3f", x); - strncat(cmd, buf, n); + size_t n= strlen(cmd); + snprintf(&cmd[n], CMDLEN-n, " X%1.3f", x); } if(!isnan(y)) { - int n = snprintf(buf, sizeof(buf), " Y%1.3f", y); - strncat(cmd, buf, n); + size_t n= strlen(cmd); + snprintf(&cmd[n], CMDLEN-n, " Y%1.3f", y); } if(!isnan(z)) { - int n = snprintf(buf, sizeof(buf), " Z%1.3f", z); - strncat(cmd, buf, n); + size_t n= strlen(cmd); + snprintf(&cmd[n], CMDLEN-n, " Z%1.3f", z); + } + + { + size_t n= strlen(cmd); + // use specified feedrate (mm/sec) + snprintf(&cmd[n], CMDLEN-n, " F%1.1f", feedrate * 60); // feed rate is converted to mm/min } - // use specified feedrate (mm/sec) - int n = snprintf(buf, sizeof(buf), " F%1.1f", feedrate * 60); // feed rate is converted to mm/min - strncat(cmd, buf, n); if(relative) strcat(cmd, " G90"); - //THEKERNEL->streams->printf("DEBUG: move: %s\n", cmd); + //THEKERNEL->streams->printf("DEBUG: move: %s: %u\n", cmd, strlen(cmd)); // send as a command line as may have multiple G codes in it + THEROBOT->push_state(); struct SerialMessage message; message.message = cmd; + delete [] cmd; + message.stream = &(StreamOutput::NullStream); THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message ); THEKERNEL->conveyor->wait_for_idle(); + THEROBOT->pop_state(); + } // issue home command