From: Joseph Coffland Date: Fri, 2 Sep 2016 09:07:20 +0000 (-0700) Subject: Correctly implemented start/stop/pause and queue flushing X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=bbed2a7414466f83ae8a16d0d0e8e281478a0fa5;p=bbctrl-firmware Correctly implemented start/stop/pause and queue flushing --- diff --git a/src/command.c b/src/command.c index 841b78e..81ddbc3 100644 --- a/src/command.c +++ b/src/command.c @@ -61,6 +61,7 @@ static void _pause() {mp_request_hold();} static void _opt_pause() {} // TODO static void _run() {mp_request_start();} static void _step() {} // TODO +static void _flush() {mp_request_flush();} static void _report() {report_request_full();} static void _home() {} // TODO static void _reboot() {hw_request_hard_reset();} @@ -68,16 +69,17 @@ static void _reboot() {hw_request_hard_reset();} static void command_i2c_cb(i2c_cmd_t cmd, uint8_t *data, uint8_t length) { switch (cmd) { + case I2C_NULL: break; case I2C_ESTOP: _estop(); break; case I2C_CLEAR: _clear(); break; case I2C_PAUSE: _pause(); break; case I2C_OPTIONAL_PAUSE: _opt_pause(); break; case I2C_RUN: _run(); break; case I2C_STEP: _step(); break; + case I2C_FLUSH: _flush(); break; case I2C_REPORT: _report(); break; case I2C_HOME: _home(); break; case I2C_REBOOT: _reboot(); break; - default: break; } } @@ -217,9 +219,10 @@ void command_callback() { case '$': status = command_parser(_cmd); break; default: - if (estop_triggered()) status = STAT_MACHINE_ALARMED; - + if (estop_triggered()) {status = STAT_MACHINE_ALARMED; break;} + else if (mp_is_flushing()) break; // Flush GCode command else if (!mp_get_planner_buffer_room() || + mp_is_resuming() || mach_arc_active() || mach_is_homing() || mach_is_probing() || @@ -316,3 +319,9 @@ uint8_t command_messages(int argc, char *argv[]) { status_help(); return 0; } + + +uint8_t command_resume(int argc, char *argv[]) { + mp_request_resume(); + return 0; +} diff --git a/src/command.def b/src/command.def index 03f5dca..2198f44 100644 --- a/src/command.def +++ b/src/command.def @@ -37,3 +37,4 @@ CMD(jog, 1, 4, "Jog") CMD(mreset, 0, 1, "Reset motor") CMD(calibrate, 0, 0, "Calibrate motors") CMD(messages, 0, 0, "Dump all possible status messages") +CMD(resume, 0, 0, "Resume processing after a flush") diff --git a/src/estop.c b/src/estop.c index faf77d8..1d23c9e 100644 --- a/src/estop.c +++ b/src/estop.c @@ -143,11 +143,11 @@ void set_estop(bool value) { PGM_P get_estop_reason() { switch (_get_reason()) { - case ESTOP_NONE: return PSTR("NONE"); - case ESTOP_USER: return PSTR("USER"); - case ESTOP_SWITCH: return PSTR("SWITCH"); - case ESTOP_LIMIT: return PSTR("LIMIT"); - case ESTOP_ALARM: return PSTR("ALARM"); - default: return PSTR("INVALID"); + case ESTOP_NONE: return PSTR("none"); + case ESTOP_USER: return PSTR("user"); + case ESTOP_SWITCH: return PSTR("switch"); + case ESTOP_LIMIT: return PSTR("limit"); + case ESTOP_ALARM: return PSTR("alarm"); + default: return PSTR("invalid"); } } diff --git a/src/homing.c b/src/homing.c index ff02ae9..5c26baf 100644 --- a/src/homing.c +++ b/src/homing.c @@ -182,7 +182,7 @@ static int8_t _get_next_axis(int8_t axis) { static void _homing_finalize_exit() { mp_flush_planner(); // should be stopped, but in case of switch closure - // restore to work coordinate system + // Restore saved machine state mach_set_coord_system(hm.saved_coord_system); mach_set_units_mode(hm.saved_units_mode); mach_set_distance_mode(hm.saved_distance_mode); @@ -199,7 +199,7 @@ static void _homing_error_exit(stat_t status) { } -/// helper that actually executes the above moves +/// Execute moves static void _homing_axis_move(int8_t axis, float target, float velocity) { float vect[AXES] = {0}; float flags[AXES] = {0}; diff --git a/src/i2c.h b/src/i2c.h index 4a833f0..9287249 100644 --- a/src/i2c.h +++ b/src/i2c.h @@ -41,6 +41,7 @@ typedef enum { I2C_OPTIONAL_PAUSE, I2C_RUN, I2C_STEP, + I2C_FLUSH, I2C_REPORT, I2C_HOME, I2C_REBOOT, diff --git a/src/machine.c b/src/machine.c index 18a8519..6aeacfb 100644 --- a/src/machine.c +++ b/src/machine.c @@ -535,8 +535,8 @@ void mach_set_model_target(float target[], float flag[]) { if (fp_FALSE(flag[axis]) || mach.a[axis].axis_mode == AXIS_DISABLED) continue; // skip axis if not flagged for update or its disabled - else if (mach.a[axis].axis_mode == AXIS_STANDARD || - mach.a[axis].axis_mode == AXIS_INHIBITED) { + if (mach.a[axis].axis_mode == AXIS_STANDARD || + mach.a[axis].axis_mode == AXIS_INHIBITED) { if (mach.gm.distance_mode == ABSOLUTE_MODE) mach.ms.target[axis] = mach_get_active_coord_offset(axis) + TO_MILLIMETERS(target[axis]); diff --git a/src/plan/buffer.c b/src/plan/buffer.c index 4a0fc4d..0028e4c 100644 --- a/src/plan/buffer.c +++ b/src/plan/buffer.c @@ -89,9 +89,7 @@ void mp_init_buffers() { memset(&mb, 0, sizeof(mb)); // clear all values, pointers and status - mb.w = &mb.bf[0]; // init write and read buffer pointers - mb.q = &mb.bf[0]; - mb.r = &mb.bf[0]; + mb.w = mb.q = mb.r = &mb.bf[0]; // init write and read buffer pointers pv = &mb.bf[PLANNER_BUFFER_POOL_SIZE - 1]; // setup ring pointers @@ -107,6 +105,9 @@ void mp_init_buffers() { } +bool mp_queue_empty() {return mb.w == mb.r;} + + /// Get pointer to next available write buffer /// Returns pointer or 0 if no buffer available. mpBuf_t *mp_get_write_buffer() { @@ -176,7 +177,7 @@ void mp_free_run_buffer() { // EMPTY current run buf & adv to next mb.buffers_available++; report_request(); - if (mb.w == mb.r) mp_state_idle(); // if queue empty + if (mp_queue_empty()) mp_state_idle(); // if queue empty } diff --git a/src/plan/buffer.h b/src/plan/buffer.h index 4bbf33c..0e317ee 100644 --- a/src/plan/buffer.h +++ b/src/plan/buffer.h @@ -104,6 +104,7 @@ typedef struct mpBuffer { // See Planning Velocity Notes uint8_t mp_get_planner_buffer_room(); void mp_wait_for_buffer(); void mp_init_buffers(); +bool mp_queue_empty(); mpBuf_t *mp_get_write_buffer(); void mp_commit_write_buffer(uint32_t line, moveType_t type); mpBuf_t *mp_get_run_buffer(); diff --git a/src/plan/exec.c b/src/plan/exec.c index 9736c05..9a424d8 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -350,6 +350,7 @@ 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]; @@ -368,7 +369,7 @@ static float _compute_next_segment_velocity() { * this point the remaining buffers, if any, are replanned from zero up to * speed. */ -void _plan_hold() { +static void _plan_hold() { mpBuf_t *bp = mp_get_run_buffer(); // working buffer pointer if (!bp) return; // Oops! nothing's running @@ -449,7 +450,8 @@ static stat_t _exec_aline_init(mpBuf_t *bf) { mr.exit_velocity = bf->exit_velocity; mr.jerk = bf->jerk; - // Generate waypoints for position correction at section ends + // Generate waypoints for position correction at section ends. This helps + // negate floating point errors in the forward differencing code. for (int axis = 0; axis < AXES; axis++) { mr.waypoint[SECTION_HEAD][axis] = mr.position[axis] + mr.unit[axis] * mr.head_length; @@ -480,7 +482,7 @@ static stat_t _exec_aline_init(mpBuf_t *bf) { * Each call to _exec_aline() must execute and prep *one and only one* * segment. If the segment is not the last segment in the bf buffer the * _aline() returns STAT_EAGAIN. If it's the last segment it returns - * STAT_OK. If it encounters a fatal error that would terminate the move it + * STAT_OK. If it encounters a fatal error that would terminate the move it * returns a valid error code. * * Notes: @@ -500,10 +502,10 @@ static stat_t _exec_aline_init(mpBuf_t *bf) { * http://www.et.byu.edu/~ered/ME537/Notes/Ch5.pdf * http://www.scribd.com/doc/63521608/Ed-Red-Ch5-537-Jerk-Equations * - * A full trapezoid is divided into 5 periods. Periods 1 and 2 are the + * A full trapezoid is divided into 5 periods. Periods 1 and 2 are the * first and second halves of the acceleration ramp (the concave and convex - * parts of the S curve in the "head"). Periods 3 and 4 are the first - * and second parts of the deceleration ramp (the tail). There is also + * parts of the S curve in the "head"). Periods 3 and 4 are the first + * and second parts of the deceleration ramp (the tail). There is also * a period for the constant-velocity plateau of the trapezoid (the body). * There are many possible degenerate trapezoids where any of the 5 periods * may be zero length but note that either none or both of a ramping pair can @@ -529,9 +531,7 @@ 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) { - // Stop here if no more moves or holding - if (bf->move_state == MOVE_OFF || mp_get_state() == STATE_HOLDING) - return STAT_NOOP; + if (bf->move_state == MOVE_OFF) return STAT_NOOP; // No more moves stat_t status = STAT_OK; @@ -561,7 +561,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) { 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 + // Note, _plan_hold() 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(); } @@ -579,6 +579,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) { /// Dequeues buffer and executes move callback stat_t mp_exec_move() { if (mp_get_state() == STATE_ESTOPPED) return STAT_MACHINE_ALARMED; + if (mp_get_state() == STATE_HOLDING) return STAT_NOOP; mpBuf_t *bf = mp_get_run_buffer(); if (!bf) return STAT_NOOP; // nothing running diff --git a/src/plan/planner.c b/src/plan/planner.c index 1086e1f..4a20248 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -59,7 +59,6 @@ #include "planner.h" #include "buffer.h" -#include "arc.h" #include "machine.h" #include "stepper.h" #include "motor.h" @@ -78,15 +77,14 @@ void planner_init() { } -/*** Flush all moves in the planner and all arcs +/*** Flush all moves in the planner * * Does not affect the move currently running in mr. Does not affect * mm or gm model positions. This function is designed to be called - * during a hold to reset the planner. This function should not - * generally be called; call mach_queue_flush() instead. + * during a hold to reset the planner. This function should not usually + * be directly called. Call mp_request_flush() instead. */ void mp_flush_planner() { - mach_abort_arc(); mp_init_buffers(); } @@ -761,8 +759,8 @@ float mp_get_target_velocity(const float Vi, const float L, const mpBuf_t *bf) { float x = pow(L, 0.66666666) * bf->cbrt_jerk + Vi; // First estimate #if (GET_VELOCITY_ITERATIONS > 0) - float L2 = L * L; - float Vi2 = Vi * Vi; + const float L2 = L * L; + const float Vi2 = Vi * Vi; for (int i = 0; i < GET_VELOCITY_ITERATIONS; i++) // x' = x - Z(x) / J'(x) diff --git a/src/plan/state.c b/src/plan/state.c index d6963b6..580899d 100644 --- a/src/plan/state.c +++ b/src/plan/state.c @@ -30,8 +30,10 @@ #include "state.h" #include "machine.h" #include "planner.h" -#include "report.h" #include "buffer.h" +#include "arc.h" + +#include "report.h" #include @@ -43,6 +45,7 @@ typedef struct { bool hold_requested; bool flush_requested; bool start_requested; + bool resume_requested; } planner_state_t; @@ -58,8 +61,8 @@ PGM_P mp_get_state_pgmstr(plannerState_t state) { case STATE_READY: return PSTR("ready"); case STATE_ESTOPPED: return PSTR("estopped"); case STATE_RUNNING: return PSTR("running"); - case STATE_STOPPING: return PSTR("stopping"); - case STATE_HOLDING: return PSTR("holding"); + case STATE_STOPPING: return PSTR("stopping"); + case STATE_HOLDING: return PSTR("holding"); } return PSTR("invalid"); @@ -79,7 +82,7 @@ PGM_P mp_get_cycle_pgmstr(plannerCycle_t cycle) { } -void mp_set_state(plannerState_t state) { +static void _set_state(plannerState_t state) { if (ps.state == state) return; // No change if (ps.state == STATE_ESTOPPED) return; // Can't leave EStop state ps.state = state; @@ -110,29 +113,34 @@ void mp_set_cycle(plannerCycle_t cycle) { } -void mp_state_holding() {mp_set_state(STATE_HOLDING);} +bool mp_is_flushing() {return ps.flush_requested && !ps.resume_requested;} +bool mp_is_resuming() {return ps.resume_requested;} + + +void mp_state_holding() {_set_state(STATE_HOLDING);} void mp_state_running() { - if (mp_get_state() == STATE_READY) mp_set_state(STATE_RUNNING); + if (mp_get_state() == STATE_READY) _set_state(STATE_RUNNING); } void mp_state_idle() { - if (mp_get_state() == STATE_RUNNING) mp_set_state(STATE_READY); + if (mp_get_state() == STATE_RUNNING) _set_state(STATE_READY); } -void mp_state_estop() {mp_set_state(STATE_ESTOPPED);} +void mp_state_estop() {_set_state(STATE_ESTOPPED);} void mp_request_hold() {ps.hold_requested = true;} -void mp_request_flush() {ps.flush_requested = true;} void mp_request_start() {ps.start_requested = true;} +void mp_request_flush() {ps.flush_requested = true;} +void mp_request_resume() {if (ps.flush_requested) ps.resume_requested = true;} -/*** Feedholds, queue flushes and starts are all related. The request functions - * set flags. The callback interprets the flags according to these rules: +/*** Feedholds, queue flushes and starts are all related. Request functions + * set flags. The callback interprets the flags according to these rules: * * A hold request received: * - during motion is honored @@ -141,48 +149,60 @@ void mp_request_start() {ps.start_requested = true;} * * A flush request received: * - during motion is ignored but not reset - * - during a feedhold is deferred until the feedhold enters HOLD state. + * - during a feedhold is deferred until the feedhold enters HOLDING state. * I.e. until deceleration is complete. * - when stopped or holding and the planner is not busy, is honored * * A start request received: * - during motion is ignored and reset - * - during a feedhold is deferred until the feedhold enters a HOLD state. + * - during a feedhold is deferred until the feedhold enters HOLDING state. * I.e. until deceleration is complete. If a queue flush request is also * present the queue flush is done first * - when stopped is honored and starts to run anything in the planner queue */ void mp_state_callback() { - if (ps.hold_requested) { + if (ps.hold_requested || ps.flush_requested) { ps.hold_requested = false; - if (mp_get_state() == STATE_RUNNING) mp_set_state(STATE_STOPPING); + if (mp_get_state() == STATE_RUNNING) _set_state(STATE_STOPPING); } - // Only flush queue when we are stopped or holding + // Only flush queue when idle or holding. if (ps.flush_requested && (mp_get_state() == STATE_READY || mp_get_state() == STATE_HOLDING) && !mp_get_runtime_busy()) { - ps.flush_requested = false; - mp_flush_planner(); + mach_abort_arc(); + + if (!mp_queue_empty()) { + mp_flush_planner(); - // NOTE: The following uses low-level mp calls for absolute position - for (int axis = 0; axis < AXES; axis++) - mach_set_position(axis, mp_get_runtime_absolute_position(axis)); + // NOTE The following uses low-level mp calls for absolute position. + // Reset to actual machine position. Otherwise machine is set to the + // position of the last queued move. + for (int axis = 0; axis < AXES; axis++) + mach_set_position(axis, mp_get_runtime_absolute_position(axis)); + } + + // Resume + if (ps.resume_requested) { + ps.flush_requested = ps.resume_requested = false; + _set_state(STATE_READY); + } } - // Don't start while stopping - if (ps.start_requested && mp_get_state() != STATE_STOPPING) { + // Don't start while flushing or stopping + if (ps.start_requested && !ps.flush_requested && + mp_get_state() != STATE_STOPPING) { ps.start_requested = false; if (mp_get_state() == STATE_HOLDING) { // Check if any moves are buffered if (mp_get_run_buffer()) { mp_replan_blocks(); - mp_set_state(STATE_RUNNING); + _set_state(STATE_RUNNING); - } else mp_set_state(STATE_READY); + } else _set_state(STATE_READY); } } } diff --git a/src/plan/state.h b/src/plan/state.h index b05358e..f5e8cf6 100644 --- a/src/plan/state.h +++ b/src/plan/state.h @@ -55,19 +55,22 @@ typedef enum { plannerState_t mp_get_state(); plannerCycle_t mp_get_cycle(); -void mp_set_state(plannerState_t state); void mp_set_cycle(plannerCycle_t cycle); PGM_P mp_get_state_pgmstr(plannerState_t state); PGM_P mp_get_cycle_pgmstr(plannerCycle_t cycle); +bool mp_is_flushing(); +bool mp_is_resuming(); + void mp_state_holding(); void mp_state_running(); void mp_state_idle(); void mp_state_estop(); void mp_request_hold(); -void mp_request_flush(); void mp_request_start(); +void mp_request_flush(); +void mp_request_resume(); void mp_state_callback(); diff --git a/src/usart.c b/src/usart.c index f5340f7..7d66f63 100644 --- a/src/usart.c +++ b/src/usart.c @@ -199,8 +199,7 @@ char *usart_readline() { bool eol = false; while (!rx_buf_empty()) { - char data = rx_buf_peek(); - rx_buf_pop(); + char data = usart_getc(); if (usart_flags & USART_ECHO) usart_putc(data);