X-Git-Url: https://git.hcoop.net/clinton/Smoothieware.git/blobdiff_plain/771fb7b281eb2c3acf82725d13e6209931b173db..f95320b7155f6fe614f7579727b840ffb01248e6:/src/modules/tools/zprobe/ZProbe.cpp diff --git a/src/modules/tools/zprobe/ZProbe.cpp b/src/modules/tools/zprobe/ZProbe.cpp index f6803b70..98240b89 100644 --- a/src/modules/tools/zprobe/ZProbe.cpp +++ b/src/modules/tools/zprobe/ZProbe.cpp @@ -15,7 +15,6 @@ #include "StreamOutputPool.h" #include "Gcode.h" #include "Conveyor.h" -#include "Stepper.h" #include "checksumm.h" #include "ConfigValue.h" #include "SlowTicker.h" @@ -26,33 +25,38 @@ #include "PublicData.h" #include "LevelingStrategy.h" #include "StepTicker.h" +#include "utils.h" // strategies we know about #include "DeltaCalibrationStrategy.h" #include "ThreePointStrategy.h" -#include "ZGridStrategy.h" +#include "DeltaGridStrategy.h" +#include "CartGridStrategy.h" #define enable_checksum CHECKSUM("enable") #define probe_pin_checksum CHECKSUM("probe_pin") -#define debounce_count_checksum CHECKSUM("debounce_count") +#define debounce_ms_checksum CHECKSUM("debounce_ms") #define slow_feedrate_checksum CHECKSUM("slow_feedrate") #define fast_feedrate_checksum CHECKSUM("fast_feedrate") +#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") +#define rdelta_homing_checksum CHECKSUM("rdelta_homing") #define X_AXIS 0 #define Y_AXIS 1 #define Z_AXIS 2 -#define STEPPER THEKERNEL->robot->actuators +#define STEPPER THEROBOT->actuators #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 @@ -61,20 +65,21 @@ void ZProbe::on_module_loaded() delete this; return; } - this->running = false; // load settings - this->on_config_reload(this); + this->config_load(); // register event-handlers register_for_event(ON_GCODE_RECEIVED); - THEKERNEL->step_ticker->register_acceleration_tick_handler([this](){acceleration_tick(); }); + // we read the probe in this timer + probing= false; + THEKERNEL->slow_ticker->attach(1000, this, &ZProbe::read_probe); } -void ZProbe::on_config_reload(void *argument) +void ZProbe::config_load() { this->pin.from_string( THEKERNEL->config->value(zprobe_checksum, probe_pin_checksum)->by_default("nc" )->as_string())->as_input(); - this->debounce_count = THEKERNEL->config->value(zprobe_checksum, debounce_count_checksum)->by_default(0 )->as_number(); + this->debounce_ms = THEKERNEL->config->value(zprobe_checksum, debounce_ms_checksum)->by_default(0 )->as_number(); // get strategies to load vector modules; @@ -82,39 +87,47 @@ void ZProbe::on_config_reload(void *argument) 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 ZGrid_leveling_checksum: - this->strategies.push_back(new ZGridStrategy(this)); - found= true; - break; + case delta_grid_leveling_strategy_checksum: + ls= new DeltaGridStrategy(this); + found= true; + break; - // add other strategies here - //case zheight_map_strategy: - // this->strategies.push_back(new ZHeightMapStrategy(this)); - // found= true; - // break; + case cart_grid_leveling_strategy_checksum: + ls= new CartGridStrategy(this); + found= true; + break; + } + if(found) { + if(ls->handleConfig()) { + this->strategies.push_back(ls); + }else{ + delete ls; + } } - if(found) this->strategies.back()->handleConfig(); } } // need to know if we need to use delta kinematics for homing this->is_delta = THEKERNEL->config->value(delta_homing_checksum)->by_default(false)->as_bool(); + this->is_rdelta = THEKERNEL->config->value(rdelta_homing_checksum)->by_default(false)->as_bool(); // default for backwards compatibility add DeltaCalibrationStrategy if a delta - // will be deprecated + // may be deprecated if(this->strategies.empty()) { if(this->is_delta) { this->strategies.push_back(new DeltaCalibrationStrategy(this)); @@ -125,131 +138,116 @@ void ZProbe::on_config_reload(void *argument) this->probe_height = THEKERNEL->config->value(zprobe_checksum, probe_height_checksum)->by_default(5.0F)->as_number(); this->slow_feedrate = THEKERNEL->config->value(zprobe_checksum, slow_feedrate_checksum)->by_default(5)->as_number(); // feedrate in mm/sec this->fast_feedrate = THEKERNEL->config->value(zprobe_checksum, fast_feedrate_checksum)->by_default(100)->as_number(); // feedrate in mm/sec - this->max_z = THEKERNEL->config->value(gamma_max_checksum)->by_default(500)->as_number(); // maximum zprobe distance + 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(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 + } -bool ZProbe::wait_for_probe(int& steps) +uint32_t ZProbe::read_probe(uint32_t dummy) { - unsigned int debounce = 0; - while(true) { - THEKERNEL->call_event(ON_IDLE); - // if no stepper is moving, moves are finished and there was no touch - if( !STEPPER[Z_AXIS]->is_moving() && (!is_delta || (!STEPPER[Y_AXIS]->is_moving() && !STEPPER[Z_AXIS]->is_moving())) ) { - return false; - } + if(!probing || probe_detected) return 0; - // if the touchprobe is active... - if( this->pin.get() ) { - //...increase debounce counter... - if( debounce < debounce_count) { - // ...but only if the counter hasn't reached the max. value + // 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(debounce < debounce_ms) { debounce++; } else { - // ...otherwise stop the steppers, return its remaining steps - if(STEPPER[Z_AXIS]->is_moving()){ - steps= STEPPER[Z_AXIS]->get_stepped(); - STEPPER[Z_AXIS]->move(0, 0); - } - if(is_delta) { - for( int i = X_AXIS; i <= Y_AXIS; i++ ) { - if ( STEPPER[i]->is_moving() ) { - STEPPER[i]->move(0, 0); - } - } - } - return true; + // we signal the motors to stop, which will preempt any moves on that axis + // we do all motors as it may be a delta + for(auto &a : THEROBOT->actuators) a->stop_moving(); + probe_detected= true; + debounce= 0; } + } else { - // The probe was not hit yet, reset debounce counter - debounce = 0; + // The endstop was not hit yet + debounce= 0; } } + + return 0; } -// single probe with custom feedrate +// single probe in Z with custom feedrate // returns boolean value indicating if probe was triggered -bool ZProbe::run_probe_feed(int& steps, float feedrate) +bool ZProbe::run_probe(float& mm, float feedrate, float max_dist, bool reverse) { - // not a block move so disable the last tick setting - for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { - STEPPER[c]->set_moved_last_block(false); + if(dwell_before_probing > .0001F) safe_delay_ms(dwell_before_probing*1000); + + if(this->pin.get()) { + // probe already triggered so abort + return false; } - // Enable the motors - THEKERNEL->stepper->turn_enable_pins_on(); - this->current_feedrate = feedrate * Z_STEPS_PER_MM; // steps/sec - float maxz= this->max_z*2; + float maxz= max_dist < 0 ? this->max_z*2 : max_dist; + + probing= true; + probe_detected= false; + debounce= 0; + + // save current actuator position so we can report how far we moved + float z_start_pos= THEROBOT->actuators[Z_AXIS]->get_current_position(); // move Z down - STEPPER[Z_AXIS]->move(true, maxz * Z_STEPS_PER_MM, 0); // always probes down, no more than 2*maxz - if(this->is_delta) { - // for delta need to move all three actuators - STEPPER[X_AXIS]->move(true, maxz * STEPS_PER_MM(X_AXIS), 0); - STEPPER[Y_AXIS]->move(true, maxz * STEPS_PER_MM(Y_AXIS), 0); - } + bool dir= (!reverse_z != reverse); // xor + float delta[3]= {0,0,0}; + delta[Z_AXIS]= dir ? -maxz : maxz; + THEROBOT->delta_move(delta, feedrate, 3); - // start acceleration processing - this->running = true; + // wait until finished + THECONVEYOR->wait_for_idle(); - bool r = wait_for_probe(steps); - this->running = false; - return r; -} + // 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= z_start_pos - THEROBOT->actuators[2]->get_current_position(); -// single probe with either fast or slow feedrate -// returns boolean value indicating if probe was triggered -bool ZProbe::run_probe(int& steps, bool fast) -{ - float feedrate = (fast ? this->fast_feedrate : this->slow_feedrate); - return run_probe_feed(steps, feedrate); + // set the last probe position to the actuator units moved during this home + THEROBOT->set_last_probe_position(std::make_tuple(0, 0, mm, probe_detected?1:0)); + + probing= false; + if(probe_detected) { + // if the probe stopped the move we need to correct the last_milestone as it did not reach where it thought + THEROBOT->reset_position_from_current_actuator_position(); + } + + return probe_detected; } -bool ZProbe::return_probe(int steps) +// do probe then return to start position +bool ZProbe::run_probe_return(float& mm, float feedrate, float max_dist, bool reverse) { - // move probe back to where it was - float fr= this->slow_feedrate*2; // nominally twice slow feedrate - if(fr > this->fast_feedrate) fr= this->fast_feedrate; // unless that is greater than fast feedrate - this->current_feedrate = fr * Z_STEPS_PER_MM; // feedrate in steps/sec - bool dir= steps < 0; - steps= abs(steps); - - STEPPER[Z_AXIS]->move(dir, steps, 0); - if(this->is_delta) { - STEPPER[X_AXIS]->move(dir, steps, 0); - STEPPER[Y_AXIS]->move(dir, steps, 0); - } + float save_z_pos= THEROBOT->get_axis_position(Z_AXIS); - this->running = true; - while(STEPPER[Z_AXIS]->is_moving() || (is_delta && (STEPPER[X_AXIS]->is_moving() || STEPPER[Y_AXIS]->is_moving())) ) { - // wait for it to complete - THEKERNEL->call_event(ON_IDLE); + bool ok= run_probe(mm, feedrate, max_dist, reverse); + + // move probe back to where it was + float fr; + if(this->return_feedrate != 0) { // use return_feedrate if set + fr = this->return_feedrate; + } else { + fr = this->slow_feedrate*2; // nominally twice slow feedrate + if(fr > this->fast_feedrate) fr = this->fast_feedrate; // unless that is greater than fast feedrate } - this->running = false; + // absolute move back to saved starting position + coordinated_move(NAN, NAN, save_z_pos, fr, false); - return true; + return ok; } -bool ZProbe::doProbeAt(int &steps, float x, float y) +bool ZProbe::doProbeAt(float &mm, float x, float y) { - int s; // move to xy coordinated_move(x, y, NAN, getFastFeedrate()); - if(!run_probe(s)) return false; - - // return to original Z - return_probe(s); - steps = s; - - return true; -} - -float ZProbe::probeDistance(float x, float y) -{ - int s; - if(!doProbeAt(s, x, y)) return NAN; - return zsteps_to_mm(s); + return run_probe_return(mm, slow_feedrate); } void ZProbe::on_gcode_received(void *argument) @@ -257,149 +255,261 @@ void ZProbe::on_gcode_received(void *argument) Gcode *gcode = static_cast(argument); if( gcode->has_g && gcode->g >= 29 && gcode->g <= 32) { + // make sure the probe is defined and not already triggered before moving motors - if(!this->pin.connected()) { - gcode->stream->printf("ZProbe not connected.\n"); + if(!this->pin.connected()) { + gcode->stream->printf("ZProbe pin not configured.\n"); return; } + if(this->pin.get()) { gcode->stream->printf("ZProbe triggered before move, aborting command.\n"); return; } if( gcode->g == 30 ) { // simple Z probe - // first wait for an empty queue i.e. no moves left - THEKERNEL->conveyor->wait_for_empty_queue(); + // first wait for all moves to finish + THEKERNEL->conveyor->wait_for_idle(); - int steps; + bool set_z= (gcode->has_letter('Z') && !is_rdelta); bool probe_result; - if(gcode->has_letter('F')) { - probe_result = run_probe_feed(steps, gcode->get_value('F') / 60); - } else { - probe_result = run_probe(steps); - } + bool reverse= (gcode->has_letter('R') && gcode->get_value('R') != 0); // specify to probe in reverse direction + float rate= gcode->has_letter('F') ? gcode->get_value('F') / 60 : this->slow_feedrate; + float mm; + + // if not setting Z then return probe to where it started, otherwise leave it where it is + probe_result = (set_z ? run_probe(mm, rate, -1, reverse) : run_probe_return(mm, rate, -1, reverse)); if(probe_result) { - gcode->stream->printf("Z:%1.4f C:%d\n", steps / Z_STEPS_PER_MM, steps); - // move back to where it started, unless a Z is specified - if(gcode->has_letter('Z')) { - // set Z to the specified value, and leave probe where it is - THEKERNEL->robot->reset_axis_position(gcode->get_value('Z'), Z_AXIS); - } else { - return_probe(steps); + // the result is in actuator coordinates moved + 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 + 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 { gcode->stream->printf("ZProbe not triggered\n"); } } else { - // find a strategy to handle the gcode - for(auto s : strategies){ - if(s->handleGcode(gcode)) { + if(!gcode->has_letter('P')) { + // find the first strategy to handle the gcode + for(auto s : strategies){ + if(s->handleGcode(gcode)) { + return; + } + } + gcode->stream->printf("No strategy found to handle G%d\n", gcode->g); + + }else{ + // P paramater selects which strategy to send the code to + // they are loaded in the order they are defined in config, 0 being the first, 1 being the second and so on. + uint16_t i= gcode->get_value('P'); + if(i < strategies.size()) { + if(!strategies[i]->handleGcode(gcode)){ + gcode->stream->printf("strategy #%d did not handle G%d\n", i, gcode->g); + } return; + + }else{ + gcode->stream->printf("strategy #%d is not loaded\n", i); } } - gcode->stream->printf("No strategy found to handle G%d\n", gcode->g); } + } 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 > 5) { + gcode->stream->printf("error:Only G38.2, G38.3, G38.4, and G38.5 are supported\n"); + return; + } + + // make sure the probe is defined and not already triggered before moving motors + if(!this->pin.connected()) { + gcode->stream->printf("error:ZProbe not connected.\n"); + return; + } + + if(gcode->subcode == 4 || gcode->subcode == 5) { + if(!this->pin.get()) { + gcode->stream->printf("error:ZProbe triggered before move, aborting command.\n"); + return; + } + } else { + if(this->pin.get()) { + gcode->stream->printf("error:ZProbe triggered before move, aborting command.\n"); + return; + } + } + + // first wait for all moves to finish + THEKERNEL->conveyor->wait_for_idle(); + + float x= NAN, y=NAN, z=NAN; + if(gcode->has_letter('X')) { + x= gcode->get_value('X'); + } + + if(gcode->has_letter('Y')) { + y= gcode->get_value('Y'); + } + + if(gcode->has_letter('Z')) { + z= gcode->get_value('Z'); + } + + 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) { + pin.set_inverting(pin.is_inverting() != 1); + } + + probe_XYZ(gcode, x, y, z); + + if(gcode->subcode == 4 || gcode->subcode == 5) { + pin.set_inverting(pin.is_inverting() != 1); + } + + return; + } else if(gcode->has_m) { // M code processing here - if(gcode->m == 119) { - int c = this->pin.get(); - gcode->stream->printf(" Probe: %d", c); - gcode->add_nl = true; - - }else { - for(auto s : strategies){ - if(s->handleGcode(gcode)) { - return; + int c; + switch (gcode->m) { + case 119: + c = this->pin.get(); + gcode->stream->printf(" Probe: %d", c); + gcode->add_nl = true; + break; + + case 670: + if (gcode->has_letter('S')) this->slow_feedrate = gcode->get_value('S'); + if (gcode->has_letter('K')) this->fast_feedrate = gcode->get_value('K'); + if (gcode->has_letter('R')) this->return_feedrate = gcode->get_value('R'); + if (gcode->has_letter('Z')) this->max_z = gcode->get_value('Z'); + if (gcode->has_letter('H')) this->probe_height = gcode->get_value('H'); + if (gcode->has_letter('I')) { // NOTE this is temporary and toggles the invertion status of the pin + 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) 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 + + default: + for(auto s : strategies){ + if(s->handleGcode(gcode)) { + return; + } } - } } } } -// Called periodically to change the speed to match acceleration -void ZProbe::acceleration_tick(void) +// 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, float x, float y, float z) { - if(!this->running) return; // nothing to do - if(STEPPER[Z_AXIS]->is_moving()) accelerate(Z_AXIS); - - if(is_delta) { - // deltas needs to move all actuators - for ( int c = X_AXIS; c <= Y_AXIS; c++ ) { - if( !STEPPER[c]->is_moving() ) continue; - accelerate(c); - } - } + // 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) - return; -} + // 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; -void ZProbe::accelerate(int c) -{ uint32_t current_rate = STEPPER[c]->get_steps_per_second(); - uint32_t target_rate = floorf(this->current_feedrate); + // do a regular move which will stop as soon as the probe is triggered, or the distance is reached + coordinated_move(x, y, z, rate, true); - // Z may have a different acceleration to X and Y - float acc= (c==Z_AXIS) ? THEKERNEL->planner->get_z_acceleration() : THEKERNEL->planner->get_acceleration(); - if( current_rate < target_rate ) { - uint32_t rate_increase = floorf((acc / THEKERNEL->acceleration_ticks_per_second) * STEPS_PER_MM(c)); - current_rate = min( target_rate, current_rate + rate_increase ); - } - if( current_rate > target_rate ) { - current_rate = target_rate; - } + // coordinated_move returns when the move is finished - // steps per second - STEPPER[c]->set_speed(current_rate); + // disable probe checking + probing= false; + THEROBOT->disable_segmentation= false; + + // if the probe stopped the move we need to correct the last_milestone as it did not reach where it thought + // this also sets last_milestone to the machine coordinates it stopped at + THEROBOT->reset_position_from_current_actuator_position(); + float pos[3]; + THEROBOT->get_axis_position(pos, 3); + + 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", 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 || 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); + } } // issue a coordinated move directly to robot, and return when done // Only move the coordinates that are passed in as not nan +// 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, "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_empty_queue(); + THEKERNEL->conveyor->wait_for_idle(); + THEROBOT->pop_state(); + } // issue home command void ZProbe::home() { - Gcode gc("G28", &(StreamOutput::NullStream)); + Gcode gc(THEKERNEL->is_grbl_mode() ? "G28.2" : "G28", &(StreamOutput::NullStream)); THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc); } - -float ZProbe::zsteps_to_mm(float steps) -{ - return steps / Z_STEPS_PER_MM; -}