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();
}
* 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
+ }
}
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();
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;
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);
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;
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);
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;
* 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;
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
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;
}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "feedhold.h"
-
-#include "planner.h"
-#include "buffer.h"
-#include "line.h"
-#include "util.h"
-#include "state.h"
-
-#include <stdbool.h>
-#include <math.h>
-
-
-/* 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
-}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-
-void mp_plan_hold_callback();
// 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);
}
-/// 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;}
* 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.
// 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,
}
+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
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;
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;}
#include "planner.h"
#include "report.h"
#include "buffer.h"
-#include "feedhold.h"
#include <stdbool.h>
typedef struct {
plannerState_t state;
plannerCycle_t cycle;
- holdState_t hold;
bool hold_requested;
bool flush_requested;
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) {
}
-void mp_set_hold_state(holdState_t hold) {ps.hold = hold;}
+void mp_state_holding() {mp_set_state(STATE_HOLDING);}
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;}
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
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
}
} 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();
projects / bbctrl-firmware / commitdiff