From 121094a5ec6653dc82c554c4f3a6396db9679c0c Mon Sep 17 00:00:00 2001 From: Jim Morris Date: Thu, 23 Jun 2016 15:39:20 -0700 Subject: [PATCH] fix extruder E values when in volumetric mode. this is a breaking change as it now always expects the E parameter to be in mm^3 - the down side here is that when you extrude 10mm form pronterface it will be interpreted as 10mm^3 instead G10/G11 should now work, M207 still specifes retract length in mm, but is converted to mm^3 when G10 is executed - we now have a function pointer set by the active extruder to get the E scaling from robot --- src/modules/robot/Robot.cpp | 148 ++++++++---------------- src/modules/robot/Robot.h | 7 +- src/modules/tools/endstops/Endstops.cpp | 12 +- src/modules/tools/extruder/Extruder.cpp | 20 ++-- src/modules/tools/zprobe/ZProbe.cpp | 4 +- 5 files changed, 67 insertions(+), 124 deletions(-) diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp index e5177b1b..d55245ef 100644 --- a/src/modules/robot/Robot.cpp +++ b/src/modules/robot/Robot.cpp @@ -103,6 +103,7 @@ Robot::Robot() seconds_per_minute = 60.0F; this->clearToolOffset(); this->compensationTransform = nullptr; + this->get_e_scale_fnc= nullptr; this->wcs_offsets.fill(wcs_t(0.0F, 0.0F, 0.0F)); this->g92_offset = wcs_t(0.0F, 0.0F, 0.0F); this->next_command_is_MCS = false; @@ -892,31 +893,24 @@ void Robot::reset_position_from_current_actuator_position() // Convert target (in machine coordinates) to machine_position, then convert to actuator position and append this to the planner // target is in machine coordinates without the compensation transform, however we save a last_machine_position that includes // all transforms and is what we actually convert to actuator positions -bool Robot::append_milestone(Gcode *gcode, const float target[], float rate_mm_s) +bool Robot::append_milestone(const float target[], float rate_mm_s, bool disable_compensation) { float deltas[n_motors]; float transformed_target[n_motors]; // adjust target for bed compensation float unit_vec[N_PRIMARY_AXIS]; float millimeters_of_travel= 0; - // catch negative or zero feed rates and return the same error as GRBL does - if(rate_mm_s <= 0.0F) { - gcode->is_error= true; - gcode->txt_after_ok= (rate_mm_s == 0 ? "Undefined feed rate" : "feed rate < 0"); - return false; - } - // unity transform by default memcpy(transformed_target, target, n_motors*sizeof(float)); // check function pointer and call if set to transform the target to compensate for bed - if(compensationTransform) { + if(!disable_compensation && compensationTransform) { // some compensation strategies can transform XYZ, some just change Z compensationTransform(transformed_target); } bool move= false; - float sos= 0; + float sos= 0, sosxyz= 0; // sun of squares for all axis and just XYZ // find distance moved by each axis, use transformed target from the current machine position for (size_t i = 0; i < n_motors; i++) { @@ -924,9 +918,12 @@ bool Robot::append_milestone(Gcode *gcode, const float target[], float rate_mm_s if(deltas[i] == 0) continue; // at least one non zero delta move = true; -// if(i <= Z_AXIS) { + if(i <= Z_AXIS) { + sosxyz += powf(deltas[i], 2); + sos= sosxyz; + }else{ sos += powf(deltas[i], 2); -// } + } } // nothing moved @@ -964,7 +961,7 @@ bool Robot::append_milestone(Gcode *gcode, const float target[], float rate_mm_s memcpy(this->last_machine_position, transformed_target, n_motors*sizeof(float)); if(!auxilliary_move) { - float d= sqrtf(powf(deltas[X_AXIS], 2) + powf(deltas[Y_AXIS], 2) + powf(deltas[Z_AXIS], 2)); // we need the actual distance over the bed + float d= sqrtf(sosxyz); // we need the actual distance over the bed for (size_t i = X_AXIS; i <= Z_AXIS; i++) { // find distance unit vector for primary axis only @@ -988,12 +985,12 @@ bool Robot::append_milestone(Gcode *gcode, const float target[], float rate_mm_s // for the extruders just copy the position, and possibly scale it from mm³ to mm for (size_t i = E_AXIS; i < n_motors; i++) { actuator_pos[i]= last_machine_position[i]; - if(!isnan(this->e_scale)) { + if(get_e_scale_fnc) { // NOTE this relies on the fact only one extruder is active at a time // scale for volumetric or flow rate // TODO is this correct? scaling the absolute target? what if the scale changes? // for volumetric it basically converts mm³ to mm, but what about flow rate? - actuator_pos[i] *= this->e_scale; + actuator_pos[i] *= get_e_scale_fnc(); } } #endif @@ -1033,109 +1030,51 @@ bool Robot::append_milestone(Gcode *gcode, const float target[], float rate_mm_s return true; } -// Used to plan a single move used by things like endstops when homing, zprobe, extruder retracts etc. -// TODO this pretty much duplicates append_milestone, so try to refactor it away. -bool Robot::solo_move(const float *delta, float rate_mm_s, uint8_t naxis) +// Used to plan a single move used by things like endstops when homing, zprobe, extruder firmware retracts etc. +bool Robot::delta_move(const float *delta, float rate_mm_s, uint8_t naxis) { if(THEKERNEL->is_halted()) return false; - // catch negative or zero feed rates and return the same error as GRBL does + // catch negative or zero feed rates if(rate_mm_s <= 0.0F) { return false; } - bool move= false; - float sos= 0; - - // find distance moved by each axis - for (size_t i = 0; i < naxis; i++) { - if(delta[i] == 0) continue; - // at least one non zero delta - move = true; - sos += powf(delta[i], 2); - } - - // nothing moved - if(!move) return false; - - // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here - // as the last milestone won't be updated we do not actually lose any moves as they will be accounted for in the next move - if(sos < 0.00001F) return false; - - float millimeters_of_travel= sqrtf(sos); - - // this is the new machine position - for (int axis = 0; axis < naxis; axis++) { - this->last_machine_position[axis] += delta[axis]; - } - // we also need to update last_milestone here which is the same as last_machine_position as there was no compensation - memcpy(this->last_milestone, this->last_machine_position, naxis*sizeof(float)); - - - // Do not move faster than the configured cartesian limits for XYZ - for (int axis = X_AXIS; axis <= Z_AXIS; axis++) { - if(delta[axis] == 0) continue; - if ( max_speeds[axis] > 0 ) { - float axis_speed = fabsf(delta[axis] / millimeters_of_travel * rate_mm_s); - - if (axis_speed > max_speeds[axis]) - rate_mm_s *= ( max_speeds[axis] / axis_speed ); - } - } - - // find actuator position given the machine position - ActuatorCoordinates actuator_pos; - arm_solution->cartesian_to_actuator( this->last_machine_position, actuator_pos ); + // get the absolute target position, default is current last_milestone + float target[n_motors]; + memcpy(target, last_milestone, n_motors*sizeof(float)); - #if MAX_ROBOT_ACTUATORS > 3 - // for the extruders just copy the position, need to copy all actuators - for (size_t i = E_AXIS; i < n_motors; i++) { - actuator_pos[i]= last_machine_position[i]; + // add in the deltas to get new target + for (int i= 0; i < naxis; i++) { + target[i] += delta[i]; } - #endif - - // use default acceleration to start with - float acceleration = default_acceleration; - float isecs = rate_mm_s / millimeters_of_travel; - - // check per-actuator speed limits - for (size_t actuator = 0; actuator < naxis; actuator++) { - float d = fabsf(actuator_pos[actuator] - actuators[actuator]->get_last_milestone()); - if(d == 0) continue; // no movement for this actuator - - float actuator_rate= d * isecs; - if (actuator_rate > actuators[actuator]->get_max_rate()) { - rate_mm_s *= (actuators[actuator]->get_max_rate() / actuator_rate); - isecs = rate_mm_s / millimeters_of_travel; - } - // adjust acceleration to lowest found in an active axis - float ma = actuators[actuator]->get_acceleration(); // in mm/sec² - if(!isnan(ma)) { // if axis does not have acceleration set then it uses the default_acceleration - float ca = fabsf((d/millimeters_of_travel) * acceleration); - if (ca > ma) { - acceleration *= ( ma / ca ); - } - } + // submit for planning and if moved update last_milestone + // NOTE this disabled compensation transforms as homing and zprobe must not use them + if(append_milestone(target, rate_mm_s, true)) { + memcpy(last_milestone, target, n_motors*sizeof(float)); + return true; } - // Append the block to the planner - THEKERNEL->planner->append_block(actuator_pos, n_motors, rate_mm_s, millimeters_of_travel, nullptr, acceleration); - return true; + return false; } // Append a move to the queue ( cutting it into segments if needed ) bool Robot::append_line(Gcode *gcode, const float target[], float rate_mm_s, float delta_e) { - // by default there is no e scaling required, but if volumetric extrusion is enabled this will be set to scale the parameter - this->e_scale= NAN; + // catch negative or zero feed rates and return the same error as GRBL does + if(rate_mm_s <= 0.0F) { + gcode->is_error= true; + gcode->txt_after_ok= (rate_mm_s == 0 ? "Undefined feed rate" : "feed rate < 0"); + return false; + } // Find out the distance for this move in XYZ in MCS float millimeters_of_travel = sqrtf(powf( target[X_AXIS] - last_milestone[X_AXIS], 2 ) + powf( target[Y_AXIS] - last_milestone[Y_AXIS], 2 ) + powf( target[Z_AXIS] - last_milestone[Z_AXIS], 2 )); if(millimeters_of_travel < 0.00001F) { - // we have no movement in XYZ, probably E only extrude or retract which is always in mm, so no E scaling required - return this->append_milestone(gcode, target, rate_mm_s); + // we have no movement in XYZ, probably E only extrude or retract + return this->append_milestone(target, rate_mm_s); } /* @@ -1150,8 +1089,6 @@ bool Robot::append_line(Gcode *gcode, const float target[], float rate_mm_s, flo float data[2]= {delta_e, rate_mm_s / millimeters_of_travel}; if(PublicData::set_value(extruder_checksum, target_checksum, data)) { rate_mm_s *= data[1]; // adjust the feedrate - // we may need to scale the amount moved too - this->e_scale= data[0]; } } @@ -1199,13 +1136,13 @@ bool Robot::append_line(Gcode *gcode, const float target[], float rate_mm_s, flo segment_end[i] += segment_delta[i]; // Append the end of this segment to the queue - bool b= this->append_milestone(gcode, segment_end, rate_mm_s); + bool b= this->append_milestone(segment_end, rate_mm_s); moved= moved || b; } } // Append the end of this full move to the queue - if(this->append_milestone(gcode, target, rate_mm_s)) moved= true; + if(this->append_milestone(target, rate_mm_s)) moved= true; this->next_command_is_MCS = false; // always reset this @@ -1216,6 +1153,13 @@ bool Robot::append_line(Gcode *gcode, const float target[], float rate_mm_s, flo // Append an arc to the queue ( cutting it into segments as needed ) bool Robot::append_arc(Gcode * gcode, const float target[], const float offset[], float radius, bool is_clockwise ) { + float rate_mm_s= this->feed_rate / seconds_per_minute; + // catch negative or zero feed rates and return the same error as GRBL does + if(rate_mm_s <= 0.0F) { + gcode->is_error= true; + gcode->txt_after_ok= (rate_mm_s == 0 ? "Undefined feed rate" : "feed rate < 0"); + return false; + } // Scary math float center_axis0 = this->last_milestone[this->plane_axis_0] + offset[this->plane_axis_0]; @@ -1321,12 +1265,12 @@ bool Robot::append_arc(Gcode * gcode, const float target[], const float offset[] arc_target[this->plane_axis_2] += linear_per_segment; // Append this segment to the queue - bool b= this->append_milestone(gcode, arc_target, this->feed_rate / seconds_per_minute); + bool b= this->append_milestone(arc_target, rate_mm_s); moved= moved || b; } // Ensure last segment arrives at target location. - if(this->append_milestone(gcode, target, this->feed_rate / seconds_per_minute)) moved= true; + if(this->append_milestone(target, rate_mm_s)) moved= true; return moved; } diff --git a/src/modules/robot/Robot.h b/src/modules/robot/Robot.h index e53fe9e7..d539b6af 100644 --- a/src/modules/robot/Robot.h +++ b/src/modules/robot/Robot.h @@ -56,7 +56,7 @@ class Robot : public Module { std::vector get_wcs_state() const; std::tuple get_last_probe_position() const { return last_probe_position; } void set_last_probe_position(std::tuple p) { last_probe_position = p; } - bool solo_move(const float target[], float rate_mm_s, uint8_t naxis); + bool delta_move(const float delta[], float rate_mm_s, uint8_t naxis); uint8_t register_motor(StepperMotor*); uint8_t get_number_registered_motors() const {return n_motors; } @@ -67,6 +67,8 @@ class Robot : public Module { // set by a leveling strategy to transform the target of a move according to the current plan std::function compensationTransform; + // set by an active extruder, returns the amount tio scale the E parameter by (to convert mm³ to mm) + std::function get_e_scale_fnc; // Workspace coordinate systems wcs_t mcs2wcs(const wcs_t &pos) const; @@ -95,7 +97,7 @@ class Robot : public Module { }; void load_config(); - bool append_milestone( Gcode *gcode, const float target[], float rate_mm_s); + bool append_milestone(const float target[], float rate_mm_s, bool disable_compensation= false); bool append_line( Gcode* gcode, const float target[], float rate_mm_s, float delta_e); bool append_arc( Gcode* gcode, const float target[], const float offset[], float radius, bool is_clockwise ); bool compute_arc(Gcode* gcode, const float offset[], const float target[], enum MOTION_MODE_T motion_mode); @@ -134,7 +136,6 @@ class Robot : public Module { // computational efficiency of generating arcs. int arc_correction; // Setting : how often to rectify arc computation float max_speeds[3]; // Setting : max allowable speed in mm/s for each axis - float e_scale; // how much to scale any e motion by uint8_t selected_extruder; uint8_t n_motors; //count of the motors/axis registered diff --git a/src/modules/tools/endstops/Endstops.cpp b/src/modules/tools/endstops/Endstops.cpp index dad5138b..bce3c74b 100644 --- a/src/modules/tools/endstops/Endstops.cpp +++ b/src/modules/tools/endstops/Endstops.cpp @@ -449,7 +449,7 @@ void Endstops::home_xy() if(this->home_direction[X_AXIS]) delta[X_AXIS]= -delta[X_AXIS]; if(this->home_direction[Y_AXIS]) delta[Y_AXIS]= -delta[Y_AXIS]; float feed_rate = std::min(fast_rates[X_AXIS], fast_rates[Y_AXIS]); - THEROBOT->solo_move(delta, feed_rate, 3); + THEROBOT->delta_move(delta, feed_rate, 3); // Wait for XY to have homed THECONVEYOR->wait_for_empty_queue(); @@ -458,7 +458,7 @@ void Endstops::home_xy() // now home X only float delta[3] {alpha_max*2, 0, 0}; if(this->home_direction[X_AXIS]) delta[X_AXIS]= -delta[X_AXIS]; - THEROBOT->solo_move(delta, fast_rates[X_AXIS], 3); + THEROBOT->delta_move(delta, fast_rates[X_AXIS], 3); // wait for X THECONVEYOR->wait_for_empty_queue(); @@ -466,7 +466,7 @@ void Endstops::home_xy() // now home Y only float delta[3] {0, beta_max*2, 0}; if(this->home_direction[Y_AXIS]) delta[Y_AXIS]= -delta[Y_AXIS]; - THEROBOT->solo_move(delta, fast_rates[Y_AXIS], 3); + THEROBOT->delta_move(delta, fast_rates[Y_AXIS], 3); // wait for Y THECONVEYOR->wait_for_empty_queue(); } @@ -490,7 +490,7 @@ void Endstops::home(std::bitset<3> a) // now home z float delta[3] {0, 0, gamma_max*2}; // we go twice the maxz just in case it was set incorrectly if(this->home_direction[Z_AXIS]) delta[Z_AXIS]= -delta[Z_AXIS]; - THEROBOT->solo_move(delta, fast_rates[Z_AXIS], 3); + THEROBOT->delta_move(delta, fast_rates[Z_AXIS], 3); // wait for Z THECONVEYOR->wait_for_empty_queue(); } @@ -512,7 +512,7 @@ void Endstops::home(std::bitset<3> a) } } - THEROBOT->solo_move(delta, feed_rate, 3); + THEROBOT->delta_move(delta, feed_rate, 3); // wait until finished THECONVEYOR->wait_for_empty_queue(); @@ -526,7 +526,7 @@ void Endstops::home(std::bitset<3> a) delta[c]= 0; } } - THEROBOT->solo_move(delta, feed_rate, 3); + THEROBOT->delta_move(delta, feed_rate, 3); // wait until finished THECONVEYOR->wait_for_empty_queue(); diff --git a/src/modules/tools/extruder/Extruder.cpp b/src/modules/tools/extruder/Extruder.cpp index d2a091be..6713993f 100644 --- a/src/modules/tools/extruder/Extruder.cpp +++ b/src/modules/tools/extruder/Extruder.cpp @@ -139,12 +139,15 @@ void Extruder::select() { selected= true; stepper_motor->set_selected(true); + // set the function pointer to return the current scaling + THEROBOT->get_e_scale_fnc= [this](void) { return volumetric_multiplier*extruder_multiplier; }; } void Extruder::deselect() { selected= false; stepper_motor->set_selected(false); + THEROBOT->get_e_scale_fnc= nullptr; } void Extruder::on_get_public_data(void *argument) @@ -183,11 +186,6 @@ void Extruder::on_set_public_data(void *argument) // check against maximum speeds and return rate modifier d[1]= check_max_speeds(delta, isecs); - - // also return the scale factor - float s= volumetric_multiplier*extruder_multiplier; - d[0]= (s == 1.0F ? NAN : s); - pdr->set_taken(); return; } @@ -354,14 +352,14 @@ void Extruder::on_gcode_received(void *argument) } // HACK ALERT due to certain slicers reseting E with G92 E0 between the G10 and G11 we need to save and restore position save_position(); - delta[motor_id]= -retract_length; - THEROBOT->solo_move(delta, retract_feedrate, motor_id+1); + delta[motor_id]= -retract_length/volumetric_multiplier; // convert from mm to mm³ + THEROBOT->delta_move(delta, retract_feedrate, motor_id+1); restore_position(); // zlift if(retract_zlift_length > 0) { float delta[3]{0,0,retract_zlift_length}; - THEROBOT->solo_move(delta, retract_zlift_feedrate, 3); + THEROBOT->delta_move(delta, retract_zlift_feedrate, 3); } }else if(gcode->g == 11) { @@ -370,7 +368,7 @@ void Extruder::on_gcode_received(void *argument) // reverse zlift happens before unretract // NOTE we do not do this if cancel_zlift_restore is set to true, which happens if there is an absolute Z move inbetween G10 and G11 float delta[3]{0,0,-retract_zlift_length}; - THEROBOT->solo_move(delta, retract_zlift_feedrate, 3); + THEROBOT->delta_move(delta, retract_zlift_feedrate, 3); } float delta[motor_id+1]; @@ -382,8 +380,8 @@ void Extruder::on_gcode_received(void *argument) // also as the move has not completed yet, when we restore the current position will be incorrect once the move finishes, // however this is not fatal for an extruder save_position(); - delta[motor_id]= retract_length + retract_recover_length; - THEROBOT->solo_move(delta, retract_recover_feedrate, motor_id+1); + delta[motor_id]= (retract_length + retract_recover_length)/volumetric_multiplier; // convert from mm to mm³ + THEROBOT->delta_move(delta, retract_recover_feedrate, motor_id+1); restore_position(); } diff --git a/src/modules/tools/zprobe/ZProbe.cpp b/src/modules/tools/zprobe/ZProbe.cpp index a2276a25..92b63ccc 100644 --- a/src/modules/tools/zprobe/ZProbe.cpp +++ b/src/modules/tools/zprobe/ZProbe.cpp @@ -183,7 +183,7 @@ bool ZProbe::run_probe(float& mm, float feedrate, float max_dist, bool reverse) bool dir= (!reverse_z != reverse); // xor float delta[3]= {0,0,0}; delta[Z_AXIS]= dir ? -maxz : maxz; - THEROBOT->solo_move(delta, feedrate, 3); + THEROBOT->delta_move(delta, feedrate, 3); // wait until finished THECONVEYOR->wait_for_empty_queue(); @@ -227,7 +227,7 @@ bool ZProbe::return_probe(float mm, bool reverse) float delta[3]= {0,0,0}; delta[Z_AXIS]= dir ? -mm : mm; - THEROBOT->solo_move(delta, fr, 3); + THEROBOT->delta_move(delta, fr, 3); // wait until finished THECONVEYOR->wait_for_empty_queue(); -- 2.20.1