From 4c80e2a1bd6173d21fd1156d8e23d59a3d70a456 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Sun, 11 Sep 2016 07:51:33 -0700 Subject: [PATCH] Work in progress --- src/machine.c | 16 +++++++------- src/plan/buffer.c | 25 +++++++++++++++++++++- src/plan/buffer.h | 5 ++++- src/plan/calibrate.c | 2 +- src/plan/dwell.c | 2 +- src/plan/exec.c | 5 +++-- src/plan/jog.c | 2 +- src/plan/line.c | 7 ++++--- src/plan/planner.c | 50 ++++++++++++++++++++++++++++++-------------- 9 files changed, 80 insertions(+), 34 deletions(-) diff --git a/src/machine.c b/src/machine.c index ef71fab..f9bd1d9 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, mach_get_line()); + mp_queue_push(_exec_spindle_speed, false, 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, mach_get_line()); + mp_queue_push(_exec_spindle_mode, false, 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, mach_get_line()); + mp_queue_push(_exec_update_work_offsets, false, mach_get_line()); } } @@ -631,7 +631,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, mach_get_line()); + mp_queue_push(_exec_absolute_origin, false, mach_get_line()); } @@ -794,7 +794,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, mach_get_line()); + mp_queue_push(_exec_change_tool, false, mach_get_line()); } @@ -809,7 +809,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, mach_get_line()); + mp_queue_push(_exec_mist_coolant, false, mach_get_line()); } @@ -824,7 +824,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, mach_get_line()); + mp_queue_push(_exec_flood_coolant, false, mach_get_line()); } @@ -893,7 +893,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, mach_get_line()); + mp_queue_push(_exec_program_stop, true, mach_get_line()); } diff --git a/src/plan/buffer.c b/src/plan/buffer.c index e2f76bc..a9e30cc 100644 --- a/src/plan/buffer.c +++ b/src/plan/buffer.c @@ -129,10 +129,11 @@ 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, uint32_t line) { +void mp_queue_push(buffer_cb_t cb, bool plan, 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; @@ -151,3 +152,25 @@ 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 90e1f99..cf1bd10 100644 --- a/src/plan/buffer.h +++ b/src/plan/buffer.h @@ -56,6 +56,7 @@ 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 @@ -93,10 +94,12 @@ 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, uint32_t line); +void mp_queue_push(buffer_cb_t func, bool plan, 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 3e4919a..d616836 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, -1); + mp_queue_push(_exec_calibrate, false, -1); return 0; } diff --git a/src/plan/dwell.c b/src/plan/dwell.c index 07bba61..485acb9 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, line); + mp_queue_push(_exec_dwell, true, line); return STAT_OK; } diff --git a/src/plan/exec.c b/src/plan/exec.c index 612eb4d..6854ec4 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -247,7 +247,7 @@ static stat_t _exec_aline_section(float length, float vin, float vout) { // len / avg. velocity float move_time = 2 * length / (vin + vout); - ex.segments = ceil(usec(move_time) / NOM_SEGMENT_USEC); + ex.segments = ceil(move_time * MICROSECONDS_PER_MINUTE / NOM_SEGMENT_USEC); ex.segment_time = move_time / ex.segments; ex.segment_count = (uint32_t)ex.segments; @@ -551,7 +551,8 @@ 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_next(bf)->replannable = false; + mp_buffer_t *bp = mp_buffer_next_plan(bf); + if (bp) bp->replannable = false; mp_queue_pop(); // Free buffer diff --git a/src/plan/jog.c b/src/plan/jog.c index 79b5f49..f0b6068 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, -1); + mp_queue_push(_exec_jog, false, -1); } return STAT_OK; diff --git a/src/plan/line.c b/src/plan/line.c index c3a2003..6ab5561 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -240,8 +240,8 @@ static void _calc_and_cache_jerk_values(mp_buffer_t *bf) { static void _calc_max_velocities(mp_buffer_t *bf, float move_time) { - float junction_velocity = - _get_junction_vmax(mp_buffer_prev(bf)->unit, bf->unit); + mp_buffer_t *bp = mp_buffer_prev_plan(bf); + float junction_velocity = bp ? _get_junction_vmax(bp->unit, bf->unit) : 0; bf->cruise_vmax = bf->length / move_time; // target velocity requested bf->entry_vmax = min(bf->cruise_vmax, junction_velocity); @@ -307,9 +307,10 @@ 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, line); // After position update + mp_queue_push(mp_exec_aline, true, line); // After position update return STAT_OK; } diff --git a/src/plan/planner.c b/src/plan/planner.c index 18d3545..d43ec2a 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -261,7 +261,8 @@ 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) { - bf->entry_velocity = mp_buffer_prev(bf)->exit_velocity; + mp_buffer_t *bp = mp_buffer_prev_plan(bf); + bf->entry_velocity = bp ? bp->exit_velocity : 0; if (fp_NOT_ZERO(bf->entry_velocity)) { bf->cruise_velocity = bf->entry_velocity; @@ -495,8 +496,7 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { * * [1] Whether or not a block is planned is controlled by the bf->replannable * setting. Replan flags are checked during the backwards pass. They prune - * the replan list to include only the the latest blocks that require - * planning. + * the replan list to include only the latest blocks that require planning. * * In normal operation, the first block (currently running block) is not * replanned, but may be for feedholds and feed overrides. In these cases, @@ -507,40 +507,52 @@ 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(mp_buffer_next(bp)->entry_vmax, - mp_buffer_next(bp)->braking_velocity) + bp->delta_vmax; + min(next->entry_vmax, next->braking_velocity) + bp->delta_vmax; + next = bp; } // Forward planning pass. Recompute trapezoids from the first block to bf. + mp_buffer_t *prev = bp; while ((bp = mp_buffer_next(bp)) != bf) { - if (mp_buffer_prev(bp) == bf) - bp->entry_velocity = bp->entry_vmax; // first block - else bp->entry_velocity = mp_buffer_prev(bp)->exit_velocity; // other blocks + 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); bp->cruise_velocity = bp->cruise_vmax; - bp->exit_velocity = min4(bp->exit_vmax, mp_buffer_next(bp)->entry_vmax, - mp_buffer_next(bp)->braking_velocity, + bp->exit_velocity = min4(bp->exit_vmax, next->entry_vmax, + next->braking_velocity, bp->entry_velocity + bp->delta_vmax); mp_calculate_trapezoid(bp); // Test for optimally planned trapezoids by checking exit conditions if ((fp_EQ(bp->exit_velocity, bp->exit_vmax) || - fp_EQ(bp->exit_velocity, mp_buffer_next(bp)->entry_vmax)) || - (!mp_buffer_prev(bp)->replannable && + fp_EQ(bp->exit_velocity, next->entry_vmax)) || + (!prev->replannable && fp_EQ(bp->exit_velocity, (bp->entry_velocity + bp->delta_vmax)))) bp->replannable = false; + + prev = bp; } // Finish last block - bp->entry_velocity = mp_buffer_prev(bp)->exit_velocity; + bp->entry_velocity = prev->exit_velocity; bp->cruise_velocity = bp->cruise_vmax; bp->exit_velocity = 0; @@ -554,12 +566,18 @@ 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; - if (mp_buffer_next(bp)->run_state == MOVE_OFF || mp_buffer_next(bp) == bf) - break; - bp = mp_buffer_next(bp); + mp_buffer_t *next = mp_buffer_next(bp); + if (next->run_state == MOVE_OFF || next == bf) break; + bp = next; } // Plan blocks -- 2.27.0