#include "plan/calibrate.h"
#include "plan/buffer.h"
#include "plan/arc.h"
+#include "plan/state.h"
#include "config.h"
#include <avr/pgmspace.h>
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();}
#include "config.h"
#include "plan/planner.h"
+#include "plan/state.h"
#include <avr/eeprom.h>
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
mach_spindle_estop();
// Set machine state
- mach_set_state(STATE_ESTOPPED);
+ mp_state_estop();
// Set axes not homed
mach_set_not_homed();
#include "report.h"
#include "plan/planner.h"
+#include "plan/state.h"
#include <avr/pgmspace.h>
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);
}
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);
bool mach_is_homing() {
- return mach_get_cycle() == CYCLE_HOMING;
+ return mp_get_cycle() == CYCLE_HOMING;
}
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;
}
/// 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
}
}
},
- ._state = STATE_READY,
- ._cycle = CYCLE_MACHINING,
-
// State
.gm = {.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE},
.gn = {0},
// 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;}
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;
}
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)
}
}
- // 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
* 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
// 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();
// 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();
* 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:
* + 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);
}
-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
}
}
-/// 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";
#include "config.h"
#include "status.h"
-#include <avr/pgmspace.h>
-
#include <stdint.h>
#include <stdbool.h>
-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
// 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
// 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();
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);
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);
#include "plan/planner.h"
#include "plan/arc.h"
#include "plan/feedhold.h"
+#include "plan/state.h"
#include <avr/pgmspace.h>
#include <avr/wdt.h>
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
} 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
// 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;
*/
#include "buffer.h"
+#include "state.h"
#include "report.h"
#include <string.h>
}
mb.buffers_available = PLANNER_BUFFER_POOL_SIZE;
+
+ mp_state_idle();
}
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;
* 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;
}
-/* 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
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
}
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;
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
#include "stepper.h"
#include "rtc.h"
#include "tmc2660.h"
+#include "state.h"
#include "config.h"
#include <stdint.h>
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 {
}
-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) {
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();
// 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
bf->mach_func(bf->ms.target, bf->unit);
// Free buffer & perform cycle_end if planner is empty
- mach_advance_buffer();
+ mp_free_run_buffer();
}
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;
}
#include "util.h"
#include "report.h"
#include "estop.h"
+#include "state.h"
#include "config.h"
#include <string.h>
// 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 {
// 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)
// 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;
}
// 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:
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;
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
}
#include "line.h"
#include "zoid.h"
#include "util.h"
+#include "state.h"
#include <stdbool.h>
#include <math.h>
-/* 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.
*
/// 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
_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;
}
_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
}
#include "motor.h"
#include "machine.h"
#include "motor.h"
+#include "state.h"
#include "config.h"
#include <stdbool.h>
// Release buffer
mp_free_run_buffer();
- mach_set_cycle(CYCLE_MACHINING);
+ mp_set_cycle(CYCLE_MACHINING);
return STAT_NOOP;
}
}
-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];
}
// 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);
}
#include "machine.h"
#include "stepper.h"
#include "motor.h"
-#include "estop.h"
+#include "state.h"
#include <string.h>
#include <stdbool.h>
void mp_flush_planner() {
mach_abort_arc();
mp_init_buffers();
- mach_set_state(STATE_READY);
}
/// 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;
}
--- /dev/null
+/******************************************************************************\
+
+ 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 <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 "state.h"
+#include "machine.h"
+#include "planner.h"
+#include "report.h"
+#include "buffer.h"
+
+#include <stdbool.h>
+
+
+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);
+ }
+ }
+}
--- /dev/null
+/******************************************************************************\
+
+ 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 <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
+
+#include <avr/pgmspace.h>
+
+
+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();
#include "util.h"
#include "plan/planner.h"
+#include "plan/state.h"
#include <avr/pgmspace.h>
* 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.
*/
// 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);
}
// 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
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;
}
/// 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
}
#include "usart.h"
#include "machine.h"
#include "plan/planner.h"
+#include "plan/state.h"
#include "plan/buffer.h"
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());}
projects / bbctrl-firmware / commitdiff