switch_set_callback(SW_ESTOP, _switch_callback);
- // Check switch state
-
+ if (estop.triggered) mach_set_state(STATE_ESTOP);
// Fault signal
if (estop.triggered) OUTSET_PIN(FAULT_PIN); // High
st_shutdown();
mach_spindle_estop();
- // Set alarm state
- mach_set_machine_state(MACHINE_ALARM);
+ // Set machine state
+ mach_set_state(STATE_ESTOP);
// Set axes not homed
mach_set_not_homed();
mach.gm.feed_rate = hm.saved_feed_rate;
mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE);
mach_cycle_end();
- mach.cycle_state = CYCLE_OFF;
}
bool mach_is_homing() {
- return mach.cycle_state == CYCLE_HOMING;
+ return mach_get_state() == STATE_HOMING;
}
hm.axis = -1; // set to retrieve initial axis
hm.func = _homing_axis_start; // bind initial processing function
- mach.cycle_state = CYCLE_HOMING;
+ mach_set_state(STATE_HOMING);
mach.homing_state = HOMING_NOT_HOMED;
}
/// Main loop callback for running the homing cycle
void mach_homing_callback() {
- if (mach.cycle_state != CYCLE_HOMING || mach_get_runtime_busy()) return;
+ if (mach_get_state() != STATE_HOMING || mach_get_runtime_busy()) return;
hm.func(hm.axis); // execute the current homing move
}
#include "util.h"
#include "usart.h" // for serial queue flush
#include "estop.h"
+#include "report.h"
#include "plan/planner.h"
#include "plan/buffer.h"
}
},
- .machine_state = MACHINE_READY,
+ ._state = STATE_IDLE,
// State
.gm = {.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE},
static void _exec_mist_coolant_control(float *value, float *flag);
static void _exec_flood_coolant_control(float *value, float *flag);
static void _exec_absolute_origin(float *value, float *flag);
-static void _exec_program_finalize(float *value, float *flag);
// Machine State functions
uint32_t mach_get_line() {return mach.gm.line;}
-machMachineState_t mach_get_machine_state() {return mach.machine_state;}
-machCycleState_t mach_get_cycle_state() {return mach.cycle_state;}
-machMotionState_t mach_get_motion_state() {return mach.motion_state;}
+
+
+bool mach_is_running() {
+ switch (mach_get_state()) {
+ case STATE_RUNNING:
+ case STATE_HOMING:
+ case STATE_PROBING:
+ return true;
+ default: return false;
+ }
+}
+
+
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;}
float mach_get_feed_rate() {return mach.gm.feed_rate;}
-void mach_set_machine_state(machMachineState_t machine_state) {
- mach.machine_state = machine_state;
-}
-
-
-void mach_set_motion_state(machMotionState_t motion_state) {
- mach.motion_state = motion_state;
+void mach_set_state(machState_t state) {
+ if (mach._state == state) return; // No change
+ if (mach._state == STATE_ESTOP) return; // Can't leave EStop state
+ mach._state = state;
+ report_request();
}
}
-/// Clear alarm
-stat_t mach_clear() {
- if (mach.cycle_state == CYCLE_OFF)
- mach.machine_state = MACHINE_PROGRAM_STOP;
- else mach.machine_state = MACHINE_CYCLE;
-
- return STAT_OK;
-}
-
-
// Representation (4.3.3)
//
// Affect the Gcode model only (asynchronous)
// 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_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
+ mp_aline(&mach.ms); // send the move to the planner
mach_finalize_move();
return STAT_OK;
/// Process feedholds, cycle starts & queue flushes
void mach_feedhold_callback() {
if (mach.feedhold_requested) {
- if (mach.motion_state == MOTION_RUN && mach.hold_state == FEEDHOLD_OFF) {
- mach_set_motion_state(MOTION_HOLD);
+ mach.feedhold_requested = false;
+
+ if (mach_is_running()) {
+ mach_set_state(STATE_STOPPING);
mach.hold_state = FEEDHOLD_SYNC; // invokes hold from aline execution
}
-
- mach.feedhold_requested = false;
}
+ // Only flush queue when we are stopped or holding
if (mach.queue_flush_requested &&
- (mach.motion_state == MOTION_STOP ||
- (mach.motion_state == MOTION_HOLD &&
- mach.hold_state == FEEDHOLD_HOLD)) &&
+ (mach_get_state() == STATE_IDLE || mach_get_state() == STATE_HOLDING) &&
!mach_get_runtime_busy()) {
- mach_queue_flush();
mach.queue_flush_requested = false;
+ mach_queue_flush();
}
- bool processing =
- mach.hold_state == FEEDHOLD_SYNC ||
- mach.hold_state == FEEDHOLD_PLAN ||
- mach.hold_state == FEEDHOLD_DECEL;
-
- if (mach.cycle_start_requested && !processing) {
+ // Don't start cycle when stopping
+ if (mach.cycle_start_requested && mach_get_state() != STATE_STOPPING) {
mach.cycle_start_requested = false;
- mach.hold_state = FEEDHOLD_END_HOLD;
- mach_cycle_start();
- mp_end_hold();
+
+ 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_IDLE);
+ }
}
mp_plan_hold_callback();
}
-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] = {MACHINE_PROGRAM_STOP};
- _exec_program_finalize(value, value); // finalize now, not later
-
- return STAT_OK;
+static void _exec_program_finalize(float *value, float *flag) {
+ mach_set_state(STATE_IDLE);
+ 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
}
-/* Program and cycle state functions
- *
- * mach_program_end() implements M2 and M30. End behaviors are defined by
+/*** _exec_program_end() implements M2 and M30. End behaviors are defined by
* NIST 3.6.1 are:
*
* 1. Axis offsets are set to zero (like G92.2) and origin offsets are set
* 8. The current motion mode is set to G_1 (like G1)
* 9. Coolant is turned off (like M9)
*
- * mach_program_end() implments things slightly differently:
+ * _exec_program_end() implements things slightly differently:
*
* 1. Axis offsets are set to G92.1 CANCEL offsets
* (instead of using G92.2 SUSPEND Offsets)
* 9. Coolant is turned off (like M9)
* + Default INCHES or MM units mode is restored
*/
-static void _exec_program_finalize(float *value, float *flag) {
- mach.machine_state = (machMachineState_t)value[0];
- mach_set_motion_state(MOTION_STOP);
- if (mach.cycle_state == CYCLE_MACHINING)
- mach.cycle_state = CYCLE_OFF; // don't end cycle if homing, probing, etc
+static void _exec_program_end(float *value, float *flag) {
+ _exec_program_finalize(value, flag);
- mach.hold_state = FEEDHOLD_OFF; // end feedhold (if in feed hold)
- mach.cycle_start_requested = false; // cancel any pending cycle start request
- mp_zero_segment_velocity(); // for reporting purposes
+ // 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);
+ mach_set_distance_mode(GCODE_DEFAULT_DISTANCE_MODE);
+ mach_spindle_control(SPINDLE_OFF); // M5
+ mach_flood_coolant_control(false); // M9
+ mach_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); // G94
+ mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE);
+}
- // perform the following resets if it's a program END
- if (mach.machine_state == MACHINE_PROGRAM_END) {
- 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);
- mach_set_distance_mode(GCODE_DEFAULT_DISTANCE_MODE);
- mach_spindle_control(SPINDLE_OFF); // M5
- mach_flood_coolant_control(false); // M9
- mach_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); // G94
- mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE);
- }
+
+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() {
- mach.machine_state = MACHINE_CYCLE;
-
// don't (re)start homing, probe or other canned cycles
- if (mach.cycle_state == CYCLE_OFF) mach.cycle_state = CYCLE_MACHINING;
+ if (mach_get_state() == STATE_IDLE) mach_set_state(STATE_RUNNING);
}
/// Do a cycle end right now
void mach_cycle_end() {
- if (mach.cycle_state != CYCLE_OFF) {
- float value[AXES] = {MACHINE_PROGRAM_STOP};
- _exec_program_finalize(value, value);
- }
+ float value[AXES] = {};
+ _exec_program_finalize(value, value);
}
/// M0 Queue a program stop
void mach_program_stop() {
- float value[AXES] = {MACHINE_PROGRAM_STOP};
+ float value[AXES] = {};
mp_queue_command(_exec_program_finalize, value, value);
}
/// M2, M30
void mach_program_end() {
- float value[AXES] = {MACHINE_PROGRAM_END};
- mp_queue_command(_exec_program_finalize, value, value);
+ float value[AXES] = {};
+ mp_queue_command(_exec_program_end, value, value);
+}
+
+
+/// Free run buffer and end cycle if planner is empty
+void mach_advance_buffer() {
+ if (mp_free_run_buffer()) mach_cycle_end();
}
-/* Machine state model
- *
- * The following main variables track machine state and state transitions.
- *
- * machine_state - overall state of machine and program execution
- * cycle_state - what cycle the machine is executing (or none)
- * motion_state - state of movement
- *
- * Allowed states:
- *
- * MACHINE STATE CYCLE STATE MOTION_STATE
- * ------------- ------------ -------------
- * MACHINE_READY CYCLE_OFF MOTION_STOP
- * MACHINE_PROG_STOP CYCLE_OFF MOTION_STOP
- * MACHINE_PROG_END CYCLE_OFF MOTION_STOP
- *
- * MACHINE_CYCLE CYCLE_STARTED MOTION_STOP
- * MACHINE_CYCLE CYCLE_STARTED MOTION_RUN
- * MACHINE_CYCLE CYCLE_STARTED MOTION_HOLD
- *
- * MACHINE_CYCLE CYCLE_HOMING MOTION_STOP
- * MACHINE_CYCLE CYCLE_HOMING MOTION_RUN
- * MACHINE_CYCLE CYCLE_HOMING MOTION_HOLD
- */
-
typedef enum {
- MACHINE_READY, // machine is ready for use
- MACHINE_ALARM, // machine in alarm state
- MACHINE_PROGRAM_STOP, // program stop or no more blocks
- MACHINE_PROGRAM_END, // program end
- MACHINE_CYCLE, // machine is running (cycling)
-} machMachineState_t;
-
-
-typedef enum {
- CYCLE_OFF, // machine is idle
- CYCLE_MACHINING, // in normal machining cycle
- CYCLE_PROBE, // in probe cycle
- CYCLE_HOMING, // homing is treated as a specialized cycle
-} machCycleState_t;
-
-
-typedef enum {
- MOTION_STOP, // motion has stopped
- MOTION_RUN, // machine is in motion
- MOTION_HOLD, // feedhold in progress
-} machMotionState_t;
+ STATE_IDLE,
+ STATE_ESTOP,
+ STATE_RUNNING,
+ STATE_HOMING,
+ STATE_PROBING,
+ STATE_STOPPING,
+ STATE_HOLDING,
+} machState_t;
typedef enum { // feedhold_state machine
FEEDHOLD_PLAN, // replan blocks for feedhold
FEEDHOLD_DECEL, // decelerate to hold point
FEEDHOLD_HOLD, // holding
- FEEDHOLD_END_HOLD, // end hold (transient state to OFF)
} machFeedholdState_t;
// settings for axes X,Y,Z,A B,C
AxisConfig_t a[AXES];
- machMachineState_t machine_state;
- machCycleState_t cycle_state;
- machMotionState_t motion_state;
+ machState_t _state;
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
// Model state getters and setters
uint32_t mach_get_line();
-machMachineState_t mach_get_machine_state();
-machCycleState_t mach_get_cycle_state();
-machMotionState_t mach_get_motion_state();
+inline machState_t mach_get_state() {return mach._state;}
+bool mach_is_running();
machFeedholdState_t mach_get_hold_state();
machHomingState_t mach_get_homing_state();
machMotionMode_t mach_get_motion_mode();
bool mach_get_runtime_busy();
float mach_get_feed_rate();
-void mach_set_machine_state(machMachineState_t machine_state);
-void mach_set_motion_state(machMotionState_t motion_state);
+void mach_set_state(machState_t state);
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 machine_init();
/// enter alarm state. returns same status code
stat_t mach_alarm(const char *location, stat_t status);
-stat_t mach_clear();
#define CM_ALARM(CODE) mach_alarm(STATUS_LOCATION, CODE)
+#define CM_ASSERT(COND) \
+ do {if (!(COND)) CM_ALARM(STAT_INTERNAL_ERROR);} while (0)
// Representation (4.3.3)
void mach_set_plane(machPlane_t plane);
void mach_pallet_change_stop();
void mach_program_end();
-// Cycles
+void mach_advance_buffer();
+
char mach_get_axis_char(int8_t axis);
* Returns true if queue is empty, false otherwise.
* This is useful for doing queue empty / end move functions.
*/
-uint8_t 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
+bool mp_free_run_buffer() { // EMPTY current run buf & adv to next
+ mp_clear_buffer(mb.r); // clear it out (& reset replannable)
+ mb.r = mb.r->nx; // advance to next run buffer
if (mb.r->buffer_state == MP_BUFFER_QUEUED) // only if queued...
mb.r->buffer_state = MP_BUFFER_PENDING; // pend next buffer
mpBuf_t *mp_get_write_buffer();
void mp_commit_write_buffer(uint32_t line, moveType_t move_type);
mpBuf_t *mp_get_run_buffer();
-uint8_t mp_free_run_buffer();
+bool mp_free_run_buffer();
mpBuf_t *mp_get_first_buffer();
mpBuf_t *mp_get_last_buffer();
/// Returns pointer to prev buffer in linked list
bf->mach_func(bf->ms.target, bf->unit);
// Free buffer & perform cycle_end if planner is empty
- if (mp_free_run_buffer()) mach_cycle_end();
+ mach_advance_buffer();
}
st_prep_dwell(bf->ms.move_time); // in seconds
// free buffer & perform cycle_end if planner is empty
- if (mp_free_run_buffer()) mach_cycle_end();
+ mach_advance_buffer();
return STAT_OK;
}
// 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.motion_state == MOTION_RUN && mach.cycle_state == CYCLE_MACHINING)
+ mach_get_state() == STATE_RUNNING)
copy_vector(mr.ms.target, mr.waypoint[mr.section]);
else {
// prevent overplanning (Note 2)
bf->nx->replannable = false;
// free buffer & end cycle if planner is empty
- if (mp_free_run_buffer()) mach_cycle_end();
+ mach_advance_buffer();
return STAT_NOOP;
}
// 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_motion_state(MOTION_HOLD);
+ mach_set_state(STATE_HOLDING);
report_request();
}
mr.section_state = SECTION_OFF;
bf->nx->replannable = false; // prevent overplanning (Note 2)
- if (bf->move_state == MOVE_RUN && mp_free_run_buffer())
- mach_cycle_end(); // free buffer & end cycle if planner is empty
+ if (bf->move_state == MOVE_RUN) mach_advance_buffer();
}
return status;
if (!bf) return STAT_NOOP; // nothing's running
if (estop_triggered()) return STAT_MACHINE_ALARMED;
- // Manage cycle and motion state transitions
+ // Manage state transitions
// Cycle auto-start for lines only
- if (bf->move_type == MOVE_TYPE_ALINE && mach.motion_state == MOTION_STOP)
- mach_set_motion_state(MOTION_RUN);
+ if (bf->move_type == MOVE_TYPE_ALINE && mach_get_state() == STATE_IDLE)
+ mach_set_state(STATE_RUNNING);
if (!bf->bf_func)
return CM_ALARM(STAT_INTERNAL_ERROR); // never supposed to get here
#include <math.h>
/* Feedhold is executed as mach.hold_state transitions executed inside
- * _exec_aline() and main loop callbacks to these functions:
- * mp_plan_hold_callback() and mp_end_hold().
+ * _exec_aline() and main loop callbacks to mp_plan_hold_callback()
*
* Holds work like this:
*
- * - Hold is asserted by calling mach_feedhold() (usually invoked via a
- * ! char) If hold_state is OFF and motion_state is RUNning it sets
- * hold_state to SYNC and motion_state to HOLD.
+ * - 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
* TRUE. It can occur any time after the hold is requested - either
* before or after motion stops.
*
- * - mp_end_hold() is executed from mach_feedhold_sequencing_callback()
- * once the hold state == HOLD and a cycle_start has been
- * requested.This sets the hold state to OFF which enables
- * _exec_aline() to continue processing. Move execution begins with
- * the first buffer after the hold.
- *
* Terms used:
* - mr is the runtime buffer. It was initially loaded from the bf
* buffer
mp_plan_block_list(mp_get_last_buffer(), &mr_flag);
mach.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit
}
-
-
-/// End a feedhold, release the hold and restart block list
-void mp_end_hold() {
- if (mach.hold_state == FEEDHOLD_END_HOLD) {
- mach.hold_state = FEEDHOLD_OFF;
-
- // 0 means nothing's running
- if (!mp_get_run_buffer()) mach_set_motion_state(MOTION_STOP);
- else mach.motion_state = MOTION_RUN;
- }
-}
void mp_plan_hold_callback();
-void mp_end_hold();
void mp_flush_planner() {
mach_abort_arc();
mp_init_buffers();
- mach_set_motion_state(MOTION_STOP);
+ mach_set_state(STATE_IDLE);
}
static struct pbProbingSingleton pb;
-/* All mach_probe_cycle_start does is prevent any new commands from
- * queueing to the planner so that the planner can move to a sop and
- * report MACHINE_PROGRAM_STOP. OK, it also queues the function
- * that's called once motion has stopped.
- *
- * Note: When coding a cycle (like this one) you get to perform one
+/* Note: When coding a cycle (like this one) you get to perform one
* queued move per entry into the continuation, then you must exit.
*
* Another Note: When coding a cycle (like this one) you must wait
// update the model with actual position
mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE);
mach_cycle_end();
- mach.cycle_state = CYCLE_OFF;
}
// it is an error for the limit or homing switches to fire, or for some other
// configuration error.
mach.probe_state = PROBE_FAILED;
- mach.cycle_state = CYCLE_PROBE;
+ mach_set_state(STATE_PROBING);
// initialize the axes - save the jerk settings & switch to the jerk_homing
// settings
bool mach_is_probing() {
- return mach.cycle_state == CYCLE_PROBE || mach.probe_state == PROBE_WAITING;
+ return mach_get_state() == STATE_PROBING || mach.probe_state == PROBE_WAITING;
}
*/
#include "switch.h"
-
-#include "hardware.h"
-#include "machine.h"
#include "config.h"
#include <avr/interrupt.h>
if (!s->count) { // switch triggered
s->debounce = SW_LOCKOUT;
if (s->cb) s->cb(i, s->state);
-
- // TODO fix this
- if (mach.cycle_state == CYCLE_HOMING || mach.cycle_state == CYCLE_PROBE)
- mach_request_feedhold();
}
}
}
#include "plan/planner.h"
#include "plan/buffer.h"
+#include <avr/pgmspace.h>
+
float get_position(int index) {return mp_get_runtime_absolute_position(index);}
float get_velocity() {return mp_get_runtime_velocity();}
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() {
+ switch (mach_get_state()) {
+ case STATE_IDLE: return PSTR("idle");
+ case STATE_ESTOP: return PSTR("estop");
+ case STATE_RUNNING: return PSTR("running");
+ case STATE_HOMING: return PSTR("homing");
+ case STATE_PROBING: return PSTR("probing");
+ case STATE_STOPPING: return PSTR("stopping");
+ case STATE_HOLDING: return PSTR("holding");
+ }
+
+ return PSTR("invalid");
+}
VAR(estop_reason, "er", pstring, 0, 0, 0, "Emergency stop reason")
VAR(line, "ln", int32_t, 0, 0, 0, "Last GCode line executed")
VAR(queue, "q", uint16_t, 0, 0, 0, "Space in planner queue")
+VAR(state, "s", pstring, 0, 0, 0, "Machine state")
projects / bbctrl-firmware / commitdiff