From: Joseph Coffland Date: Sun, 28 Aug 2016 01:56:19 +0000 (-0700) Subject: Isolated planner state from machine X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=568dc8f1e08e9e979771378cdbdb894702c80d13;p=bbctrl-firmware Isolated planner state from machine --- diff --git a/src/command.c b/src/command.c index 189b340..21c4679 100644 --- a/src/command.c +++ b/src/command.c @@ -40,6 +40,7 @@ #include "plan/calibrate.h" #include "plan/buffer.h" #include "plan/arc.h" +#include "plan/state.h" #include "config.h" #include @@ -56,8 +57,8 @@ static char *_cmd = 0; static void _estop() {estop_trigger(ESTOP_USER);} static void _clear() {estop_clear();} -static void _pause() {mach_request_feedhold();} -static void _run() {mach_request_cycle_start();} +static void _pause() {mp_request_hold();} +static void _run() {mp_request_start();} static void _step() {} // TODO static void _report() {report_request_full();} static void _reboot() {hw_request_hard_reset();} diff --git a/src/estop.c b/src/estop.c index e9edd66..faf77d8 100644 --- a/src/estop.c +++ b/src/estop.c @@ -36,6 +36,7 @@ #include "config.h" #include "plan/planner.h" +#include "plan/state.h" #include @@ -73,7 +74,7 @@ void estop_init() { switch_set_callback(SW_ESTOP, _switch_callback); - if (estop.triggered) mach_set_state(STATE_ESTOPPED); + if (estop.triggered) mp_state_estop(); // Fault signal if (estop.triggered) OUTSET_PIN(FAULT_PIN); // High @@ -95,7 +96,7 @@ void estop_trigger(estop_reason_t reason) { mach_spindle_estop(); // Set machine state - mach_set_state(STATE_ESTOPPED); + mp_state_estop(); // Set axes not homed mach_set_not_homed(); diff --git a/src/homing.c b/src/homing.c index c1d517d..809d045 100644 --- a/src/homing.c +++ b/src/homing.c @@ -32,6 +32,7 @@ #include "report.h" #include "plan/planner.h" +#include "plan/state.h" #include @@ -176,8 +177,7 @@ static void _homing_finalize_exit() { mach_set_feed_rate_mode(hm.saved_feed_rate_mode); mach.gm.feed_rate = hm.saved_feed_rate; mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); - mach_cycle_end(); - mach_set_cycle(CYCLE_MACHINING); + mp_set_cycle(CYCLE_MACHINING); } @@ -195,8 +195,7 @@ static void _homing_axis_move(int8_t axis, float target, float velocity) { vect[axis] = target; flags[axis] = true; mach.gm.feed_rate = velocity; - mp_flush_planner(); // don't use mach_request_queue_flush() here - mach_request_cycle_start(); + mp_flush_planner(); // don't use mp_request_flush() here stat_t status = mach_straight_feed(vect, flags); if (status) _homing_error_exit(status); @@ -343,7 +342,7 @@ static void _homing_axis_start(int8_t axis) { bool mach_is_homing() { - return mach_get_cycle() == CYCLE_HOMING; + return mp_get_cycle() == CYCLE_HOMING; } @@ -372,7 +371,7 @@ void mach_homing_cycle_start() { hm.axis = -1; // set to retrieve initial axis hm.func = _homing_axis_start; // bind initial processing function - mach_set_cycle(CYCLE_HOMING); + mp_set_cycle(CYCLE_HOMING); mach.homing_state = HOMING_NOT_HOMED; } @@ -385,6 +384,6 @@ void mach_homing_cycle_start_no_set() { /// Main loop callback for running the homing cycle void mach_homing_callback() { - if (mach_get_cycle() != CYCLE_HOMING || mach_get_runtime_busy()) return; + if (mp_get_cycle() != CYCLE_HOMING || mp_get_runtime_busy()) return; hm.func(hm.axis); // execute the current homing move } diff --git a/src/machine.c b/src/machine.c index 462d60c..5c5c95b 100644 --- a/src/machine.c +++ b/src/machine.c @@ -182,9 +182,6 @@ machine_t mach = { } }, - ._state = STATE_READY, - ._cycle = CYCLE_MACHINING, - // State .gm = {.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE}, .gn = {0}, @@ -203,34 +200,6 @@ static void _exec_absolute_origin(float *value, float *flag); // Machine State functions uint32_t mach_get_line() {return mach.gm.line;} - -PGM_P mach_get_state_pgmstr(machState_t state) { - switch (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"); - } - - return PSTR("invalid"); -} - - -PGM_P mach_get_cycle_pgmstr(machCycle_t cycle) { - switch (cycle) { - case CYCLE_MACHINING: return PSTR("machining"); - case CYCLE_HOMING: return PSTR("homing"); - case CYCLE_PROBING: return PSTR("probing"); - case CYCLE_CALIBRATING: return PSTR("calibrating"); - case CYCLE_JOGGING: return PSTR("jogging"); - } - - return PSTR("invalid"); -} - - -machFeedholdState_t mach_get_hold_state() {return mach.hold_state;} machHomingState_t mach_get_homing_state() {return mach.homing_state;} machMotionMode_t mach_get_motion_mode() {return mach.gm.motion_mode;} machCoordSystem_t mach_get_coord_system() {return mach.gm.coord_system;} @@ -241,41 +210,9 @@ machDistanceMode_t mach_get_distance_mode() {return mach.gm.distance_mode;} machFeedRateMode_t mach_get_feed_rate_mode() {return mach.gm.feed_rate_mode;} uint8_t mach_get_tool() {return mach.gm.tool;} machSpindleMode_t mach_get_spindle_mode() {return mach.gm.spindle_mode;} -bool mach_get_runtime_busy() {return mp_get_runtime_busy();} float mach_get_feed_rate() {return mach.gm.feed_rate;} -void mach_set_state(machState_t state) { - if (mach._state == state) return; // No change - if (mach._state == STATE_ESTOPPED) return; // Can't leave EStop state - mach._state = state; - report_request(); -} - - -void mach_set_cycle(machCycle_t cycle) { - if (mach._cycle == cycle) return; // No change - - if (mach._state != STATE_READY) { - STATUS_ERROR(STAT_INTERNAL_ERROR, "Cannot transition to %S while %S", - mach_get_cycle_pgmstr(cycle), - mach_get_state_pgmstr(mach._state)); - return; - } - - if (mach._cycle != CYCLE_MACHINING && cycle != CYCLE_MACHINING) { - STATUS_ERROR(STAT_INTERNAL_ERROR, - "Cannot transition to cycle %S while in %S", - mach_get_cycle_pgmstr(cycle), - mach_get_cycle_pgmstr(mach._cycle)); - return; - } - - mach._cycle = cycle; - report_request(); -} - - void mach_set_motion_mode(machMotionMode_t motion_mode) { mach.gm.motion_mode = motion_mode; } @@ -440,7 +377,7 @@ float mach_get_work_position(uint8_t axis) { void mach_finalize_move() { copy_vector(mach.position, mach.ms.target); // update model position - // if in ivnerse time mode reset feed rate so next block requires an + // if in inverse time mode reset feed rate so next block requires an // explicit feed rate setting if (mach.gm.feed_rate_mode == INVERSE_TIME_MODE && mach.gm.motion_mode == MOTION_MODE_STRAIGHT_FEED) @@ -613,7 +550,7 @@ void mach_set_model_target(float target[], float flag[]) { } } - // FYI: The ABC loop below relies on the XYZ loop having been run first + // NOTE: The ABC loop below relies on the XYZ loop having been run first for (int axis = AXIS_A; axis <= AXIS_C; axis++) { if (fp_FALSE(flag[axis]) || mach.a[axis].axis_mode == AXIS_DISABLED) continue; // skip axis if not flagged for update or its disabled @@ -776,7 +713,7 @@ static void _exec_offset(float *value, float *flag) { * only be called during initialization sequences and during cycles * (such as homing cycles) when you know there are no more moves in * the planner and that all motion has stopped. Use - * mach_get_runtime_busy() to be sure the system is quiescent. + * mp_get_runtime_busy() to be sure the system is quiescent. */ void mach_set_position(int axis, float position) { // TODO: Interlock involving runtime_busy test @@ -889,7 +826,6 @@ stat_t mach_straight_traverse(float target[], float flags[]) { // prep and plan the move mach_set_work_offsets(&mach.gm); // capture fully resolved offsets to state - mach_cycle_start(); // required for homing & other cycles mach.ms.line = mach.gm.line; // copy line number mp_aline(&mach.ms); // send the move to the planner mach_finalize_move(); @@ -986,7 +922,6 @@ stat_t mach_straight_feed(float target[], float flags[]) { // prep and plan the move mach_set_work_offsets(&mach.gm); // capture fully resolved offsets to state - mach_cycle_start(); // required for homing & other cycles mach.ms.line = mach.gm.line; // copy line number status = mp_aline(&mach.ms); // send the move to the planner mach_finalize_move(); @@ -1133,76 +1068,6 @@ void mach_message(const char *message) { * mach_program_end is a stop that also resets the machine to initial state */ -/* Feedholds, queue flushes and cycles starts are all related. The request - * functions set flags for these. The sequencing callback interprets the flags - * according to the following rules: - * - * Feedhold request received during motion is honored - * Feedhold request received during a feedhold is ignored and reset - * Feedhold request received during a motion stop is ignored and reset - * - * Queue flush request received during motion is ignored but not reset - * Queue flush request received during a feedhold is deferred until - * the feedhold enters a HOLD state (i.e. until deceleration is complete) - * Queue flush request received during a motion stop is honored - * - * Cycle start request received during motion is ignored and reset - * Cycle start request received during a feedhold is deferred until - * the feedhold enters a HOLD state (i.e. until deceleration is complete) - * If a queue flush request is also present the queue flush is done first - * Cycle start request received during a motion stop is honored and starts - * to run anything in the planner queue - */ - - -void mach_request_feedhold() {mach.feedhold_requested = true;} -void mach_request_queue_flush() {mach.queue_flush_requested = true;} -void mach_request_cycle_start() {mach.cycle_start_requested = true;} - - -/// Process feedholds, cycle starts & queue flushes -void mach_feedhold_callback() { - if (mach.feedhold_requested) { - mach.feedhold_requested = false; - - if (mach_get_state() == STATE_RUNNING) { - mach_set_state(STATE_STOPPING); - mach.hold_state = FEEDHOLD_SYNC; // invokes hold from aline execution - } - } - - // Only flush queue when we are stopped or holding - if (mach.queue_flush_requested && - (mach_get_state() == STATE_READY || mach_get_state() == STATE_HOLDING) && - !mach_get_runtime_busy()) { - mach.queue_flush_requested = false; - mach_queue_flush(); - } - - // Don't start cycle when stopping - if (mach.cycle_start_requested && mach_get_state() != STATE_STOPPING) { - mach.cycle_start_requested = false; - - if (mach_get_state() == STATE_HOLDING) { - mach.hold_state = FEEDHOLD_OFF; - - // Check if any moves are buffered - if (mp_get_run_buffer()) mach_set_state(STATE_RUNNING); - else mach_set_state(STATE_READY); - } - } - - mp_plan_hold_callback(); -} - - -static void _exec_program_finalize(float *value, float *flag) { - mach_set_state(STATE_READY); - mach.hold_state = FEEDHOLD_OFF; // if in feedhold, end it - mach.cycle_start_requested = false; // cancel any pending cycle start request - mp_zero_segment_velocity(); // for reporting purposes -} - /*** _exec_program_end() implements M2 and M30. End behaviors are defined by * NIST 3.6.1 are: @@ -1234,9 +1099,6 @@ static void _exec_program_finalize(float *value, float *flag) { * + Default INCHES or MM units mode is restored */ static void _exec_program_end(float *value, float *flag) { - _exec_program_finalize(value, flag); - - // Perform resets mach_reset_origin_offsets(); // G92.1 - we do G91.1 instead of G92.2 mach_set_coord_system(GCODE_DEFAULT_COORD_SYSTEM); mach_set_plane(GCODE_DEFAULT_PLANE); @@ -1248,41 +1110,10 @@ static void _exec_program_end(float *value, float *flag) { } -stat_t mach_queue_flush() { - if (mach_get_runtime_busy()) return STAT_COMMAND_NOT_ACCEPTED; - - mp_flush_planner(); // flush planner queue - - // Note: The following uses low-level mp calls for absolute position. - for (int axis = 0; axis < AXES; axis++) - // set mm from mr - mach_set_position(axis, mp_get_runtime_absolute_position(axis)); - - float value[AXES] = {}; - _exec_program_finalize(value, value); // finalize now, not later - - return STAT_OK; -} - - -/// Do a cycle start right now -void mach_cycle_start() { - if (mach_get_state() == STATE_READY) mach_set_state(STATE_RUNNING); -} - - -/// Do a cycle end right now -void mach_cycle_end() { - float value[AXES] = {}; - _exec_program_finalize(value, value); -} - - - -/// M0 Queue a program stop +/// M0 Queue a program stop void mach_program_stop() { - float value[AXES] = {}; - mp_queue_command(_exec_program_finalize, value, value); + // TODO actually stop program + // Need to queue a feedhold event in the planner buffer } @@ -1307,12 +1138,6 @@ void mach_program_end() { } -/// Free run buffer and end cycle if planner is empty -void mach_advance_buffer() { - if (mp_free_run_buffer()) mach_cycle_end(); -} - - /// return ASCII char for axis given the axis number char mach_get_axis_char(int8_t axis) { char axis_char[] = "XYZABC"; diff --git a/src/machine.h b/src/machine.h index 63550d0..b8a3800 100644 --- a/src/machine.h +++ b/src/machine.h @@ -32,8 +32,6 @@ #include "config.h" #include "status.h" -#include - #include #include @@ -44,33 +42,6 @@ -typedef enum { - STATE_READY, - STATE_ESTOPPED, - STATE_RUNNING, - STATE_STOPPING, - STATE_HOLDING, -} machState_t; - - -typedef enum { - CYCLE_MACHINING, - CYCLE_HOMING, - CYCLE_PROBING, - CYCLE_CALIBRATING, - CYCLE_JOGGING, -} machCycle_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 -} machFeedholdState_t; - - typedef enum { // applies to mach.homing_state HOMING_NOT_HOMED, // machine is not homed HOMING_HOMED, // machine is homed @@ -356,19 +327,12 @@ typedef struct { // struct to manage mach globals and cycles // settings for axes X,Y,Z,A B,C AxisConfig_t a[AXES]; - machState_t _state; - machCycle_t _cycle; - machFeedholdState_t hold_state; // hold: feedhold sub-state machine machHomingState_t homing_state; // home: homing cycle sub-state machine bool homed[AXES]; // individual axis homing flags machProbeState_t probe_state; float probe_results[AXES]; // probing results - bool feedhold_requested; - bool queue_flush_requested; - bool cycle_start_requested; - // Model states MoveState_t ms; GCodeState_t gm; // core gcode model state @@ -382,9 +346,6 @@ extern machine_t mach; // machine controller singleton // Model state getters and setters uint32_t mach_get_line(); -inline machState_t mach_get_state() {return mach._state;} -inline machCycle_t mach_get_cycle() {return mach._cycle;} -machFeedholdState_t mach_get_hold_state(); machHomingState_t mach_get_homing_state(); machMotionMode_t mach_get_motion_mode(); machCoordSystem_t mach_get_coord_system(); @@ -396,13 +357,8 @@ machFeedRateMode_t mach_get_feed_rate_mode(); uint8_t mach_get_tool(); machSpindleMode_t mach_get_spindle_mode(); inline float mach_get_spindle_speed() {return mach.gm.spindle_speed;} -bool mach_get_runtime_busy(); float mach_get_feed_rate(); -void mach_set_state(machState_t state); -void mach_set_cycle(machCycle_t cycle); -PGM_P mach_get_state_pgmstr(machState_t state); -PGM_P mach_get_cycle_pgmstr(machCycle_t cycle); void mach_set_motion_mode(machMotionMode_t motion_mode); void mach_set_spindle_mode(machSpindleMode_t spindle_mode); void mach_set_spindle_speed_parameter(float speed); @@ -495,21 +451,9 @@ void mach_spindle_override_factor(bool flag); void mach_message(const char *message); // Program Functions (4.3.10) -void mach_request_feedhold(); -void mach_request_queue_flush(); -void mach_request_cycle_start(); - -void mach_feedhold_callback(); -stat_t mach_queue_flush(); - -void mach_cycle_start(); -void mach_cycle_end(); -void mach_feedhold(); void mach_program_stop(); void mach_optional_program_stop(); void mach_pallet_change_stop(); void mach_program_end(); -void mach_advance_buffer(); - char mach_get_axis_char(int8_t axis); diff --git a/src/main.c b/src/main.c index 4f33b18..eab6b2b 100644 --- a/src/main.c +++ b/src/main.c @@ -46,6 +46,7 @@ #include "plan/planner.h" #include "plan/arc.h" #include "plan/feedhold.h" +#include "plan/state.h" #include #include @@ -83,7 +84,8 @@ int main() { while (true) { hw_reset_handler(); // handle hard reset requests if (!estop_triggered()) { - mach_feedhold_callback(); // feedhold state machine + mp_state_callback(); + mp_plan_hold_callback(); // feedhold state machine mach_arc_callback(); // arc generation runs mach_homing_callback(); // G28.2 continuation mach_probe_callback(); // G38.2 continuation diff --git a/src/plan/arc.c b/src/plan/arc.c index de44ee3..2d8ef61 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -426,8 +426,7 @@ stat_t mach_arc_feed(float target[], float flags[], // arc endpoints } else if (offset_i) return STAT_ARC_SPECIFICATION_ERROR; } - // set values in the Gcode model state & copy it (line was already - // captured) + // set values in Gcode model state & copy it (line was already captured) mach_set_model_target(target, flags); // in radius mode it's an error for start == end @@ -462,8 +461,8 @@ stat_t mach_arc_feed(float target[], float flags[], // arc endpoints // trap zero length arcs that _compute_arc can throw if (fp_ZERO(arc.length)) return STAT_MINIMUM_LENGTH_MOVE; - mach_cycle_start(); // if not already started arc.run_state = MOVE_RUN; // enable arc run from the callback + mach_arc_callback(); // Queue initial arc moves mach_finalize_move(); return STAT_OK; diff --git a/src/plan/buffer.c b/src/plan/buffer.c index e0c914a..c7a097a 100644 --- a/src/plan/buffer.c +++ b/src/plan/buffer.c @@ -50,6 +50,7 @@ */ #include "buffer.h" +#include "state.h" #include "report.h" #include @@ -101,6 +102,8 @@ void mp_init_buffers() { } mb.buffers_available = PLANNER_BUFFER_POOL_SIZE; + + mp_state_idle(); } @@ -116,9 +119,9 @@ mpBuf_t *mp_get_write_buffer() { w->nx = nx; // restore pointers w->pv = pv; w->buffer_state = MP_BUFFER_LOADING; - mb.buffers_available--; mb.w = w->nx; + mb.buffers_available--; report_request(); return w; @@ -136,6 +139,8 @@ mpBuf_t *mp_get_write_buffer() { * invalidating its contents */ void mp_commit_write_buffer(uint32_t line, moveType_t move_type) { + mp_state_running(); + mb.q->ms.line = line; mb.q->move_type = move_type; mb.q->move_state = MOVE_NEW; @@ -164,11 +169,8 @@ mpBuf_t *mp_get_run_buffer() { } -/* Release the run buffer & return to buffer pool. - * Returns true if queue is empty, false otherwise. - * This is useful for doing queue empty / end move functions. - */ -bool mp_free_run_buffer() { // EMPTY current run buf & adv to next +/// Release the run buffer & return to buffer pool. +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 @@ -178,7 +180,7 @@ bool mp_free_run_buffer() { // EMPTY current run buf & adv to next mb.buffers_available++; report_request(); - return mb.w == mb.r; // return true if the queue emptied + if (mb.w == mb.r) mp_state_idle(); // if queue empty } diff --git a/src/plan/buffer.h b/src/plan/buffer.h index 19e729e..8244603 100644 --- a/src/plan/buffer.h +++ b/src/plan/buffer.h @@ -74,7 +74,7 @@ typedef struct mpBuffer { // See Planning Velocity Notes struct mpBuffer *nx; // pointer to next buffer bf_func_t bf_func; // callback to buffer exec function - mach_exec_t mach_func; // callback to machine + mach_exec_t mach_func; // callback to machine float naive_move_time; @@ -116,7 +116,7 @@ void mp_init_buffers(); mpBuf_t *mp_get_write_buffer(); void mp_commit_write_buffer(uint32_t line, moveType_t move_type); mpBuf_t *mp_get_run_buffer(); -bool mp_free_run_buffer(); +void mp_free_run_buffer(); mpBuf_t *mp_get_first_buffer(); mpBuf_t *mp_get_last_buffer(); /// Returns pointer to prev buffer in linked list diff --git a/src/plan/calibrate.c b/src/plan/calibrate.c index d11a2e4..01a9577 100644 --- a/src/plan/calibrate.c +++ b/src/plan/calibrate.c @@ -35,6 +35,7 @@ #include "stepper.h" #include "rtc.h" #include "tmc2660.h" +#include "state.h" #include "config.h" #include @@ -111,7 +112,7 @@ static stat_t _exec_calibrate(mpBuf_t *bf) { tmc2660_set_stallguard_threshold(cal.motor, 63); mp_free_run_buffer(); // Release buffer - mach_set_cycle(CYCLE_MACHINING); + mp_set_cycle(CYCLE_MACHINING); return STAT_OK; } else { @@ -144,7 +145,7 @@ static stat_t _exec_calibrate(mpBuf_t *bf) { } -bool calibrate_busy() {return mach_get_cycle() == CYCLE_CALIBRATING;} +bool calibrate_busy() {return mp_get_cycle() == CYCLE_CALIBRATING;} void calibrate_set_stallguard(int motor, uint16_t sg) { @@ -163,7 +164,7 @@ void calibrate_set_stallguard(int motor, uint16_t sg) { uint8_t command_calibrate(int argc, char *argv[]) { - if (mach_get_cycle() != CYCLE_MACHINING || mach_get_state() != STATE_READY) + if (mp_get_cycle() != CYCLE_MACHINING || mp_get_state() != STATE_READY) return 0; mpBuf_t *bf = mp_get_write_buffer(); @@ -174,7 +175,7 @@ uint8_t command_calibrate(int argc, char *argv[]) { // Start memset(&cal, 0, sizeof(cal)); - mach_set_cycle(CYCLE_CALIBRATING); + mp_set_cycle(CYCLE_CALIBRATING); cal.motor = 1; bf->bf_func = _exec_calibrate; // register callback diff --git a/src/plan/command.c b/src/plan/command.c index dc41ddc..71d2397 100644 --- a/src/plan/command.c +++ b/src/plan/command.c @@ -88,5 +88,5 @@ void mp_runtime_command(mpBuf_t *bf) { bf->mach_func(bf->ms.target, bf->unit); // Free buffer & perform cycle_end if planner is empty - mach_advance_buffer(); + mp_free_run_buffer(); } diff --git a/src/plan/dwell.c b/src/plan/dwell.c index d5be8c0..7c5be81 100644 --- a/src/plan/dwell.c +++ b/src/plan/dwell.c @@ -42,7 +42,7 @@ static stat_t _exec_dwell(mpBuf_t *bf) { st_prep_dwell(bf->ms.move_time); // in seconds // free buffer & perform cycle_end if planner is empty - mach_advance_buffer(); + mp_free_run_buffer(); return STAT_OK; } diff --git a/src/plan/exec.c b/src/plan/exec.c index 6a0f26d..2ebf6e4 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -34,6 +34,7 @@ #include "util.h" #include "report.h" #include "estop.h" +#include "state.h" #include "config.h" #include @@ -70,7 +71,7 @@ static stat_t _exec_aline_segment() { // compute target from segment time and velocity Don't do waypoint // correction if you are going into a hold. if (--mr.segment_count == 0 && mr.section_state == SECTION_2nd_HALF && - mach_get_state() == STATE_RUNNING) + mp_get_state() == STATE_RUNNING) copy_vector(mr.ms.target, mr.waypoint[mr.section]); else { @@ -701,7 +702,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) { // start a new move by setting up local context (singleton) if (mr.move_state == MOVE_OFF) { - if (mach.hold_state == FEEDHOLD_HOLD) + if (mp_get_hold_state() == FEEDHOLD_HOLD) return STAT_NOOP; // stops here if holding // initialization to process the new incoming bf buffer (Gcode block) @@ -717,7 +718,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) { // prevent overplanning (Note 2) bf->nx->replannable = false; // free buffer & end cycle if planner is empty - mach_advance_buffer(); + mp_free_run_buffer(); return STAT_NOOP; } @@ -767,13 +768,12 @@ stat_t mp_exec_aline(mpBuf_t *bf) { // Feedhold processing. Refer to machine.h for state machine // Catch the feedhold request and start the planning the hold - if (mach.hold_state == FEEDHOLD_SYNC) mach.hold_state = FEEDHOLD_PLAN; + if (mp_get_hold_state() == FEEDHOLD_SYNC) mp_set_hold_state(FEEDHOLD_PLAN); // Look for the end of the decel to go into HOLD state - if (mach.hold_state == FEEDHOLD_DECEL && status == STAT_OK) { - mach.hold_state = FEEDHOLD_HOLD; - mach_set_state(STATE_HOLDING); - report_request(); + if (mp_get_hold_state() == FEEDHOLD_DECEL && status == STAT_OK) { + mp_set_hold_state(FEEDHOLD_HOLD); + mp_state_hold(); } // There are 3 things that can happen here depending on return conditions: @@ -789,14 +789,14 @@ stat_t mp_exec_aline(mpBuf_t *bf) { mr.section_state = SECTION_OFF; bf->nx->replannable = false; // prevent overplanning (Note 2) - if (bf->move_state == MOVE_RUN) mach_advance_buffer(); + if (bf->move_state == MOVE_RUN) mp_free_run_buffer(); } return status; } -/// Dequeues buffer, executes move callback and enters RUNNING state +/// Dequeues buffer and executes move callback stat_t mp_exec_move() { if (estop_triggered()) return STAT_MACHINE_ALARMED; @@ -804,7 +804,5 @@ stat_t mp_exec_move() { if (!bf) return STAT_NOOP; // nothing running if (!bf->bf_func) return CM_ALARM(STAT_INTERNAL_ERROR); - if (mach_get_state() == STATE_READY) mach_set_state(STATE_RUNNING); - return bf->bf_func(bf); // move callback } diff --git a/src/plan/feedhold.c b/src/plan/feedhold.c index df0bc29..1ffb4aa 100644 --- a/src/plan/feedhold.c +++ b/src/plan/feedhold.c @@ -34,19 +34,18 @@ #include "line.h" #include "zoid.h" #include "util.h" +#include "state.h" #include #include -/* Feedhold is executed as mach.hold_state transitions executed inside +/* 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 is asserted by calling mach_feedhold() - * * - Hold state == SYNC tells the aline exec routine to execute the - * next aline segment then set hold_state to PLAN. This gives 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. * @@ -112,7 +111,7 @@ static float _compute_next_segment_velocity() { /// replan block list to execute hold void mp_plan_hold_callback() { - if (mach.hold_state != FEEDHOLD_PLAN) return; // not planning a feedhold + 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 @@ -157,7 +156,7 @@ void mp_plan_hold_callback() { _reset_replannable_list(); // make it replan all the blocks mp_plan_block_list(mp_get_last_buffer(), &mr_flag); - mach.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit + mp_set_hold_state(FEEDHOLD_DECEL); // set state to decelerate and exit return; } @@ -211,5 +210,5 @@ void mp_plan_hold_callback() { _reset_replannable_list(); // replan all the blocks mp_plan_block_list(mp_get_last_buffer(), &mr_flag); - mach.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit + mp_set_hold_state(FEEDHOLD_DECEL); // set state to decelerate and exit } diff --git a/src/plan/jog.c b/src/plan/jog.c index 46c5c79..408f8f2 100644 --- a/src/plan/jog.c +++ b/src/plan/jog.c @@ -33,6 +33,7 @@ #include "motor.h" #include "machine.h" #include "motor.h" +#include "state.h" #include "config.h" #include @@ -95,7 +96,7 @@ static stat_t _exec_jog(mpBuf_t *bf) { // Release buffer mp_free_run_buffer(); - mach_set_cycle(CYCLE_MACHINING); + mp_set_cycle(CYCLE_MACHINING); return STAT_NOOP; } @@ -112,12 +113,12 @@ static stat_t _exec_jog(mpBuf_t *bf) { } -bool mp_jog_busy() {return mach_get_cycle() == CYCLE_JOGGING;} +bool mp_jog_busy() {return mp_get_cycle() == CYCLE_JOGGING;} uint8_t command_jog(int argc, char *argv[]) { if (!mp_jog_busy() && - (mach_get_state() != STATE_READY || mach_get_cycle() != CYCLE_MACHINING)) + (mp_get_state() != STATE_READY || mp_get_cycle() != CYCLE_MACHINING)) return STAT_NOOP; float velocity[AXES]; @@ -143,7 +144,7 @@ uint8_t command_jog(int argc, char *argv[]) { } // Start - mach_set_cycle(CYCLE_JOGGING); + mp_set_cycle(CYCLE_JOGGING); bf->bf_func = _exec_jog; // register callback mp_commit_write_buffer(-1, MOVE_TYPE_COMMAND); } diff --git a/src/plan/planner.c b/src/plan/planner.c index cb1a396..5375deb 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -64,7 +64,7 @@ #include "machine.h" #include "stepper.h" #include "motor.h" -#include "estop.h" +#include "state.h" #include #include @@ -89,7 +89,6 @@ void planner_init() { void mp_flush_planner() { mach_abort_arc(); mp_init_buffers(); - mach_set_state(STATE_READY); } @@ -172,7 +171,7 @@ float mp_get_runtime_work_position(uint8_t axis) { /// Use this function to sync to the queue. If you wait until it returns /// FALSE you know the queue is empty and the motors have stopped. uint8_t mp_get_runtime_busy() { - if (estop_triggered()) return false; + if (mp_get_state() == STATE_ESTOPPED) return false; return st_runtime_isbusy() || mr.move_state == MOVE_RUN; } diff --git a/src/plan/state.c b/src/plan/state.c new file mode 100644 index 0000000..b2c12e8 --- /dev/null +++ b/src/plan/state.c @@ -0,0 +1,206 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2013 - 2015 Alden S. Hart, Jr. + Copyright (c) 2013 - 2015 Robert 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 "state.h" +#include "machine.h" +#include "planner.h" +#include "report.h" +#include "buffer.h" + +#include + + +typedef struct { + plannerState_t state; + plannerCycle_t cycle; + holdState_t hold; + + bool hold_requested; + bool flush_requested; + bool start_requested; +} planner_state_t; + + +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) { + switch (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"); + } + + return PSTR("invalid"); +} + + +PGM_P mp_get_cycle_pgmstr(plannerCycle_t cycle) { + switch (cycle) { + case CYCLE_MACHINING: return PSTR("machining"); + case CYCLE_HOMING: return PSTR("homing"); + case CYCLE_PROBING: return PSTR("probing"); + case CYCLE_CALIBRATING: return PSTR("calibrating"); + case CYCLE_JOGGING: return PSTR("jogging"); + } + + return PSTR("invalid"); +} + + +void mp_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; + report_request(); +} + + +void mp_set_cycle(plannerCycle_t cycle) { + if (ps.cycle == cycle) return; // No change + + if (ps.state != STATE_READY) { + STATUS_ERROR(STAT_INTERNAL_ERROR, "Cannot transition to %S while %S", + mp_get_cycle_pgmstr(cycle), + mp_get_state_pgmstr(ps.state)); + return; + } + + if (ps.cycle != CYCLE_MACHINING && cycle != CYCLE_MACHINING) { + STATUS_ERROR(STAT_INTERNAL_ERROR, + "Cannot transition to cycle %S while in %S", + mp_get_cycle_pgmstr(cycle), + mp_get_cycle_pgmstr(ps.cycle)); + return; + } + + ps.cycle = cycle; + report_request(); +} + + +void mp_set_hold_state(holdState_t hold) { + ps.hold = hold; +} + + +void mp_state_running() { + if (mp_get_state() == STATE_READY) mp_set_state(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 cycle start request + mp_zero_segment_velocity(); // for reporting purposes +} + + +void mp_state_hold() { + mp_set_state(STATE_HOLDING); +} + + +void mp_state_estop() { + mp_set_state(STATE_ESTOPPED); +} + + +/* Feedholds, queue flushes and cycles starts are all related. The request + * functions set flags for these. The sequencing callback interprets the flags + * according to the following rules: + * + * Feedhold request received during motion is honored + * Feedhold request received during a feedhold is ignored and reset + * Feedhold request received during a motion stop is ignored and reset + * + * Queue flush request received during motion is ignored but not reset + * Queue flush request received during a feedhold is deferred until + * the feedhold enters a HOLD state (i.e. until deceleration is complete) + * Queue flush request received during a motion stop is honored + * + * Cycle start request received during motion is ignored and reset + * Cycle start request received during a feedhold is deferred until + * the feedhold enters a HOLD state (i.e. until deceleration is complete) + * If a queue flush request is also present the queue flush is done first + * Cycle start request received during a motion stop is honored and starts + * to run anything in the planner queue + */ + + +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_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 + } + } + + // Only flush queue when we are stopped 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(); // flush planner queue + + // NOTE: The following uses low-level mp calls for absolute position + for (int axis = 0; axis < AXES; axis++) + // set mm from mr + mach_set_position(axis, mp_get_runtime_absolute_position(axis)); + } + + // Don't start cycle when stopping + if (ps.start_requested && mp_get_state() != STATE_STOPPING) { + 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); + } + } +} diff --git a/src/plan/state.h b/src/plan/state.h new file mode 100644 index 0000000..5c255b2 --- /dev/null +++ b/src/plan/state.h @@ -0,0 +1,82 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2013 - 2015 Alden S. Hart, Jr. + Copyright (c) 2013 - 2015 Robert 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" + +\******************************************************************************/ + +#pragma once + +#include + + +typedef enum { + STATE_READY, + STATE_ESTOPPED, + STATE_RUNNING, + STATE_STOPPING, + STATE_HOLDING, +} plannerState_t; + + +typedef enum { + CYCLE_MACHINING, + CYCLE_HOMING, + CYCLE_PROBING, + CYCLE_CALIBRATING, + CYCLE_JOGGING, +} 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_running(); +void mp_state_idle(); +void mp_state_hold(); +void mp_state_estop(); + +void mp_request_hold(); +void mp_request_flush(); +void mp_request_start(); + +void mp_state_callback(); diff --git a/src/probing.c b/src/probing.c index e79ee52..0ac1f3d 100644 --- a/src/probing.c +++ b/src/probing.c @@ -32,6 +32,7 @@ #include "util.h" #include "plan/planner.h" +#include "plan/state.h" #include @@ -69,7 +70,7 @@ static struct pbProbingSingleton pb; * before declaring the cycle to be done. Otherwise there is a nasty * race condition in the tg_controller() that will accept the next * command before the position of the final move has been recorded in - * the Gcode model. That's what the call to mach_get_runtime_busy() is + * the Gcode model. That's what the call to mp_get_runtime_busy() is * about. */ @@ -88,8 +89,7 @@ static void _probe_restore_settings() { // update the model with actual position mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); - mach_cycle_end(); - mach_set_cycle(CYCLE_MACHINING); + mp_set_cycle(CYCLE_MACHINING); } @@ -141,7 +141,7 @@ static void _probing_init() { // it is an error for the limit or homing switches to fire, or for some other // configuration error. mach.probe_state = PROBE_FAILED; - mach_set_cycle(CYCLE_PROBING); + mp_set_cycle(CYCLE_PROBING); // initialize the axes - save the jerk settings & switch to the jerk_homing // settings @@ -176,7 +176,7 @@ static void _probing_init() { bool mach_is_probing() { - return mach_get_cycle() == CYCLE_PROBING || mach.probe_state == PROBE_WAITING; + return mp_get_cycle() == CYCLE_PROBING || mach.probe_state == PROBE_WAITING; } @@ -208,7 +208,7 @@ stat_t mach_straight_probe(float target[], float flags[]) { /// main loop callback for running the homing cycle void mach_probe_callback() { // sync to planner move ends - if (!mach_is_probing() || mach_get_runtime_busy()) return; + if (!mach_is_probing() || mp_get_runtime_busy()) return; pb.func(); // execute the current homing move } diff --git a/src/varcb.c b/src/varcb.c index e75a169..1725107 100644 --- a/src/varcb.c +++ b/src/varcb.c @@ -29,6 +29,7 @@ #include "usart.h" #include "machine.h" #include "plan/planner.h" +#include "plan/state.h" #include "plan/buffer.h" @@ -41,5 +42,5 @@ bool get_echo() {return usart_is_set(USART_ECHO);} void set_echo(bool value) {return usart_set(USART_ECHO, value);} uint16_t get_queue() {return mp_get_planner_buffer_room();} int32_t get_line() {return mr.ms.line;} -PGM_P get_state() {return mach_get_state_pgmstr(mach_get_state());} -PGM_P get_cycle() {return mach_get_cycle_pgmstr(mach_get_cycle());} +PGM_P get_state() {return mp_get_state_pgmstr(mp_get_state());} +PGM_P get_cycle() {return mp_get_cycle_pgmstr(mp_get_cycle());}