From b310a6b5784b2ba03db4321d181a41426b57ad46 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Thu, 1 Sep 2016 18:00:22 -0700 Subject: [PATCH] Greatly simplified feedhold processing --- src/machine.c | 3 +- src/plan/buffer.c | 16 ++-- src/plan/buffer.h | 5 +- src/plan/exec.c | 85 ++++++++++++++++-- src/plan/feedhold.c | 205 -------------------------------------------- src/plan/feedhold.h | 31 ------- src/plan/line.c | 4 +- src/plan/planner.c | 35 +++++--- src/plan/planner.h | 5 +- src/plan/state.c | 45 ++-------- src/plan/state.h | 14 +-- 11 files changed, 121 insertions(+), 327 deletions(-) delete mode 100644 src/plan/feedhold.c delete mode 100644 src/plan/feedhold.h diff --git a/src/machine.c b/src/machine.c index 5df90de..18a8519 100644 --- a/src/machine.c +++ b/src/machine.c @@ -1105,7 +1105,8 @@ static void _exec_program_end(float *value, float *flag) { static void _exec_program_stop(float *value, float *flag) { - mp_set_hold_state(FEEDHOLD_HOLD); + // This is ok because we should be already stopped at this point + mp_state_holding(); } diff --git a/src/plan/buffer.c b/src/plan/buffer.c index ecd7da2..4a0fc4d 100644 --- a/src/plan/buffer.c +++ b/src/plan/buffer.c @@ -156,16 +156,16 @@ void mp_commit_write_buffer(uint32_t line, moveType_t type) { * The behavior supports continuations (iteration) */ mpBuf_t *mp_get_run_buffer() { - // CASE: fresh buffer; becomes running if queued or pending - if (mb.r->buffer_state == MP_BUFFER_QUEUED || - mb.r->buffer_state == MP_BUFFER_PENDING) + switch (mb.r->buffer_state) { + case MP_BUFFER_QUEUED: // fresh buffer; becomes running if queued or pending mb.r->buffer_state = MP_BUFFER_RUNNING; + // Fall through - // CASE: asking for the same run buffer for the Nth time - if (mb.r->buffer_state == MP_BUFFER_RUNNING) + case MP_BUFFER_RUNNING: // asking for the same run buffer for the Nth time return mb.r; // return same buffer - return 0; // CASE: no queued buffers. fail it. + default: return 0; // no queued buffers + } } @@ -173,10 +173,6 @@ mpBuf_t *mp_get_run_buffer() { void mp_free_run_buffer() { // EMPTY current run buf & adv to next mp_clear_buffer(mb.r); // clear it out (& reset replannable) mb.r = mb.r->nx; // advance to next run buffer - - if (mb.r->buffer_state == MP_BUFFER_QUEUED) // only if queued... - mb.r->buffer_state = MP_BUFFER_PENDING; // pend next buffer - mb.buffers_available++; report_request(); diff --git a/src/plan/buffer.h b/src/plan/buffer.h index 0765dd7..4bbf33c 100644 --- a/src/plan/buffer.h +++ b/src/plan/buffer.h @@ -53,7 +53,6 @@ typedef enum { // bf->buffer_state values MP_BUFFER_EMPTY, // struct is available for use (MUST BE 0) MP_BUFFER_LOADING, // being written ("checked out") MP_BUFFER_QUEUED, // in queue - MP_BUFFER_PENDING, // marked as the next buffer to run MP_BUFFER_RUNNING // current running buffer } bufferState_t; @@ -111,8 +110,8 @@ mpBuf_t *mp_get_run_buffer(); void mp_free_run_buffer(); mpBuf_t *mp_get_last_buffer(); /// Returns pointer to prev buffer in linked list -#define mp_get_prev_buffer(b) ((mpBuf_t *)(b->pv)) +#define mp_get_prev_buffer(b) (b->pv) /// Returns pointer to next buffer in linked list -#define mp_get_next_buffer(b) ((mpBuf_t *)(b->nx)) +#define mp_get_next_buffer(b) (b->nx) void mp_clear_buffer(mpBuf_t *bf); void mp_copy_buffer(mpBuf_t *bf, const mpBuf_t *bp); diff --git a/src/plan/exec.c b/src/plan/exec.c index 539ba97..9736c05 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -350,12 +350,73 @@ static stat_t _exec_aline_head() { return status; } +static float _compute_next_segment_velocity() { + if (mr.section == SECTION_BODY) return mr.segment_velocity; + return mr.segment_velocity + mr.forward_diff[4]; +} + + +/*** Replan current move to execute hold + * + * Holds are initiated by the planner entering STATE_STOPPING. In which case + * _plan_hold() is called to replan the current move towards zero. If it is + * unable to plan to zero in the remaining length of the current move it will + * decelerate as much as possible and then wait for the next move. Once it + * is possible to plan to zero velocity in the current move the remaining length + * is put into the run buffer, which is still allocated, and the run buffer + * becomes the hold point. The hold is left by a start request in state.c. At + * this point the remaining buffers, if any, are replanned from zero up to + * speed. + */ +void _plan_hold() { + mpBuf_t *bp = mp_get_run_buffer(); // working buffer pointer + if (!bp) return; // Oops! nothing's running + + // Examine and process mr buffer and compute length left for decel + float available_length = get_axis_vector_length(mr.final_target, mr.position); + // Compute next_segment velocity, velocity left to shed to brake to zero + float braking_velocity = _compute_next_segment_velocity(); + // Distance to brake to zero from braking_velocity, bp is OK to use here + float braking_length = mp_get_target_length(braking_velocity, 0, bp); + + // Hack to prevent Case 2 moves for perfect-fit decels. Happens in + // homing situations. The real fix: The braking velocity cannot + // simply be the mr.segment_velocity as this is the velocity of the + // last segment, not the one that's going to be executed next. The + // braking_velocity needs to be the velocity of the next segment + // that has not yet been computed. In the mean time, this hack will work. + if (available_length < braking_length && fp_ZERO(bp->exit_velocity)) + braking_length = available_length; + + // Replan mr to decelerate + mr.section = SECTION_TAIL; + mr.section_new = true; + mr.cruise_velocity = braking_velocity; + mr.hold_planned = true; + + // Case 1: deceleration fits entirely into the length remaining in mr buffer + if (braking_length <= available_length) { + // set mr to a tail to perform the deceleration + mr.exit_velocity = 0; + mr.tail_length = braking_length; + + // Re-use bp+0 to be the hold point and to run the remaining block length + bp->length = available_length - braking_length; + bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp); + bp->entry_vmax = 0; // set bp+0 as hold point + bp->move_state = MOVE_NEW; // tell _exec to re-use the bf buffer + + } else { // Case 2: deceleration exceeds length remaining in mr buffer + // Replan mr to minimum (but non-zero) exit velocity + mr.tail_length = available_length; + mr.exit_velocity = + braking_velocity - mp_get_target_velocity(0, available_length, bp); + } +} + /// Initializes move runtime with a new planner buffer static stat_t _exec_aline_init(mpBuf_t *bf) { - // Stop here if holding - if (mp_get_hold_state() == FEEDHOLD_HOLD) return STAT_NOOP; - // copy in the gcode model state memcpy(&mr.ms, &bf->ms, sizeof(MoveState_t)); bf->replannable = false; @@ -375,6 +436,7 @@ static stat_t _exec_aline_init(mpBuf_t *bf) { mr.active = true; mr.section = SECTION_HEAD; mr.section_new = true; + mr.hold_planned = false; copy_vector(mr.unit, bf->unit); copy_vector(mr.final_target, bf->ms.target); @@ -395,9 +457,7 @@ static stat_t _exec_aline_init(mpBuf_t *bf) { mr.waypoint[SECTION_BODY][axis] = mr.position[axis] + mr.unit[axis] * (mr.head_length + mr.body_length); - mr.waypoint[SECTION_TAIL][axis] = - mr.position[axis] + mr.unit[axis] * - (mr.head_length + mr.body_length + mr.tail_length); + mr.waypoint[SECTION_TAIL][axis] = mr.final_target[axis]; } return STAT_OK; @@ -469,7 +529,9 @@ static stat_t _exec_aline_init(mpBuf_t *bf) { * from _RUN to _OFF on final call or just remain _OFF */ stat_t mp_exec_aline(mpBuf_t *bf) { - if (bf->move_state == MOVE_OFF) return STAT_NOOP; + // Stop here if no more moves or holding + if (bf->move_state == MOVE_OFF || mp_get_state() == STATE_HOLDING) + return STAT_NOOP; stat_t status = STAT_OK; @@ -487,8 +549,6 @@ stat_t mp_exec_aline(mpBuf_t *bf) { case SECTION_TAIL: status = _exec_aline_tail(); break; } - mp_state_hold_callback(status == STAT_OK); - // There are 3 things that can happen here depending on return conditions: // // status bf->move_state Description @@ -500,11 +560,18 @@ stat_t mp_exec_aline(mpBuf_t *bf) { if (status != STAT_EAGAIN) { mr.active = false; // reset mr buffer bf->nx->replannable = false; // prevent overplanning (Note 2) + if (fp_ZERO(mr.exit_velocity)) mr.segment_velocity = 0; // Note, feedhold.c may change bf->move_state to reuse this buffer so it // can plan the deceleration. if (bf->move_state == MOVE_RUN) mp_free_run_buffer(); } + if (mp_get_state() == STATE_STOPPING && + (status == STAT_OK || status == STAT_EAGAIN)) { + if (!mr.active && !mr.segment_velocity) mp_state_holding(); + else if (mr.active && !mr.hold_planned) _plan_hold(); + } + return status; } diff --git a/src/plan/feedhold.c b/src/plan/feedhold.c deleted file mode 100644 index b9b52ad..0000000 --- a/src/plan/feedhold.c +++ /dev/null @@ -1,205 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "feedhold.h" - -#include "planner.h" -#include "buffer.h" -#include "line.h" -#include "util.h" -#include "state.h" - -#include -#include - - -/* Feedhold is executed as hold state transitions executed inside - * _exec_aline() and main loop callbacks to mp_plan_hold_callback() - * - * Holds work like this: - * - * - Hold state == SYNC tells the aline exec routine to execute the - * next aline segment then set hold state to PLAN. This gives the - * planner sufficient time to replan the block list for the hold - * before the next aline segment needs to be processed. - * - * - Hold state == PLAN tells the planner to replan the mr buffer, the - * current run buffer (bf), and any subsequent bf buffers as - * necessary to execute a hold. Hold planning replans the planner - * buffer queue down to zero and then back up from zero. Hold state - * is set to DECEL when planning is complete. - * - * - Hold state == DECEL persists until the aline execution runs to - * zero velocity, at which point hold state transitions to HOLD. - * - * - Hold state == HOLD persists until the cycle is restarted. A cycle - * start is an asynchronous event that sets the cycle_start_flag - * TRUE. It can occur any time after the hold is requested - either - * before or after motion stops. - * - * Terms used: - * - mr is the runtime buffer. It was initially loaded from the bf buffer - * - bp + 0 is the "companion" bf buffer to the mr buffer. - * - bp + 1 is the bf buffer following bp+0. This runs through bp + N - * - bp (by itself) refers to the current buffer being adjusted / replanned - * - * Details: - * Planning re-uses bp + 0 as an "extra" buffer. Normally bp + 0 - * is returned to the buffer pool as it is redundant once mr is - * loaded. Use the extra buffer to split the move in two where the - * hold decelerates to zero. Use one buffer to go to zero, the other - * to replan up from zero. All buffers past that point are - * unaffected other than that they need to be replanned for velocity. - * - * Note: - * There are multiple opportunities for more efficient - * organization of code in this module, but the code is so - * complicated I just left it organized for clarity and hoped for - * the best from compiler optimization. - */ - - -/// Resets all blocks in the planning list to be replannable -static void _reset_replannable_list() { - mpBuf_t *bf = mp_get_run_buffer(); - if (!bf) return; - - mpBuf_t *bp = bf; - - do { - bp->replannable = true; - } while ((bp = mp_get_next_buffer(bp)) != bf && bp->move_state != MOVE_OFF); -} - - -static float _compute_next_segment_velocity() { - if (mr.section == SECTION_BODY) return mr.segment_velocity; - return mr.segment_velocity + mr.forward_diff[4]; -} - - -/// Replan block list to execute hold -void mp_plan_hold_callback() { - if (mp_get_hold_state() != FEEDHOLD_PLAN) return; // not planning a feedhold - - mpBuf_t *bp = mp_get_run_buffer(); // working buffer pointer - if (!bp) return; // Oops! nothing's running - - // examine and process mr buffer and compute length left for decel - float mr_available_length = - get_axis_vector_length(mr.final_target, mr.position); - - // compute next_segment velocity, velocity left to shed to brake to zero - float braking_velocity = _compute_next_segment_velocity(); - // distance to brake to zero from braking_velocity, bp is OK to use here - float braking_length = mp_get_target_length(braking_velocity, 0, bp); - - // Hack to prevent Case 2 moves for perfect-fit decels. Happens in - // homing situations. The real fix: The braking velocity cannot - // simply be the mr.segment_velocity as this is the velocity of the - // last segment, not the one that's going to be executed next. The - // braking_velocity needs to be the velocity of the next segment - // that has not yet been computed. In the mean time, this hack will work. - if (mr_available_length < braking_length && fp_ZERO(bp->exit_velocity)) - braking_length = mr_available_length; - - // Case 1: deceleration fits entirely into the length remaining in mr buffer - if (braking_length <= mr_available_length) { - // set mr to a tail to perform the deceleration - mr.exit_velocity = 0; - mr.tail_length = braking_length; - mr.cruise_velocity = braking_velocity; - mr.section = SECTION_TAIL; - mr.section_new = true; - - // re-use bp+0 to be the hold point and to run the remaining block length - bp->length = mr_available_length - braking_length; - bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp); - bp->entry_vmax = 0; // set bp+0 as hold point - bp->move_state = MOVE_NEW; // tell _exec to re-use the bf buffer - - _reset_replannable_list(); // make it replan all the blocks - mp_plan_block_list(mp_get_last_buffer(), true); - mp_set_hold_state(FEEDHOLD_DECEL); // set state to decelerate and exit - - return; - } - - // Case 2: deceleration exceeds length remaining in mr buffer - // First, replan mr to minimum (but non-zero) exit velocity - mr.section = SECTION_TAIL; - mr.section_new = true; - mr.tail_length = mr_available_length; - mr.cruise_velocity = braking_velocity; - mr.exit_velocity = - braking_velocity - mp_get_target_velocity(0, mr_available_length, bp); - - // Find where deceleration reaches zero. This could span multiple buffers. - braking_velocity = mr.exit_velocity; // adjust braking velocity downward - bp->move_state = MOVE_NEW; // tell _exec to re-use buffer - - // A safety to avoid wraparound - for (int i = 0; i < PLANNER_BUFFER_POOL_SIZE; i++) { - mp_copy_buffer(bp, bp->nx); // copy bp+1 into bp+0, and onward - - // TODO What about dwells? Shouldn't we be stopped when we reach a dwell. - if (bp->move_type != MOVE_TYPE_ALINE) { // skip any non-move buffers - bp = mp_get_next_buffer(bp); // point to next buffer - continue; - } - - bp->entry_vmax = braking_velocity; // velocity we need to shed - braking_length = mp_get_target_length(braking_velocity, 0, bp); - - if (braking_length > bp->length) { // decel does not fit in bp buffer - bp->exit_vmax = - braking_velocity - mp_get_target_velocity(0, bp->length, bp); - braking_velocity = bp->exit_vmax; // braking velocity for next buffer - bp = mp_get_next_buffer(bp); // point to next buffer - continue; - } - - break; - } - - // Deceleration now fits in the current bp buffer - // Plan the first buffer of the pair as the decel, the second as the accel - bp->length = braking_length; - bp->exit_vmax = 0; - - bp = mp_get_next_buffer(bp); // point to the acceleration buffer - bp->entry_vmax = 0; - bp->length -= braking_length; // identical buffers, hence their lengths - bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp); - bp->exit_vmax = bp->delta_vmax; - - _reset_replannable_list(); // replan all the blocks - mp_plan_block_list(mp_get_last_buffer(), true); - mp_set_hold_state(FEEDHOLD_DECEL); // set state to decelerate -} diff --git a/src/plan/feedhold.h b/src/plan/feedhold.h deleted file mode 100644 index dcf70aa..0000000 --- a/src/plan/feedhold.h +++ /dev/null @@ -1,31 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - - -void mp_plan_hold_callback(); diff --git a/src/plan/line.c b/src/plan/line.c index 945fc84..e710e73 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -370,8 +370,8 @@ stat_t mp_aline(MoveState_t *ms) { // NOTE: these next lines must remain in exact order. Position must update // before committing the buffer. - mp_plan_block_list(bf, false); // replan block list - copy_vector(mm.position, bf->ms.target); // set the planner position + mp_plan_block_list(bf); // plan block list + copy_vector(mm.position, bf->ms.target); // set the planner position // commit current block (must follow the position update) mp_commit_write_buffer(ms->line, MOVE_TYPE_ALINE); diff --git a/src/plan/planner.c b/src/plan/planner.c index fd19618..1086e1f 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -141,10 +141,6 @@ void mp_set_steps_to_runtime_position() { } -/// Correct velocity in last segment for reporting purposes -void mp_zero_segment_velocity() {mr.segment_velocity = 0;} - - /// Returns current velocity (aggregate) float mp_get_runtime_velocity() {return mr.segment_velocity;} @@ -586,12 +582,8 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { * These routines also set all blocks in the list to be * replannable so the list can be recomputed regardless of * exact stops and previous replanning optimizations. - * - * [2] The mr_flag is used to tell replan to account for mr - * buffer's exit velocity (Vx) mr's Vx is always found in the - * provided bf buffer. Used to replan feedholds */ -void mp_plan_block_list(mpBuf_t *bf, bool mr_flag) { +void mp_plan_block_list(mpBuf_t *bf) { mpBuf_t *bp = bf; // Backward planning pass. Find first block and update braking velocities. @@ -604,11 +596,8 @@ void mp_plan_block_list(mpBuf_t *bf, bool mr_flag) { // Forward planning pass. Recompute trapezoids from the first block to bf. while ((bp = mp_get_next_buffer(bp)) != bf) { - if (bp->pv == bf || mr_flag) { - bp->entry_velocity = bp->entry_vmax; // first block - mr_flag = false; - - } else bp->entry_velocity = bp->pv->exit_velocity; // other blocks + if (bp->pv == bf) bp->entry_velocity = bp->entry_vmax; // first block + else bp->entry_velocity = bp->pv->exit_velocity; // other blocks bp->cruise_velocity = bp->cruise_vmax; bp->exit_velocity = min4(bp->exit_vmax, bp->nx->entry_vmax, @@ -634,6 +623,24 @@ void mp_plan_block_list(mpBuf_t *bf, bool mr_flag) { } +void mp_replan_blocks() { + mpBuf_t *bf = mp_get_run_buffer(); + mpBuf_t *bp = bf; + + // Mark all blocks replanable + while (true) { + if (bp->move_state != MOVE_OFF || fp_ZERO(bp->exit_velocity)) break; + bp->replannable = true; + + bp = mp_get_next_buffer(bp); + if (bp == bf) break; // Avoid wrap around + } + + // Plan blocks + mp_plan_block_list(bp); +} + + /*** 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 e6ae33d..4f91da2 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -111,6 +111,7 @@ typedef struct mpMoveRuntimeSingleton { // persistent runtime variables float segment_velocity; // computed velocity for aline segment float segment_time; // actual time increment per aline segment float forward_diff[5]; // forward difference levels + bool hold_planned; // true when a feedhold has been planned MoveState_t ms; } mpMoveRuntimeSingleton_t; @@ -128,10 +129,10 @@ float mp_get_runtime_velocity(); float mp_get_runtime_work_position(uint8_t axis); float mp_get_runtime_absolute_position(uint8_t axis); void mp_set_runtime_work_offset(float offset[]); -void mp_zero_segment_velocity(); uint8_t mp_get_runtime_busy(); void mp_kinematics(const float travel[], float steps[]); -void mp_plan_block_list(mpBuf_t *bf, bool mr_flag); +void mp_plan_block_list(mpBuf_t *bf); +void mp_replan_blocks(); float mp_get_target_length(const float Vi, const float Vf, const mpBuf_t *bf); float mp_get_target_velocity(const float Vi, const float L, const mpBuf_t *bf); inline int32_t mp_get_line() {return mr.ms.line;} diff --git a/src/plan/state.c b/src/plan/state.c index 0acb438..d6963b6 100644 --- a/src/plan/state.c +++ b/src/plan/state.c @@ -32,7 +32,6 @@ #include "planner.h" #include "report.h" #include "buffer.h" -#include "feedhold.h" #include @@ -40,7 +39,6 @@ typedef struct { plannerState_t state; plannerCycle_t cycle; - holdState_t hold; bool hold_requested; bool flush_requested; @@ -53,7 +51,6 @@ static planner_state_t ps = {0}; plannerState_t mp_get_state() {return ps.state;} plannerCycle_t mp_get_cycle() {return ps.cycle;} -holdState_t mp_get_hold_state() {return ps.hold;} PGM_P mp_get_state_pgmstr(plannerState_t state) { @@ -113,7 +110,7 @@ void mp_set_cycle(plannerCycle_t cycle) { } -void mp_set_hold_state(holdState_t hold) {ps.hold = hold;} +void mp_state_holding() {mp_set_state(STATE_HOLDING);} void mp_state_running() { @@ -122,35 +119,13 @@ void mp_state_running() { void mp_state_idle() { - mp_set_state(STATE_READY); - mp_set_hold_state(FEEDHOLD_OFF); // if in feedhold, end it - ps.start_requested = false; // cancel any pending start request - mp_zero_segment_velocity(); // for reporting purposes + if (mp_get_state() == STATE_RUNNING) mp_set_state(STATE_READY); } void mp_state_estop() {mp_set_state(STATE_ESTOPPED);} -/// Called by the planner to update feedhold state in sync with planning -void mp_state_hold_callback(bool done) { - switch (mp_get_hold_state()) { - case FEEDHOLD_OFF: case FEEDHOLD_PLAN: case FEEDHOLD_HOLD: break; - - // Catch the feedhold request and start planning the hold - case FEEDHOLD_SYNC: mp_set_hold_state(FEEDHOLD_PLAN); break; - - case FEEDHOLD_DECEL: - // Look for the end of the decel to go into HOLD state - if (done) { - mp_set_hold_state(FEEDHOLD_HOLD); - mp_set_state(STATE_HOLDING); - } - break; - } -} - - void mp_request_hold() {ps.hold_requested = true;} void mp_request_flush() {ps.flush_requested = true;} void mp_request_start() {ps.start_requested = true;} @@ -181,10 +156,7 @@ void mp_state_callback() { if (ps.hold_requested) { ps.hold_requested = false; - if (mp_get_state() == STATE_RUNNING) { - mp_set_state(STATE_STOPPING); - mp_set_hold_state(FEEDHOLD_SYNC); // invokes hold from aline execution - } + if (mp_get_state() == STATE_RUNNING) mp_set_state(STATE_STOPPING); } // Only flush queue when we are stopped or holding @@ -205,13 +177,12 @@ void mp_state_callback() { ps.start_requested = false; if (mp_get_state() == STATE_HOLDING) { - mp_set_hold_state(FEEDHOLD_OFF); - // Check if any moves are buffered - if (mp_get_run_buffer()) mp_set_state(STATE_RUNNING); - else mp_set_state(STATE_READY); + if (mp_get_run_buffer()) { + mp_replan_blocks(); + mp_set_state(STATE_RUNNING); + + } else mp_set_state(STATE_READY); } } - - mp_plan_hold_callback(); // call feedhold planner } diff --git a/src/plan/state.h b/src/plan/state.h index 0ea36bf..b05358e 100644 --- a/src/plan/state.h +++ b/src/plan/state.h @@ -52,32 +52,20 @@ typedef enum { } plannerCycle_t; -typedef enum { // feedhold state machine - FEEDHOLD_OFF, // no feedhold in effect - FEEDHOLD_SYNC, // start hold - sync to latest aline segment - FEEDHOLD_PLAN, // replan blocks for feedhold - FEEDHOLD_DECEL, // decelerate to hold point - FEEDHOLD_HOLD, // holding -} holdState_t; - - plannerState_t mp_get_state(); plannerCycle_t mp_get_cycle(); -holdState_t mp_get_hold_state(); void mp_set_state(plannerState_t state); void mp_set_cycle(plannerCycle_t cycle); -void mp_set_hold_state(holdState_t hold); PGM_P mp_get_state_pgmstr(plannerState_t state); PGM_P mp_get_cycle_pgmstr(plannerCycle_t cycle); +void mp_state_holding(); void mp_state_running(); void mp_state_idle(); void mp_state_estop(); -void mp_state_hold_callback(bool done); - void mp_request_hold(); void mp_request_flush(); void mp_request_start(); -- 2.27.0