From: Joseph Coffland Date: Sat, 10 Sep 2016 07:38:33 +0000 (-0700) Subject: Fix feed rate mode handling X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=db2d1160c3f95adc174674d548f4350aa3949166;p=bbctrl-firmware Fix feed rate mode handling --- diff --git a/src/command.c b/src/command.c index 4a43ec3..16397d2 100644 --- a/src/command.c +++ b/src/command.c @@ -227,7 +227,7 @@ void command_callback() { default: if (estop_triggered()) {status = STAT_MACHINE_ALARMED; break;} else if (mp_is_flushing()) break; // Flush GCode command - else if (mp_get_planner_buffer_room() < 2 || + else if (!mp_get_planner_buffer_room() || mp_is_resuming() || mach_arc_active() || mach_is_homing() || diff --git a/src/machine.c b/src/machine.c index aa6e59a..98b3fef 100644 --- a/src/machine.c +++ b/src/machine.c @@ -456,28 +456,6 @@ float mach_get_work_position(uint8_t axis) { * These functions are not part of the NIST defined functions */ -/* Perform final operations for a rapid or feed - * - * These routines set the point position in the gcode model. - * - * Note: As far as the machine is concerned the final - * position of a Gcode block (move) is achieved as soon as the move is - * planned and the move target becomes the new model position. In - * reality the planner will (in all likelihood) have only just queued - * the move for later execution, and the real tool position is still - * close to the starting point. - */ -void mach_finalize_move() { - copy_vector(mach.position, mach.gm.target); // update model position - - // if in inverse time mode reset feed rate so next block requires an - // explicit feed rate setting - if (mach.gm.feed_rate_mode == INVERSE_TIME_MODE && - mach.gm.motion_mode == MOTION_MODE_FEED) - mach.gm.feed_rate = 0; -} - - /* Compute optimal and minimum move times into the gcode_state * * "Minimum time" is the fastest the move can be performed given the velocity @@ -543,12 +521,11 @@ float mach_calc_move_time(const float axis_length[], // Compute times for feed motion if (mach.gm.motion_mode != MOTION_MODE_RAPID) { - if (mach.gm.feed_rate_mode == INVERSE_TIME_MODE) { + if (mach.gm.feed_rate_mode == INVERSE_TIME_MODE) // Feed rate was un-inverted to minutes by mach_set_feed_rate() max_time = mach.gm.feed_rate; - mach.gm.feed_rate_mode = UNITS_PER_MINUTE_MODE; - } else { + else { // Compute length of linear move in millimeters. Feed rate in mm/min. max_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] + axis_square[AXIS_Z]) / mach.gm.feed_rate; @@ -881,7 +858,7 @@ stat_t mach_rapid(float target[], float flags[]) { // prep and plan the move mach_update_work_offsets(); // update fully resolved offsets to state status = mp_aline(mach.gm.target, mach.gm.line); // send the move to planner - mach_finalize_move(); + copy_vector(mach.position, mach.gm.target); // update model position return status; } @@ -934,7 +911,7 @@ stat_t mach_goto_g30_position(float target[], float flags[]) { void mach_set_feed_rate(float feed_rate) { if (mach.gm.feed_rate_mode == INVERSE_TIME_MODE) // normalize to minutes (active for this gcode block only) - mach.gm.feed_rate = 1 / feed_rate; + mach.gm.feed_rate = feed_rate ? 1 / feed_rate : 0; // Avoid div by zero else mach.gm.feed_rate = TO_MILLIMETERS(feed_rate); } @@ -942,6 +919,8 @@ void mach_set_feed_rate(float feed_rate) { /// G93, G94 void mach_set_feed_rate_mode(feed_rate_mode_t mode) { + if (mach.gm.feed_rate_mode == mode) return; + mach.gm.feed_rate = 0; // Force setting feed rate after changing modes mach.gm.feed_rate_mode = mode; } @@ -963,7 +942,8 @@ stat_t mach_dwell(float seconds) { /// G1 stat_t mach_feed(float target[], float flags[]) { // trap zero feed rate condition - if (mach.gm.feed_rate_mode != INVERSE_TIME_MODE && fp_ZERO(mach.gm.feed_rate)) + if (fp_ZERO(mach.gm.feed_rate) || + (mach.gm.feed_rate_mode == INVERSE_TIME_MODE && !mach.gf.feed_rate)) return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; mach.gm.motion_mode = MOTION_MODE_FEED; @@ -976,7 +956,7 @@ stat_t mach_feed(float target[], float flags[]) { // prep and plan the move mach_update_work_offsets(); // update fully resolved offsets to state status = mp_aline(mach.gm.target, mach.gm.line); // send the move to planner - mach_finalize_move(); + copy_vector(mach.position, mach.gm.target); // update model position return status; } diff --git a/src/machine.h b/src/machine.h index 6d2ba47..342d7bf 100644 --- a/src/machine.h +++ b/src/machine.h @@ -346,7 +346,6 @@ float mach_get_work_position(uint8_t axis); // Critical helpers float mach_calc_move_time(const float axis_length[], const float axis_square[]); -void mach_finalize_move(); stat_t mach_deferred_write_callback(); void mach_set_model_target(float target[], float flag[]); stat_t mach_test_soft_limits(float target[]); diff --git a/src/plan/arc.c b/src/plan/arc.c index 105788b..b8a346d 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -77,27 +77,15 @@ arc_t arc = {0}; */ static float _estimate_arc_time(float length, float linear_travel, float planar_travel) { - float t; - // Determine move time at requested feed rate - if (mach.gm.feed_rate_mode == INVERSE_TIME_MODE) { - // Inverse feed rate has been normalized to minutes - t = mach.gm.feed_rate; - - // Reset feed rate so next block requires an explicit feed rate setting - mach.gm.feed_rate = 0; - mach.gm.feed_rate_mode = UNITS_PER_MINUTE_MODE; - - } else t = length / mach.gm.feed_rate; + // Inverse feed rate is normalized to minutes + float time = mach.gm.feed_rate_mode == INVERSE_TIME_MODE ? + mach.gm.feed_rate : length / mach.gm.feed_rate; // Downgrade the time if there is a rate-limiting axis - t = max3(t, planar_travel / mach.a[arc.plane_axis_0].feedrate_max, - planar_travel / mach.a[arc.plane_axis_1].feedrate_max); - - if (0 < fabs(linear_travel)) - t = max(t, fabs(linear_travel / mach.a[arc.linear_axis].feedrate_max)); - - return t; + return max4(time, planar_travel / mach.a[arc.plane_axis_0].feedrate_max, + planar_travel / mach.a[arc.plane_axis_1].feedrate_max, + fabs(linear_travel) / mach.a[arc.linear_axis].feedrate_max); } @@ -344,7 +332,7 @@ static stat_t _compute_arc(const float position[], float offset[], } -/*** machine entry point for arc +/*** Machine entry point for arc * * Generates an arc by queuing line segments to the move buffer. The arc is * approximated by generating a large number of tiny, linear segments. @@ -354,7 +342,8 @@ stat_t mach_arc_feed(float target[], float flags[], // arc endpoints float radius, // non-zero radius implies radius mode uint8_t motion_mode) { // defined motion mode // trap missing feed rate - if (mach.gm.feed_rate_mode != INVERSE_TIME_MODE && fp_ZERO(mach.gm.feed_rate)) + if (fp_ZERO(mach.gm.feed_rate) || + (mach.gm.feed_rate_mode == INVERSE_TIME_MODE && !mach.gf.feed_rate)) return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; // set radius mode flag and do simple test(s) @@ -443,15 +432,15 @@ stat_t mach_arc_feed(float target[], float flags[], // arc endpoints // compute arc runtime values RITORNO(_compute_arc(mach.position, offset, rotations, full_circle)); - arc.run_state = MOVE_RUN; // enable arc run from the callback - mach_arc_callback(); // Queue initial arc moves - mach_finalize_move(); + arc.run_state = MOVE_RUN; // Enable arc run in callback + mach_arc_callback(); // Queue initial arc moves + copy_vector(mach.position, mach.gm.target); // update model position return STAT_OK; } -/* Generate an arc +/*** Generate an arc * * Called from the controller main loop. Each time it's called it queues * as many arc segments (lines) as it can before it blocks, then returns.