X-Git-Url: http://git.hcoop.net/clinton/Smoothieware.git/blobdiff_plain/eb0d75d33ccdda68f8dc19fb3446175ea95b2da1..8c2f95f9f7a20a41d315a894f2e229e79a125435:/src/modules/tools/zprobe/ZProbe.cpp diff --git a/src/modules/tools/zprobe/ZProbe.cpp b/src/modules/tools/zprobe/ZProbe.cpp index 51a2b1f2..23c6b751 100644 --- a/src/modules/tools/zprobe/ZProbe.cpp +++ b/src/modules/tools/zprobe/ZProbe.cpp @@ -41,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") @@ -138,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) @@ -148,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 { @@ -172,6 +179,8 @@ 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; @@ -246,7 +255,8 @@ 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 pin not configured.\n"); @@ -273,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 @@ -316,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; } @@ -327,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; } @@ -335,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) { @@ -373,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 @@ -393,7 +416,7 @@ 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; @@ -404,11 +427,7 @@ void ZProbe::probe_XYZ(Gcode *gcode, int axis) 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 @@ -425,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); } } @@ -440,28 +459,29 @@ 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 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)) { size_t n= strlen(cmd); - snprintf(&cmd[n], sizeof(cmd)-n, " X%1.3f", x); + snprintf(&cmd[n], CMDLEN-n, " X%1.3f", x); } if(!isnan(y)) { size_t n= strlen(cmd); - snprintf(&cmd[n], sizeof(cmd)-n, " Y%1.3f", y); + snprintf(&cmd[n], CMDLEN-n, " Y%1.3f", y); } if(!isnan(z)) { size_t n= strlen(cmd); - snprintf(&cmd[n], sizeof(cmd)-n, " Z%1.3f", z); + snprintf(&cmd[n], CMDLEN-n, " Z%1.3f", z); } { size_t n= strlen(cmd); // use specified feedrate (mm/sec) - snprintf(&cmd[n], sizeof(cmd)-n, " F%1.1f", feedrate * 60); // feed rate is converted to mm/min + snprintf(&cmd[n], CMDLEN-n, " F%1.1f", feedrate * 60); // feed rate is converted to mm/min } if(relative) strcat(cmd, " G90"); @@ -469,11 +489,16 @@ void ZProbe::coordinated_move(float x, float y, float z, float feedrate, bool re //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