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();}
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;
}
}
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() ||
status_help();
return 0;
}
+
+
+uint8_t command_resume(int argc, char *argv[]) {
+ mp_request_resume();
+ return 0;
+}
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")
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");
}
}
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);
}
-/// 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};
I2C_OPTIONAL_PAUSE,
I2C_RUN,
I2C_STEP,
+ I2C_FLUSH,
I2C_REPORT,
I2C_HOME,
I2C_REBOOT,
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]);
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
}
+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() {
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
}
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();
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];
* 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
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;
* 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:
* 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
* 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;
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();
}
/// 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
#include "planner.h"
#include "buffer.h"
-#include "arc.h"
#include "machine.h"
#include "stepper.h"
#include "motor.h"
}
-/*** 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();
}
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)
#include "state.h"
#include "machine.h"
#include "planner.h"
-#include "report.h"
#include "buffer.h"
+#include "arc.h"
+
+#include "report.h"
#include <stdbool.h>
bool hold_requested;
bool flush_requested;
bool start_requested;
+ bool resume_requested;
} planner_state_t;
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");
}
-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;
}
-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
*
* 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);
}
}
}
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();
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);
projects / bbctrl-firmware / commitdiff