From: Joseph Coffland Date: Mon, 12 Sep 2016 03:52:21 +0000 (-0700) Subject: Smooth holds with intermixed nonstop commands. X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=a1c9a848032e4cbd52605d37347ee17bcadf1652;p=bbctrl-firmware Smooth holds with intermixed nonstop commands. --- diff --git a/src/axes.c b/src/axes.c index 5d94394..f11efef 100644 --- a/src/axes.c +++ b/src/axes.c @@ -111,131 +111,50 @@ axis_config_t axes[AXES] = { /* Jerk functions * - * Jerk values can be rather large, often in the billions. This makes - * for some pretty big numbers for people to deal with. Jerk values - * are stored in the system in truncated format; values are divided by - * 1,000,000 then reconstituted before use. + * Jerk values can be rather large. Jerk values are stored in the system in + * truncated format; values are divided by 1,000,000 then multiplied before use. * - * The axis_jerk() functions expect the jerk in divided-by 1,000,000 form + * The axis_jerk() functions expect the jerk in divided by 1,000,000 form. */ -/// returns jerk for an axis -float axes_get_jerk(uint8_t axis) { - return axes[axis].jerk_max; -} +/// Returns axis jerk +float axes_get_jerk(uint8_t axis) {return axes[axis].jerk_max;} -/// sets the jerk for an axis, including recirpcal and cached values +/// Sets jerk and its reciprocal for axis void axes_set_jerk(uint8_t axis, float jerk) { axes[axis].jerk_max = jerk; axes[axis].recip_jerk = 1 / (jerk * JERK_MULTIPLIER); } -uint8_t get_axis_mode(int axis) { - return axes[axis].axis_mode; -} +uint8_t get_axis_mode(int axis) {return axes[axis].axis_mode;} void set_axis_mode(int axis, uint8_t value) { - if (value < AXIS_MODE_MAX) - axes[axis].axis_mode = value; -} - - -float get_max_velocity(int axis) { - return axes[axis].velocity_max; -} - - -void set_max_velocity(int axis, float value) { - axes[axis].velocity_max = value; -} - - -float get_max_feedrate(int axis) { - return axes[axis].feedrate_max; -} - - -void set_max_feedrate(int axis, float value) { - axes[axis].feedrate_max = value; -} - - -float get_max_jerk(int axis) { - return axes[axis].jerk_max; -} - - -void set_max_jerk(int axis, float value) { - axes[axis].jerk_max = value; -} - - -float get_junction_dev(int axis) { - return axes[axis].junction_dev; -} - - -void set_junction_dev(int axis, float value) { - axes[axis].junction_dev = value; -} - - -float get_travel_min(int axis) { - return axes[axis].travel_min; -} - - -void set_travel_min(int axis, float value) { - axes[axis].travel_min = value; -} - - -float get_travel_max(int axis) { - return axes[axis].travel_max; -} - - -void set_travel_max(int axis, float value) { - axes[axis].travel_max = value; -} - - -float get_jerk_homing(int axis) { - return axes[axis].jerk_homing; -} - - -void set_jerk_homing(int axis, float value) { - axes[axis].jerk_homing = value; -} - - -float get_search_vel(int axis) { - return axes[axis].search_velocity; -} - - -void set_search_vel(int axis, float value) { - axes[axis].search_velocity = value; -} - - -float get_latch_vel(int axis) { - return axes[axis].latch_velocity; -} - - -void set_latch_vel(int axis, float value) { - axes[axis].latch_velocity = value; -} - - -float get_latch_backoff(int axis) { - return axes[axis].latch_backoff; -} + if (value <= AXIS_RADIUS) axes[axis].axis_mode = value; +} + + +float get_max_velocity(int axis) {return axes[axis].velocity_max;} +void set_max_velocity(int axis, float value) {axes[axis].velocity_max = value;} +float get_max_feedrate(int axis) {return axes[axis].feedrate_max;} +void set_max_feedrate(int axis, float value) {axes[axis].feedrate_max = value;} +float get_max_jerk(int axis) {return axes[axis].jerk_max;} +void set_max_jerk(int axis, float value) {axes[axis].jerk_max = value;} +float get_junction_dev(int axis) {return axes[axis].junction_dev;} +void set_junction_dev(int axis, float value) {axes[axis].junction_dev = value;} +float get_travel_min(int axis) {return axes[axis].travel_min;} +void set_travel_min(int axis, float value) {axes[axis].travel_min = value;} +float get_travel_max(int axis) {return axes[axis].travel_max;} +void set_travel_max(int axis, float value) {axes[axis].travel_max = value;} +float get_jerk_homing(int axis) {return axes[axis].jerk_homing;} +void set_jerk_homing(int axis, float value) {axes[axis].jerk_homing = value;} +float get_search_vel(int axis) {return axes[axis].search_velocity;} +void set_search_vel(int axis, float value) {axes[axis].search_velocity = value;} +float get_latch_vel(int axis) {return axes[axis].latch_velocity;} +void set_latch_vel(int axis, float value) {axes[axis].latch_velocity = value;} +float get_latch_backoff(int axis) {return axes[axis].latch_backoff;} void set_latch_backoff(int axis, float value) { @@ -243,11 +162,5 @@ void set_latch_backoff(int axis, float value) { } -float get_zero_backoff(int axis) { - return axes[axis].zero_backoff; -} - - -void set_zero_backoff(int axis, float value) { - axes[axis].zero_backoff = value; -} +float get_zero_backoff(int axis) {return axes[axis].zero_backoff;} +void set_zero_backoff(int axis, float value) {axes[axis].zero_backoff = value;} diff --git a/src/axes.h b/src/axes.h index e50071d..2f3d728 100644 --- a/src/axes.h +++ b/src/axes.h @@ -32,11 +32,9 @@ typedef enum { - AXIS_DISABLED, // disable axis - AXIS_STANDARD, // axis in coordinated motion w/standard behaviors - AXIS_INHIBITED, // axis is computed but not activated - AXIS_RADIUS, // rotary axis calibrated to circumference - AXIS_MODE_MAX, + AXIS_DISABLED, // disabled axis + AXIS_STANDARD, // axis in coordinated motion w/standard behaviors + AXIS_RADIUS, // rotary axis calibrated to circumference } axis_mode_t; diff --git a/src/machine.c b/src/machine.c index 409a77a..c4f064c 100644 --- a/src/machine.c +++ b/src/machine.c @@ -151,7 +151,7 @@ static stat_t _exec_spindle_speed(mp_buffer_t *bf) { void mach_set_spindle_speed(float speed) { mp_buffer_t *bf = mp_queue_get_tail(); bf->value = speed * mach_get_spindle_override(); - mp_queue_push(_exec_spindle_speed, false, mach_get_line()); + mp_queue_push_nonstop(_exec_spindle_speed, mach_get_line()); } @@ -166,7 +166,7 @@ static stat_t _exec_spindle_mode(mp_buffer_t *bf) { void mach_set_spindle_mode(spindle_mode_t mode) { mp_buffer_t *bf = mp_queue_get_tail(); bf->value = mode; - mp_queue_push(_exec_spindle_mode, false, mach_get_line()); + mp_queue_push_nonstop(_exec_spindle_mode, mach_get_line()); } @@ -253,7 +253,7 @@ void mach_update_work_offsets() { if (!same) { mp_buffer_t *bf = mp_queue_get_tail(); copy_vector(bf->target, work_offset); - mp_queue_push(_exec_update_work_offsets, false, mach_get_line()); + mp_queue_push_nonstop(_exec_update_work_offsets, mach_get_line()); } } @@ -385,54 +385,38 @@ float mach_calc_move_time(const float axis_length[], * - conversion of relative mode to absolute (internal canonical form) * - translation of work coordinates to machine coordinates (internal * canonical form) - * - computation and application of axis modes as so: - * - * DISABLED - Incoming value is ignored. Target value is not changed - * ENABLED - Convert axis values to canonical format and store as target - * INHIBITED - Same processing as ENABLED, but axis will not actually be run - * RADIUS - ABC axis value is provided in Gcode block in linear units - * - Target is set to degrees based on axis' Radius value - * - Radius mode is only processed for ABC axes. Application to - * XYZ is ignored. - * - * Target coordinates are provided in target[] - * Axes that need processing are signaled in flag[] + * - application of axis modes: + * + * DISABLED - Incoming value is ignored. + * ENABLED - Convert axis values to canonical format. + * RADIUS - ABC axis value is provided in Gcode block in linear units. + * - Target is set to degrees based on axis' Radius value. + * + * Target coordinates are provided in @param values. + * Axes that need processing are signaled in @param flags. */ -void mach_calc_model_target(float target[], const float values[], - const bool flags[]) { - // process XYZABC for lower modes - for (int axis = AXIS_X; axis <= AXIS_Z; axis++) { - if (!flags[axis] || axes[axis].axis_mode == AXIS_DISABLED) - continue; // skip axis if not flagged for update or its disabled - - if (axes[axis].axis_mode == AXIS_STANDARD || - axes[axis].axis_mode == AXIS_INHIBITED) { - if (mach.gm.distance_mode == ABSOLUTE_MODE) - target[axis] = - mach_get_active_coord_offset(axis) + TO_MILLIMETERS(values[axis]); - else target[axis] = mach.position[axis] + TO_MILLIMETERS(values[axis]); - } - } +void mach_calc_target(float target[], const float values[], + const bool flags[]) { + for (int axis = 0; axis < AXES; axis++) { + target[axis] = mach.position[axis]; + if (!flags[axis]) continue; - // Note: The ABC loop below relies on the XYZ loop having been run first - for (int axis = AXIS_A; axis <= AXIS_C; axis++) { - if (!flags[axis] || axes[axis].axis_mode == AXIS_DISABLED) - continue; // skip axis if not flagged for update or its disabled + const float offset = mach.gm.distance_mode == ABSOLUTE_MODE ? + mach_get_active_coord_offset(axis) : mach.position[axis]; - float tmp; switch (axes[axis].axis_mode) { + case AXIS_DISABLED: break; case AXIS_STANDARD: - case AXIS_INHIBITED: - tmp = values[axis]; // no mm conversion - it's in degrees - - default: - tmp = TO_MILLIMETERS(values[axis]) * 360 / (2 * M_PI * axes[axis].radius); + // For ABC axes no mm conversion - it's already in degrees + target[axis] = + offset + (AXIS_Z < axis ? values[axis] : TO_MM(values[axis])); + break; + + case AXIS_RADIUS: + target[axis] = + offset + TO_MM(values[axis]) * 360 / (2 * M_PI * axes[axis].radius); + break; } - - if (mach.gm.distance_mode == ABSOLUTE_MODE) - // sacidu93's fix to Issue #22 - target[axis] = tmp + mach_get_active_coord_offset(axis); - else target[axis] = mach.position[axis] + tmp; } } @@ -440,7 +424,7 @@ void mach_calc_model_target(float target[], const float values[], /*** Return error code if soft limit is exceeded * * Must be called with target properly set in GM struct. Best done - * after mach_calc_model_target(). + * after mach_calc_target(). * * Tests for soft limit for any homed axis if min and max are * different values. You can set min and max to 0,0 to disable soft @@ -532,7 +516,7 @@ void mach_set_coord_offsets(coord_system_t coord_system, float offset[], for (int axis = 0; axis < AXES; axis++) if (flags[axis]) - mach.offset[coord_system][axis] = TO_MILLIMETERS(offset[axis]); + mach.offset[coord_system][axis] = TO_MM(offset[axis]); } @@ -623,7 +607,7 @@ void mach_set_absolute_origin(float origin[], bool flags[]) { for (int axis = 0; axis < AXES; axis++) if (flags[axis]) { - value[axis] = TO_MILLIMETERS(origin[axis]); + value[axis] = TO_MM(origin[axis]); mach.position[axis] = value[axis]; // set model position mp_set_axis_position(axis, value[axis]); // set mm position } @@ -631,7 +615,7 @@ void mach_set_absolute_origin(float origin[], bool flags[]) { mp_buffer_t *bf = mp_queue_get_tail(); copy_vector(bf->target, origin); copy_vector(bf->unit, flags); - mp_queue_push(_exec_absolute_origin, false, mach_get_line()); + mp_queue_push_nonstop(_exec_absolute_origin, mach_get_line()); } @@ -646,7 +630,7 @@ void mach_set_origin_offsets(float offset[], bool flags[]) { for (int axis = 0; axis < AXES; axis++) if (flags[axis]) mach.origin_offset[axis] = mach.position[axis] - - mach.offset[mach.gm.coord_system][axis] - TO_MILLIMETERS(offset[axis]); + mach.offset[mach.gm.coord_system][axis] - TO_MM(offset[axis]); } @@ -673,8 +657,7 @@ void mach_resume_origin_offsets() { // Free Space Motion (4.3.4) static stat_t _feed(float values[], bool flags[]) { float target[AXES]; - copy_vector(target, mach.position); - mach_calc_model_target(target, values, flags); + mach_calc_target(target, values, flags); // test soft limits stat_t status = mach_test_soft_limits(target); @@ -739,7 +722,7 @@ void mach_set_feed_rate(float feed_rate) { // normalize to minutes (active for this gcode block only) mach.gm.feed_rate = feed_rate ? 1 / feed_rate : 0; // Avoid div by zero - else mach.gm.feed_rate = TO_MILLIMETERS(feed_rate); + else mach.gm.feed_rate = TO_MM(feed_rate); } @@ -795,7 +778,7 @@ static stat_t _exec_change_tool(mp_buffer_t *bf) { void mach_change_tool(bool x) { mp_buffer_t *bf = mp_queue_get_tail(); bf->value = mach.gm.tool; - mp_queue_push(_exec_change_tool, false, mach_get_line()); + mp_queue_push_nonstop(_exec_change_tool, mach_get_line()); } @@ -810,7 +793,7 @@ static stat_t _exec_mist_coolant(mp_buffer_t *bf) { void mach_mist_coolant_control(bool mist_coolant) { mp_buffer_t *bf = mp_queue_get_tail(); bf->value = mist_coolant; - mp_queue_push(_exec_mist_coolant, false, mach_get_line()); + mp_queue_push_nonstop(_exec_mist_coolant, mach_get_line()); } @@ -825,7 +808,7 @@ static stat_t _exec_flood_coolant(mp_buffer_t *bf) { void mach_flood_coolant_control(bool flood_coolant) { mp_buffer_t *bf = mp_queue_get_tail(); bf->value = flood_coolant; - mp_queue_push(_exec_flood_coolant, false, mach_get_line()); + mp_queue_push_nonstop(_exec_flood_coolant, mach_get_line()); } @@ -894,7 +877,7 @@ static stat_t _exec_program_stop(mp_buffer_t *bf) { /// M0 Queue a program stop void mach_program_stop() { - mp_queue_push(_exec_program_stop, true, mach_get_line()); + mp_queue_push(_exec_program_stop, mach_get_line()); } diff --git a/src/machine.h b/src/machine.h index 50b9f06..2151ddb 100644 --- a/src/machine.h +++ b/src/machine.h @@ -34,7 +34,7 @@ #include "gcode_state.h" -#define TO_MILLIMETERS(a) (mach_get_units() == INCHES ? (a) * MM_PER_INCH : a) +#define TO_MM(a) (mach_get_units() == INCHES ? (a) * MM_PER_INCH : a) // Model state getters and setters uint32_t mach_get_line(); @@ -66,7 +66,7 @@ float mach_get_axis_position(uint8_t axis); // Critical helpers float mach_calc_move_time(const float axis_length[], const float axis_square[]); -void mach_calc_model_target(float target[], const float values[], +void mach_calc_target(float target[], const float values[], const bool flags[]); stat_t mach_test_soft_limits(float target[]); diff --git a/src/plan/arc.c b/src/plan/arc.c index d7eaf39..6aeab3d 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -412,7 +412,7 @@ stat_t mach_arc_feed(float values[], bool values_f[], // arc endpoints // Test radius arcs for radius tolerance if (radius_f) { - arc.radius = TO_MILLIMETERS(radius); // set to internal format (mm) + arc.radius = TO_MM(radius); // set to internal format (mm) if (fabs(arc.radius) < MIN_ARC_RADIUS) // radius value must be > minimum return STAT_ARC_RADIUS_OUT_OF_TOLERANCE; @@ -434,8 +434,7 @@ stat_t mach_arc_feed(float values[], bool values_f[], // arc endpoints // Set model target const float *position = mach_get_position(); - copy_vector(arc.target, position); - mach_calc_model_target(arc.target, values, values_f); + mach_calc_target(arc.target, values, values_f); // in radius mode it's an error for start == end if (radius_f && fp_EQ(position[AXIS_X], arc.target[AXIS_X]) && @@ -447,10 +446,10 @@ stat_t mach_arc_feed(float values[], bool values_f[], // arc endpoints mach_set_motion_mode(motion_mode); mach_update_work_offsets(); // Update resolved offsets arc.line = mach_get_line(); // copy line number - arc.radius = TO_MILLIMETERS(radius); // set arc radius or zero + arc.radius = TO_MM(radius); // set arc radius or zero float offset[3]; - for (int i = 0; i < 3; i++) offset[i] = TO_MILLIMETERS(offsets[i]); + for (int i = 0; i < 3; i++) offset[i] = TO_MM(offsets[i]); if (mach_get_arc_distance_mode() == ABSOLUTE_MODE) { if (offsets_f[0]) offset[0] -= position[0]; diff --git a/src/plan/buffer.c b/src/plan/buffer.c index 7cd1929..c900a74 100644 --- a/src/plan/buffer.c +++ b/src/plan/buffer.c @@ -35,8 +35,11 @@ #include "buffer.h" #include "state.h" #include "rtc.h" +#include "util.h" #include +#include +#include typedef struct { @@ -119,7 +122,6 @@ bool mp_queue_is_empty() {return mb.tail == mb.head;} /// Get pointer to next buffer, waiting until one is available. mp_buffer_t *mp_queue_get_tail() { while (!mb.space) continue; // Wait for a buffer - mb.tail->run_state = MOVE_NEW; return mb.tail; } @@ -130,13 +132,13 @@ mp_buffer_t *mp_queue_get_tail() { * buffer once it has been queued. Action may start on the buffer immediately, * invalidating its contents */ -void mp_queue_push(buffer_cb_t cb, bool plan, uint32_t line) { +void mp_queue_push(buffer_cb_t cb, uint32_t line) { mp_state_running(); mb.tail->ts = rtc_get_time(); - mb.tail->plan = plan; mb.tail->cb = cb; mb.tail->line = line; + mb.tail->run_state = MOVE_NEW; _push(); } @@ -152,25 +154,3 @@ void mp_queue_pop() { _clear_buffer(mb.head); _pop(); } - - -mp_buffer_t *mp_buffer_prev_plan(mp_buffer_t *bp) { - for (int i = 0; i < PLANNER_BUFFER_POOL_SIZE; i++) { - bp = mp_buffer_prev(bp); - if (bp->run_state == MOVE_OFF) break; - if (bp->plan) return bp; - } - - return 0; -} - - -mp_buffer_t *mp_buffer_next_plan(mp_buffer_t *bp) { - for (int i = 0; i < PLANNER_BUFFER_POOL_SIZE; i++) { - bp = mp_buffer_next(bp); - if (bp->run_state == MOVE_OFF) break; - if (bp->plan) return bp; - } - - return 0; -} diff --git a/src/plan/buffer.h b/src/plan/buffer.h index cf1bd10..90e1f99 100644 --- a/src/plan/buffer.h +++ b/src/plan/buffer.h @@ -56,7 +56,6 @@ typedef struct mp_buffer_t { // See Planning Velocity Notes buffer_cb_t cb; // callback to buffer exec function run_state_t run_state; // run state machine sequence - bool plan; // if false, ignored by the planner bool replannable; // true if move can be re-planned float value; // used in dwell and other callbacks @@ -94,12 +93,10 @@ uint8_t mp_queue_get_fill(); bool mp_queue_is_empty(); mp_buffer_t *mp_queue_get_tail(); -void mp_queue_push(buffer_cb_t func, bool plan, uint32_t line); +void mp_queue_push(buffer_cb_t func, uint32_t line); mp_buffer_t *mp_queue_get_head(); void mp_queue_pop(); static inline mp_buffer_t *mp_buffer_prev(mp_buffer_t *bp) {return bp->prev;} static inline mp_buffer_t *mp_buffer_next(mp_buffer_t *bp) {return bp->next;} -mp_buffer_t *mp_buffer_prev_plan(mp_buffer_t *bp); -mp_buffer_t *mp_buffer_next_plan(mp_buffer_t *bp); diff --git a/src/plan/calibrate.c b/src/plan/calibrate.c index d616836..18e873a 100644 --- a/src/plan/calibrate.c +++ b/src/plan/calibrate.c @@ -169,7 +169,7 @@ uint8_t command_calibrate(int argc, char *argv[]) { mp_set_cycle(CYCLE_CALIBRATING); cal.motor = 1; - mp_queue_push(_exec_calibrate, false, -1); + mp_queue_push_nonstop(_exec_calibrate, -1); return 0; } diff --git a/src/plan/dwell.c b/src/plan/dwell.c index 485acb9..07bba61 100644 --- a/src/plan/dwell.c +++ b/src/plan/dwell.c @@ -48,7 +48,7 @@ static stat_t _exec_dwell(mp_buffer_t *bf) { stat_t mp_dwell(float seconds, int32_t line) { mp_buffer_t *bf = mp_queue_get_tail(); bf->value = seconds; // in seconds, not minutes - mp_queue_push(_exec_dwell, true, line); + mp_queue_push(_exec_dwell, line); return STAT_OK; } diff --git a/src/plan/exec.c b/src/plan/exec.c index 6854ec4..c16c08e 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -33,6 +33,7 @@ #include "runtime.h" #include "state.h" #include "rtc.h" +#include "usart.h" #include "config.h" #include @@ -332,7 +333,7 @@ static stat_t _exec_aline_head() { static float _compute_next_segment_velocity() { if (ex.section_new) { - if (ex.section == SECTION_HEAD) return ex.entry_velocity; + if (ex.section == SECTION_HEAD) return mp_runtime_get_velocity(); else return ex.cruise_velocity; } @@ -375,9 +376,15 @@ static void _plan_hold() { ex.cruise_velocity = braking_velocity; ex.hold_planned = true; - // Case 1: deceleration fits entirely into the length remaining in buffer - if (braking_length <= available_length) { - // Set to a tail to perform the deceleration + // Avoid creating segments before or after the hold which are too small. + if (fabs(available_length - braking_length) < HOLD_DECELERATION_TOLERANCE) { + // Case 0: deceleration fits almost exactly + ex.exit_velocity = 0; + ex.tail_length = available_length; + + } else if (braking_length <= available_length) { + // Case 1: deceleration fits entirely into the remaining length + // Setup tail to perform the deceleration ex.exit_velocity = 0; ex.tail_length = braking_length; @@ -387,7 +394,8 @@ static void _plan_hold() { bf->entry_vmax = 0; bf->run_state = MOVE_RESTART; // Restart the buffer when done - } else { // Case 2: deceleration exceeds length remaining in buffer + } else { + // Case 2: deceleration exceeds length remaining in buffer // Replan to minimum (but non-zero) exit velocity ex.tail_length = available_length; ex.exit_velocity = @@ -522,19 +530,21 @@ stat_t mp_exec_move() { if (bf->run_state == MOVE_NEW) { // On restart wait a bit to give planner queue a chance to fill if (!mp_runtime_is_busy() && mp_queue_get_fill() < 4 && - !rtc_expired(bf->ts + 250)) return STAT_NOOP; + !rtc_expired(bf->ts + 250)) return STAT_NOOP; // Take control of buffer bf->run_state = MOVE_INIT; bf->replannable = false; // Update runtime - mp_runtime_set_busy(true); mp_runtime_set_line(bf->line); } stat_t status = bf->cb(bf); // Move callback + // Busy only if a move was queued + if (status == STAT_EAGAIN || status == STAT_OK) mp_runtime_set_busy(true); + if (status != STAT_EAGAIN) { bool idle = false; @@ -551,10 +561,9 @@ stat_t mp_exec_move() { // Solves a potential race condition where the current move ends but // the new move has not started because the current move is still // being run by the steppers. Planning can overwrite the new move. - mp_buffer_t *bp = mp_buffer_next_plan(bf); - if (bp) bp->replannable = false; + mp_buffer_next(bf)->replannable = false; - mp_queue_pop(); // Free buffer + mp_queue_pop(); // Release buffer // Enter READY state if (mp_queue_is_empty()) { diff --git a/src/plan/jog.c b/src/plan/jog.c index f0b6068..4463301 100644 --- a/src/plan/jog.c +++ b/src/plan/jog.c @@ -127,7 +127,7 @@ uint8_t command_jog(int argc, char *argv[]) { if (!mp_jog_busy()) { mp_set_cycle(CYCLE_JOGGING); - mp_queue_push(_exec_jog, false, -1); + mp_queue_push_nonstop(_exec_jog, -1); } return STAT_OK; diff --git a/src/plan/line.c b/src/plan/line.c index 6ab5561..053d90c 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -132,6 +132,8 @@ static float _get_junction_vmax(const float a_unit[], const float b_unit[]) { b_delta += square(b_unit[axis] * axes[axis].junction_dev); } + if (!a_delta || !b_delta) return 0; // One or both unit vectors are null + float delta = (sqrt(a_delta) + sqrt(b_delta)) / 2; float sintheta_over2 = sqrt((1 - costheta) / 2); float radius = delta * sintheta_over2 / (1 - sintheta_over2); @@ -240,8 +242,8 @@ static void _calc_and_cache_jerk_values(mp_buffer_t *bf) { static void _calc_max_velocities(mp_buffer_t *bf, float move_time) { - mp_buffer_t *bp = mp_buffer_prev_plan(bf); - float junction_velocity = bp ? _get_junction_vmax(bp->unit, bf->unit) : 0; + float junction_velocity = + _get_junction_vmax(mp_buffer_prev(bf)->unit, bf->unit); bf->cruise_vmax = bf->length / move_time; // target velocity requested bf->entry_vmax = min(bf->cruise_vmax, junction_velocity); @@ -307,10 +309,9 @@ stat_t mp_aline(const float target[], int32_t line) { _calc_max_velocities(bf, time); // Note, the following lines must remain in order. - bf->plan = true; mp_plan_block_list(bf); // Plan block list mp_set_position(target); // Set planner position before committing buffer - mp_queue_push(mp_exec_aline, true, line); // After position update + mp_queue_push(mp_exec_aline, line); // After position update return STAT_OK; } diff --git a/src/plan/planner.c b/src/plan/planner.c index d43ec2a..1097d28 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -64,6 +64,7 @@ #include "stepper.h" #include "motor.h" #include "state.h" +#include "usart.h" #include #include @@ -100,8 +101,7 @@ void mp_set_position(const float position[]) { void mp_flush_planner() {mp_queue_init();} -/* Performs axis mapping & conversion of length units to steps (and deals - * with inhibited axes) +/*** Performs axis mapping & conversion of length units to steps. * * The reason steps are returned as floats (as opposed to, say, * uint32_t) is to accommodate fractional steps. stepper.c deals @@ -118,11 +118,9 @@ void mp_kinematics(const float travel[], float steps[]) { // Most of the conversion math has already been done during config in // steps_per_unit() which takes axis travel, step angle and microsteps into // account. - for (int motor = 0; motor < MOTORS; motor++) { - int axis = motor_get_axis(motor); - if (axes[axis].axis_mode == AXIS_INHIBITED) steps[motor] = 0; - else steps[motor] = travel[axis] * motor_get_steps_per_unit(motor); - } + for (int motor = 0; motor < MOTORS; motor++) + steps[motor] = + travel[motor_get_axis(motor)] * motor_get_steps_per_unit(motor); } @@ -238,7 +236,9 @@ void mp_kinematics(const float travel[], float steps[]) { */ void mp_calculate_trapezoid(mp_buffer_t *bf) { // RULE #1 of mp_calculate_trapezoid(): Don't change bf->length - // + + if (!bf->length) return; + // F case: Block is too short - run time < minimum segment time // Force block into a single segment body with limited velocities // Accept the entry velocity, limit the cruise, and go for the best exit @@ -249,8 +249,8 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { if (naive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) { bf->cruise_velocity = bf->length / MIN_SEGMENT_TIME_PLUS_MARGIN; - bf->exit_velocity = max(0.0, min(bf->cruise_velocity, - (bf->entry_velocity - bf->delta_vmax))); + bf->exit_velocity = + max(0, min(bf->cruise_velocity, (bf->entry_velocity - bf->delta_vmax))); bf->body_length = bf->length; bf->head_length = 0; bf->tail_length = 0; @@ -261,8 +261,7 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { // B" case: Block is short, but fits into a single body segment if (naive_move_time <= NOM_SEGMENT_TIME) { - mp_buffer_t *bp = mp_buffer_prev_plan(bf); - bf->entry_velocity = bp ? bp->exit_velocity : 0; + bf->entry_velocity = mp_buffer_prev(bf)->exit_velocity; if (fp_NOT_ZERO(bf->entry_velocity)) { bf->cruise_velocity = bf->entry_velocity; @@ -441,6 +440,33 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { } +void mp_print_queue(mp_buffer_t *bf) { + printf_P(PSTR("{\"msg\":\",id,replannable,callback," + "length,head_length,body_length,tail_length," + "entry_velocity,cruise_velocity,exit_velocity,braking_velocity," + "entry_vmax,cruise_vmax,exit_vmax,\"}\n")); + + int i = 0; + mp_buffer_t *bp = bf; + while (bp) { + printf_P(PSTR("{\"msg\":\",%d,%d,0x%04x," + "%0.2f,%0.2f,%0.2f,%0.2f," + "%0.2f,%0.2f,%0.2f,%0.2f," + "%0.2f,%0.2f,%0.2f,\"}\n"), + i++, bp->replannable, bp->cb, + bp->length, bp->head_length, bp->body_length, bp->tail_length, + bp->entry_velocity, bp->cruise_velocity, bp->exit_velocity, + bp->braking_velocity, + bp->entry_vmax, bp->cruise_vmax, bp->exit_vmax); + + bp = mp_buffer_prev(bp); + if (bp == bf || bp->run_state == MOVE_OFF) break; + } + + while (!usart_tx_empty()) continue; +} + + /*** Plans the entire block list * * The block list is the circular buffer of planner buffers (bf's). The block @@ -507,15 +533,12 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { * optimizations. */ void mp_plan_block_list(mp_buffer_t *bf) { - ASSERT(bf->plan); // Must start with a plannable buffer - mp_buffer_t *bp = bf; // Backward planning pass. Find first block and update braking velocities. // By the end bp points to the buffer before the first block. mp_buffer_t *next = bp; while ((bp = mp_buffer_prev(bp)) != bf) { - if (!bp->plan && bp->run_state != MOVE_OFF) continue; if (!bp->replannable) break; bp->braking_velocity = min(next->entry_vmax, next->braking_velocity) + bp->delta_vmax; @@ -525,15 +548,9 @@ void mp_plan_block_list(mp_buffer_t *bf) { // Forward planning pass. Recompute trapezoids from the first block to bf. mp_buffer_t *prev = bp; while ((bp = mp_buffer_next(bp)) != bf) { - if (!bp->plan) continue; - - if (prev == bf) bp->entry_velocity = bp->entry_vmax; // first block - else bp->entry_velocity = prev->exit_velocity; // other blocks - - // Note, next cannot be null. Since bp != bf, bf is yet to come. - mp_buffer_t *next = mp_buffer_next_plan(bp); - ASSERT(next); + mp_buffer_t *next = mp_buffer_next(bp); + bp->entry_velocity = prev == bf ? bp->entry_vmax : prev->exit_velocity; bp->cruise_velocity = bp->cruise_vmax; bp->exit_velocity = min4(bp->exit_vmax, next->entry_vmax, next->braking_velocity, @@ -552,11 +569,13 @@ void mp_plan_block_list(mp_buffer_t *bf) { } // Finish last block - bp->entry_velocity = prev->exit_velocity; - bp->cruise_velocity = bp->cruise_vmax; - bp->exit_velocity = 0; + bf->entry_velocity = prev->exit_velocity; + bf->cruise_velocity = bf->cruise_vmax; + bf->exit_velocity = 0; - mp_calculate_trapezoid(bp); + mp_calculate_trapezoid(bf); + + //mp_print_queue(bf); } @@ -566,12 +585,6 @@ void mp_replan_blocks() { mp_buffer_t *bp = bf; - // Skip leading non-plannable blocks - while (!bp->plan) { - bp = mp_buffer_next(bp); - if (bp->run_state == MOVE_OFF || bp == bf) return; // Nothing to plan - } - // Mark all blocks replanable while (true) { bp->replannable = true; @@ -585,6 +598,19 @@ void mp_replan_blocks() { } +/// Push a non-stop command to the queue. I.e. one that does not cause the +/// planner to plan to zero. +void mp_queue_push_nonstop(buffer_cb_t cb, uint32_t line) { + mp_buffer_t *bp = mp_queue_get_tail(); + + bp->entry_vmax = bp->cruise_vmax = bp->exit_vmax = INFINITY; + copy_vector(bp->unit, bp->prev->unit); + bp->replannable = true; + + mp_queue_push(cb, line); +} + + /*** Derive accel/decel length from delta V and jerk * * A convenient function for determining the optimal_length (L) of a line diff --git a/src/plan/planner.h b/src/plan/planner.h index f6adb48..9c8262a 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -64,6 +64,12 @@ /// Adaptive velocity tolerance term #define TRAPEZOID_VELOCITY_TOLERANCE (max(2, bf->entry_velocity / 100)) +/*** If the absolute value of the remaining deceleration length would be less + * than this value then finish the deceleration in the current move. This is + * used to avoid creating segements before or after the hold which are too + * short to process correctly. + */ +#define HOLD_DECELERATION_TOLERANCE 1 // In mm typedef enum { SECTION_HEAD, // acceleration @@ -80,5 +86,6 @@ void mp_flush_planner(); void mp_kinematics(const float travel[], float steps[]); void mp_plan_block_list(mp_buffer_t *bf); void mp_replan_blocks(); +void mp_queue_push_nonstop(buffer_cb_t cb, uint32_t line); float mp_get_target_length(float Vi, float Vf, const mp_buffer_t *bf); float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf); diff --git a/src/vars.def b/src/vars.def index 945e092..ce50533 100644 --- a/src/vars.def +++ b/src/vars.def @@ -54,10 +54,8 @@ VAR(max_velocity, "vm", float, AXES, 1, 1, "Maxium velocity in mm/min") VAR(max_feedrate, "fr", float, AXES, 1, 1, "Maxium feedrate in mm/min") VAR(max_jerk, "jm", float, AXES, 1, 1, "Maxium jerk in mm/min^3") VAR(junction_dev, "jd", float, AXES, 1, 1, "Junction deviation") - VAR(travel_min, "tn", float, AXES, 1, 1, "Minimum soft limit") VAR(travel_max, "tm", float, AXES, 1, 1, "Maximum soft limit") - VAR(jerk_homing, "jh", float, AXES, 1, 1, "Maxium homing jerk") VAR(search_vel, "sv", float, AXES, 1, 1, "Homing search velocity") VAR(latch_vel, "lv", float, AXES, 1, 1, "Homing latch velocity")