mirror of
https://github.com/grblHAL/core.git
synced 2026-02-05 08:34:01 +08:00
13145a4a46
Parking mode improvements. Removed requirement for external encoder for spindle sync if stepper spindle is enabled. Improved handling of $680 stepper enable delay.
863 lines
33 KiB
C
863 lines
33 KiB
C
/*
|
|
state_machine.c - An embedded CNC Controller with rs274/ngc (g-code) support
|
|
|
|
Main state machine
|
|
|
|
Part of grblHAL
|
|
|
|
Copyright (c) 2018-2025 Terje Io
|
|
Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
|
|
Copyright (c) 2009-2011 Simen Svale Skogsrud
|
|
|
|
grblHAL is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
grblHAL 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 General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with grblHAL. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <string.h>
|
|
|
|
#include "hal.h"
|
|
#include "motion_control.h"
|
|
#include "state_machine.h"
|
|
#include "override.h"
|
|
|
|
static void state_idle (uint_fast16_t new_state);
|
|
static void state_cycle (uint_fast16_t rt_exec);
|
|
static void state_await_hold (uint_fast16_t rt_exec);
|
|
static void state_noop (uint_fast16_t rt_exec);
|
|
static void state_await_motion_cancel (uint_fast16_t rt_exec);
|
|
static void state_await_resume (uint_fast16_t rt_exec);
|
|
static void state_await_toolchanged (uint_fast16_t rt_exec);
|
|
static void state_await_waypoint_retract (uint_fast16_t rt_exec);
|
|
static void state_restore (uint_fast16_t rt_exec);
|
|
static void state_await_resumed (uint_fast16_t rt_exec);
|
|
|
|
static void (*volatile stateHandler)(uint_fast16_t rt_exec) = state_idle;
|
|
|
|
typedef struct {
|
|
coolant_state_t coolant;
|
|
spindle_num_t spindle_num; // Active spindle
|
|
spindle_t spindle[N_SYS_SPINDLE];
|
|
} restore_condition_t;
|
|
|
|
static restore_condition_t restore_condition;
|
|
static sys_state_t pending_state = STATE_IDLE, sys_state = STATE_IDLE;
|
|
|
|
typedef union {
|
|
uint8_t value;
|
|
struct {
|
|
uint8_t active :1,
|
|
motion :1,
|
|
restart :1,
|
|
restoring :1,
|
|
unassigned :4;
|
|
};
|
|
} parking_flags_t;
|
|
|
|
typedef struct {
|
|
float target[N_AXIS];
|
|
float restore_target[N_AXIS];
|
|
float retract_waypoint;
|
|
volatile parking_flags_t flags;
|
|
plan_line_data_t plan_data;
|
|
} parking_data_t;
|
|
|
|
// Declare and initialize parking local variables
|
|
static parking_data_t park = {0};
|
|
|
|
static void state_spindle_restore (spindle_t *spindle, uint16_t on_delay_ms)
|
|
{
|
|
if(spindle->hal) {
|
|
if(grbl.on_spindle_programmed)
|
|
grbl.on_spindle_programmed(spindle->hal, spindle->state, spindle->rpm, spindle->rpm_mode);
|
|
spindle_restore(spindle->hal, spindle->state, spindle->rpm, on_delay_ms);
|
|
}
|
|
}
|
|
|
|
static void state_restore_conditions (restore_condition_t *condition)
|
|
{
|
|
if(!settings.parking.flags.enabled || !park.flags.restart) {
|
|
|
|
spindle_num_t spindle_num = N_SYS_SPINDLE;
|
|
|
|
park.flags.restoring = On; //
|
|
|
|
do {
|
|
state_spindle_restore(&condition->spindle[--spindle_num], (uint16_t)(settings.safety_door.spindle_on_delay * 1000.0f));
|
|
} while(spindle_num);
|
|
|
|
// Block if safety door re-opened during prior restore actions.
|
|
if(gc_state.modal.coolant.value != hal.coolant.get_state().value) {
|
|
// NOTE: Laser mode will honor this delay. An exhaust system is often controlled by this signal.
|
|
coolant_restore(condition->coolant, (uint16_t)(settings.safety_door.coolant_on_delay * 1000.0f));
|
|
gc_coolant(condition->coolant);
|
|
}
|
|
|
|
park.flags.restoring = Off;
|
|
|
|
sys.override.spindle_stop.value = 0; // Clear spindle stop override states
|
|
}
|
|
}
|
|
|
|
static void enter_sleep (void)
|
|
{
|
|
st_go_idle();
|
|
spindle_all_off();
|
|
hal.coolant.set_state((coolant_state_t){0});
|
|
grbl.report.feedback_message(Message_SleepMode);
|
|
stateHandler = state_noop;
|
|
}
|
|
|
|
static bool initiate_hold (uint_fast16_t new_state)
|
|
{
|
|
spindle_t *spindle;
|
|
spindle_num_t spindle_num = N_SYS_SPINDLE;
|
|
|
|
if (settings.parking.flags.enabled) {
|
|
plan_data_init(&park.plan_data);
|
|
park.plan_data.condition.system_motion = On;
|
|
park.plan_data.condition.no_feed_override = On;
|
|
park.plan_data.line_number = PARKING_MOTION_LINE_NUMBER;
|
|
}
|
|
|
|
plan_block_t *block = plan_get_current_block();
|
|
|
|
restore_condition.spindle_num = 0;
|
|
|
|
do {
|
|
if((spindle = gc_spindle_get(--spindle_num))) {
|
|
if(block && block->spindle.hal == spindle->hal) {
|
|
restore_condition.spindle_num = spindle_num;
|
|
restore_condition.spindle[spindle_num].hal = block->spindle.hal;
|
|
restore_condition.spindle[spindle_num].rpm = block->spindle.rpm;
|
|
restore_condition.spindle[spindle_num].state = block->spindle.state;
|
|
} else if(spindle->hal) {
|
|
restore_condition.spindle_num = spindle_num;
|
|
restore_condition.spindle[spindle_num].hal = spindle->hal;
|
|
restore_condition.spindle[spindle_num].rpm = spindle->rpm;
|
|
restore_condition.spindle[spindle_num].state = spindle->state;
|
|
} else {
|
|
restore_condition.spindle[spindle_num].hal = NULL;
|
|
// restore_condition.spindle[spindle_num].rpm = spindle->param->rpm;
|
|
// restore_condition.spindle[spindle_num].state = spindle->param->state;
|
|
}
|
|
} else
|
|
restore_condition.spindle[spindle_num].hal = NULL;
|
|
} while(spindle_num);
|
|
|
|
if (block)
|
|
restore_condition.coolant.mask = block->condition.coolant.mask;
|
|
else
|
|
restore_condition.coolant.mask = gc_state.modal.coolant.mask | hal.coolant.get_state().mask;
|
|
|
|
if (restore_condition.spindle[restore_condition.spindle_num].hal->cap.laser && settings.flags.disable_laser_during_hold)
|
|
enqueue_spindle_override(CMD_OVERRIDE_SPINDLE_STOP);
|
|
|
|
if (sys_state & (STATE_CYCLE|STATE_JOG)) {
|
|
st_update_plan_block_parameters(false); // Notify stepper module to recompute for hold deceleration.
|
|
sys.step_control.execute_hold = On; // Initiate suspend state with active flag.
|
|
stateHandler = state_await_hold;
|
|
}
|
|
|
|
if (new_state == STATE_HOLD)
|
|
sys.holding_state = Hold_Pending;
|
|
else {
|
|
sys.parking_state = Parking_Retracting;
|
|
park.flags.value = 0;
|
|
}
|
|
|
|
sys.suspend = !sys.flags.soft_limit;
|
|
pending_state = sys_state == STATE_JOG ? new_state : STATE_IDLE;
|
|
|
|
return sys_state == STATE_CYCLE;
|
|
}
|
|
|
|
bool state_door_reopened (void)
|
|
{
|
|
return settings.parking.flags.enabled && park.flags.restart;
|
|
}
|
|
|
|
void state_update (rt_exec_t rt_exec)
|
|
{
|
|
if((rt_exec & EXEC_SAFETY_DOOR) && sys_state != STATE_SAFETY_DOOR)
|
|
state_set(STATE_SAFETY_DOOR);
|
|
|
|
stateHandler(rt_exec);
|
|
}
|
|
|
|
ISR_CODE sys_state_t ISR_FUNC(state_get)(void)
|
|
{
|
|
return sys_state;
|
|
}
|
|
|
|
uint8_t state_get_substate (void)
|
|
{
|
|
uint8_t substate = 0;
|
|
|
|
switch(sys_state) {
|
|
|
|
case STATE_CYCLE:
|
|
if(sys.flags.feed_hold_pending)
|
|
substate = 1;
|
|
else if(sys.probing_state == Probing_Active || (hal.probe.get_state && hal.probe.get_state().triggered))
|
|
substate = 2;
|
|
break;
|
|
|
|
case STATE_HOLD:
|
|
substate = sys.holding_state - 1;
|
|
break;
|
|
|
|
case STATE_ESTOP:
|
|
case STATE_ALARM:
|
|
substate = sys.alarm;
|
|
break;
|
|
|
|
case STATE_SAFETY_DOOR:
|
|
substate = sys.parking_state;
|
|
break;
|
|
}
|
|
|
|
return substate;
|
|
}
|
|
|
|
void state_set (sys_state_t new_state)
|
|
{
|
|
if(new_state != sys_state) {
|
|
|
|
sys_state_t org_state = sys_state;
|
|
|
|
switch(new_state) { // Set up new state and handler
|
|
|
|
case STATE_IDLE:
|
|
sys.suspend = false; // Break suspend state.
|
|
sys.step_control.flags = 0; // Restore step control to normal operation.
|
|
sys.parking_state = Parking_DoorClosed;
|
|
sys.holding_state = Hold_NotHolding;
|
|
sys_state = pending_state = new_state;
|
|
park.flags.value = 0;
|
|
stateHandler = state_idle;
|
|
if(sys.flags.feed_hold_pending) {
|
|
pending_state = STATE_HOLD;
|
|
system_set_exec_state_flag(EXEC_FEED_HOLD);
|
|
}
|
|
break;
|
|
|
|
case STATE_CYCLE:
|
|
if (sys_state == STATE_IDLE) {
|
|
// Start cycle only if queued motions exist in planner buffer and the motion is not canceled.
|
|
plan_block_t *block;
|
|
if ((block = plan_get_current_block())) {
|
|
sys_state = new_state;
|
|
sys.steppers_deenergize = false; // Cancel stepper deenergize if pending.
|
|
st_prep_buffer(); // Initialize step segment buffer before beginning cycle.
|
|
if (block->spindle.state.synchronized) {
|
|
|
|
uint32_t ms = hal.get_elapsed_ticks();
|
|
|
|
if (block->spindle.hal->reset_data)
|
|
block->spindle.hal->reset_data();
|
|
|
|
if(!block->condition.units_per_rev) {
|
|
|
|
uint32_t index = block->spindle.hal->get_data(SpindleData_Counters)->index_count + 2;
|
|
|
|
while(index != block->spindle.hal->get_data(SpindleData_Counters)->index_count) {
|
|
|
|
grbl.on_execute_realtime(sys_state);
|
|
|
|
if(hal.get_elapsed_ticks() - ms > 5000) {
|
|
system_raise_alarm(Alarm_Spindle);
|
|
return;
|
|
}
|
|
|
|
if(sys.rt_exec_state & (EXEC_RESET|EXEC_STOP)) {
|
|
system_set_exec_state_flag(EXEC_RESET);
|
|
return;
|
|
}
|
|
// TODO: allow real time reporting?
|
|
}
|
|
} else if(block->spindle.hal->get_data(SpindleData_RPM)->rpm == 0.0f) {
|
|
system_raise_alarm(Alarm_Spindle);
|
|
return;
|
|
}
|
|
}
|
|
st_wake_up();
|
|
stateHandler = state_cycle;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case STATE_JOG:
|
|
if (sys_state == STATE_TOOL_CHANGE)
|
|
pending_state = STATE_TOOL_CHANGE;
|
|
sys_state = new_state;
|
|
stateHandler = state_cycle;
|
|
break;
|
|
|
|
case STATE_TOOL_CHANGE:
|
|
sys_state = new_state;
|
|
stateHandler = state_await_toolchanged;
|
|
break;
|
|
|
|
case STATE_HOLD:
|
|
if(!((sys_state & STATE_JOG) || sys.override.control.feed_hold_disable) ||
|
|
(pending_state == STATE_HOLD && sys.flags.feed_hold_pending)) {
|
|
if(!initiate_hold(new_state)) {
|
|
sys.holding_state = Hold_Complete;
|
|
stateHandler = state_await_resume;
|
|
}
|
|
sys_state = new_state;
|
|
sys.flags.feed_hold_pending = Off;
|
|
} else if(sys.override.control.sync && sys.override.control.feed_hold_disable)
|
|
sys.flags.feed_hold_pending = On;
|
|
break;
|
|
|
|
case STATE_SAFETY_DOOR:
|
|
if ((sys_state & (STATE_ALARM|STATE_ESTOP|STATE_SLEEP|STATE_CHECK_MODE)))
|
|
return;
|
|
grbl.report.feedback_message(Message_SafetyDoorAjar);
|
|
// no break
|
|
case STATE_SLEEP:
|
|
sys.parking_state = Parking_Retracting;
|
|
if (!initiate_hold(new_state)) {
|
|
if (pending_state != new_state) {
|
|
sys_state = new_state;
|
|
state_await_hold(EXEC_CYCLE_COMPLETE); // "Simulate" a cycle stop
|
|
}
|
|
} else
|
|
sys_state = new_state;
|
|
if(sys_state == STATE_SLEEP && stateHandler != state_await_waypoint_retract)
|
|
enter_sleep();
|
|
break;
|
|
|
|
case STATE_ALARM:
|
|
case STATE_ESTOP:
|
|
case STATE_HOMING:
|
|
case STATE_CHECK_MODE:
|
|
sys_state = new_state;
|
|
sys.suspend = false;
|
|
stateHandler = state_noop;
|
|
break;
|
|
}
|
|
|
|
sys.flags.is_homing = sys_state == STATE_HOMING;
|
|
|
|
if(!(sys_state & (STATE_ALARM|STATE_ESTOP)))
|
|
sys.alarm = Alarm_None;
|
|
|
|
if(sys_state != org_state && grbl.on_state_change)
|
|
grbl.on_state_change(sys_state);
|
|
}
|
|
}
|
|
|
|
// Suspend manager. Controls spindle overrides in hold states.
|
|
void state_suspend_manager (void)
|
|
{
|
|
if (stateHandler != state_await_resume || !gc_spindle_get(0)->state.on)
|
|
return;
|
|
|
|
if(sys.override.spindle_stop.value) {
|
|
|
|
// Handles beginning of spindle stop
|
|
if(sys.override.spindle_stop.initiate) {
|
|
sys.override.spindle_stop.value = 0; // Clear stop override state
|
|
if(grbl.on_spindle_programmed)
|
|
grbl.on_spindle_programmed(restore_condition.spindle[restore_condition.spindle_num].hal, (spindle_state_t){0}, 0.0f, 0);
|
|
spindle_set_state(restore_condition.spindle[restore_condition.spindle_num].hal, (spindle_state_t){0}, 0.0f); // De-energize
|
|
sys.override.spindle_stop.enabled = On; // Set stop override state to enabled, if de-energized.
|
|
if(grbl.on_override_changed)
|
|
grbl.on_override_changed(OverrideChanged_SpindleState);
|
|
}
|
|
|
|
// Handles restoring of spindle state
|
|
if(sys.override.spindle_stop.restore) {
|
|
grbl.report.feedback_message(Message_SpindleRestore);
|
|
state_spindle_restore(&restore_condition.spindle[restore_condition.spindle_num], settings.spindle.on_delay);
|
|
sys.override.spindle_stop.value = 0; // Clear stop override state
|
|
if(grbl.on_override_changed)
|
|
grbl.on_override_changed(OverrideChanged_SpindleState);
|
|
}
|
|
|
|
} else if(sys.step_control.update_spindle_rpm && restore_condition.spindle[0].hal->get_state(restore_condition.spindle[0].hal).on) {
|
|
// Handles spindle state during hold. NOTE: Spindle speed overrides may be altered during hold state.
|
|
state_spindle_restore(&restore_condition.spindle[restore_condition.spindle_num], settings.spindle.on_delay);
|
|
sys.step_control.update_spindle_rpm = Off;
|
|
}
|
|
}
|
|
|
|
// **************
|
|
// State handlers
|
|
// **************
|
|
|
|
/*! /brief No operation handler.
|
|
*/
|
|
static void state_noop (uint_fast16_t rt_exec)
|
|
{
|
|
// Do nothing - state change requests are handled elsewhere or ignored.
|
|
}
|
|
|
|
/*! /brief Waits for idle actions and executes them by switching to the appropriate sys_state.
|
|
*/
|
|
static void state_idle (uint_fast16_t rt_exec)
|
|
{
|
|
if ((rt_exec & EXEC_CYCLE_START))
|
|
state_set(STATE_CYCLE);
|
|
|
|
if (rt_exec & EXEC_FEED_HOLD)
|
|
state_set(STATE_HOLD);
|
|
|
|
if ((rt_exec & EXEC_TOOL_CHANGE)) {
|
|
hal.stream.suspend_read(true); // Block reading from input stream until tool change state is acknowledged
|
|
state_set(STATE_TOOL_CHANGE);
|
|
}
|
|
|
|
if (rt_exec & EXEC_SLEEP)
|
|
state_set(STATE_SLEEP);
|
|
}
|
|
|
|
/*! /brief Waits for cycle actions and executes them by switching to the appropriate sys_state.
|
|
*/
|
|
static void state_cycle (uint_fast16_t rt_exec)
|
|
{
|
|
if (rt_exec == EXEC_CYCLE_START)
|
|
return; // no need to perform other tests...
|
|
|
|
if ((rt_exec & EXEC_TOOL_CHANGE))
|
|
hal.stream.suspend_read(true); // Block reading from input stream until tool change state is acknowledged
|
|
|
|
if (rt_exec & EXEC_CYCLE_COMPLETE)
|
|
state_set(gc_state.tool_change ? STATE_TOOL_CHANGE : STATE_IDLE);
|
|
|
|
if (rt_exec & (EXEC_MOTION_CANCEL|EXEC_MOTION_CANCEL_FAST)) {
|
|
st_update_plan_block_parameters(!!(rt_exec & EXEC_MOTION_CANCEL_FAST)); // Notify stepper module to recompute for hold deceleration.
|
|
sys.suspend = true;
|
|
sys.step_control.execute_hold = On; // Initiate suspend state with active flag.
|
|
stateHandler = state_await_motion_cancel;
|
|
}
|
|
|
|
if ((rt_exec & EXEC_FEED_HOLD))
|
|
state_set(STATE_HOLD);
|
|
}
|
|
|
|
/*! /brief Waits for tool change cycle to end then restarts the cycle.
|
|
*/
|
|
static void state_await_toolchanged (uint_fast16_t rt_exec)
|
|
{
|
|
if (rt_exec & EXEC_CYCLE_START) {
|
|
if (!gc_state.tool_change) {
|
|
|
|
if (hal.stream.suspend_read)
|
|
hal.stream.suspend_read(false); // Tool change complete, restore "normal" stream input.
|
|
|
|
if(grbl.on_tool_changed)
|
|
grbl.on_tool_changed(gc_state.tool);
|
|
|
|
system_add_rt_report(Report_Tool);
|
|
}
|
|
pending_state = gc_state.tool_change ? STATE_TOOL_CHANGE : STATE_IDLE;
|
|
state_set(STATE_IDLE);
|
|
state_set(STATE_CYCLE);
|
|
// Force a status report to let the sender know tool change is completed.
|
|
system_set_exec_state_flag(EXEC_STATUS_REPORT);
|
|
}
|
|
}
|
|
|
|
/*! /brief Waits for motion to end to complete then executes actions depending on the current sys_state.
|
|
*/
|
|
static void state_await_motion_cancel (uint_fast16_t rt_exec)
|
|
{
|
|
if (rt_exec & EXEC_CYCLE_COMPLETE) {
|
|
if (sys_state == STATE_JOG) {
|
|
sys.step_control.flags = 0;
|
|
plan_reset();
|
|
st_reset();
|
|
sync_position();
|
|
sys.suspend = false;
|
|
}
|
|
|
|
state_set(pending_state);
|
|
|
|
if(sys.alarm_pending) {
|
|
system_set_exec_alarm(sys.alarm_pending);
|
|
sys.alarm_pending = Alarm_None;
|
|
} else if(gc_state.tool_change)
|
|
state_set(STATE_TOOL_CHANGE);
|
|
}
|
|
}
|
|
|
|
/*! /brief Waits for feed hold to complete then executes actions depending on the current sys_state.
|
|
*/
|
|
static void state_await_hold (uint_fast16_t rt_exec)
|
|
{
|
|
if (rt_exec & EXEC_CYCLE_COMPLETE) {
|
|
|
|
bool handler_changed = false;
|
|
|
|
plan_cycle_reinitialize();
|
|
sys.step_control.flags = 0;
|
|
|
|
if (sys.alarm_pending) {
|
|
system_set_exec_alarm(sys.alarm_pending);
|
|
sys.alarm_pending = Alarm_None;
|
|
}
|
|
|
|
switch (sys_state) {
|
|
|
|
case STATE_TOOL_CHANGE:
|
|
spindle_all_off(); // De-energize
|
|
hal.coolant.set_state((coolant_state_t){0}); // De-energize
|
|
break;
|
|
|
|
// Resume door state when parking motion has retracted and door has been closed.
|
|
case STATE_SLEEP:
|
|
case STATE_SAFETY_DOOR:
|
|
// Parking manager. Handles de/re-energizing, switch state checks, and parking motions for
|
|
// the safety door and sleep states.
|
|
|
|
// Handles retraction motions and de-energizing.
|
|
// Ensure any prior spindle stop override is disabled at start of safety door routine.
|
|
sys.override.spindle_stop.value = 0;
|
|
|
|
// Parking requires parking axis homed, the current location not exceeding the
|
|
// parking target location, and laser mode disabled.
|
|
if (settings.parking.flags.enabled && !sys.override.control.parking_disable && settings.mode != Mode_Laser) {
|
|
|
|
// Get current position and store as restore location.
|
|
if (!park.flags.active) {
|
|
park.flags.active = On;
|
|
system_convert_array_steps_to_mpos(park.restore_target, sys.position);
|
|
}
|
|
|
|
// Execute slow pull-out parking retract motion if parking axis is homed and parking target is above restore target.
|
|
if (bit_istrue(sys.homed.mask, bit(settings.parking.axis)) && (park.restore_target[settings.parking.axis] < settings.parking.target)) {
|
|
|
|
bool await_motion;
|
|
|
|
handler_changed = true;
|
|
stateHandler = state_await_waypoint_retract;
|
|
|
|
// Copy current location to park target and calculate retract waypoint if not restarting.
|
|
if(park.flags.restart)
|
|
system_convert_array_steps_to_mpos(park.target, sys.position);
|
|
else {
|
|
memcpy(park.target, park.restore_target, sizeof(park.target));
|
|
park.retract_waypoint = settings.parking.pullout_increment + park.target[settings.parking.axis];
|
|
park.retract_waypoint = min(park.retract_waypoint, settings.parking.target);
|
|
}
|
|
|
|
// Retract by pullout distance. Ensure retraction motion moves away from
|
|
// the workpiece and waypoint motion doesn't exceed the parking target location.
|
|
if ((await_motion = park.target[settings.parking.axis] < park.retract_waypoint)) {
|
|
park.target[settings.parking.axis] = park.retract_waypoint;
|
|
park.plan_data.feed_rate = settings.parking.pullout_rate;
|
|
park.plan_data.condition.coolant = restore_condition.coolant; // Retain coolant state
|
|
park.plan_data.spindle.state = restore_condition.spindle[restore_condition.spindle_num].state; // Retain spindle state
|
|
park.plan_data.spindle.hal = restore_condition.spindle[restore_condition.spindle_num].hal;
|
|
park.plan_data.spindle.rpm = restore_condition.spindle[restore_condition.spindle_num].rpm;
|
|
await_motion = mc_parking_motion(park.target, &park.plan_data);
|
|
}
|
|
|
|
if(!park.flags.restart)
|
|
park.flags.motion = await_motion;
|
|
|
|
if (!await_motion)
|
|
stateHandler(EXEC_CYCLE_COMPLETE); // No motion, proceed to next step immediately.
|
|
|
|
} else {
|
|
// Parking motion not possible. Just disable the spindle and coolant.
|
|
// NOTE: Laser mode does not start a parking motion to ensure the laser stops immediately.
|
|
spindle_all_off(); // De-energize
|
|
if(sys.flags.is_parking || sys_state == STATE_SLEEP || !settings.safety_door.flags.keep_coolant_on)
|
|
hal.coolant.set_state((coolant_state_t){0}); // De-energize
|
|
sys.parking_state = hal.control.get_state().safety_door_ajar ? Parking_DoorAjar : Parking_DoorClosed;
|
|
}
|
|
} else {
|
|
spindle_all_off(); // De-energize
|
|
if(sys.flags.is_parking || sys_state == STATE_SLEEP || !settings.safety_door.flags.keep_coolant_on)
|
|
hal.coolant.set_state((coolant_state_t){0}); // De-energize
|
|
sys.parking_state = hal.control.get_state().safety_door_ajar ? Parking_DoorAjar : Parking_DoorClosed;
|
|
}
|
|
sys.flags.is_parking = false;
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
if (!handler_changed) {
|
|
if(sys.flags.soft_limit)
|
|
state_set(STATE_IDLE);
|
|
else {
|
|
sys.holding_state = Hold_Complete;
|
|
stateHandler = state_await_resume;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/*! /brief Waits for action to execute when in feed hold state.
|
|
*/
|
|
static void state_await_resume (uint_fast16_t rt_exec)
|
|
{
|
|
if ((rt_exec & EXEC_CYCLE_COMPLETE) && settings.parking.flags.enabled) {
|
|
if (sys.step_control.execute_sys_motion) {
|
|
sys.step_control.execute_sys_motion = Off;
|
|
st_parking_restore_buffer(); // Restore step segment buffer to normal run state.
|
|
}
|
|
sys.parking_state = hal.control.get_state().safety_door_ajar ? Parking_DoorAjar : Parking_DoorClosed;
|
|
if(sys_state == STATE_SLEEP) {
|
|
enter_sleep();
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (rt_exec & EXEC_SLEEP)
|
|
state_set(STATE_SLEEP);
|
|
|
|
if (rt_exec & EXEC_SAFETY_DOOR)
|
|
sys.parking_state = hal.control.get_state().safety_door_ajar ? Parking_DoorAjar : Parking_DoorClosed;
|
|
|
|
else if (rt_exec & EXEC_CYCLE_START) {
|
|
|
|
if (sys_state == STATE_HOLD && !sys.override.spindle_stop.value)
|
|
sys.override.spindle_stop.restore_cycle = On;
|
|
|
|
switch (sys_state) {
|
|
|
|
case STATE_TOOL_CHANGE:
|
|
break;
|
|
|
|
case STATE_SLEEP:
|
|
break;
|
|
|
|
case STATE_SAFETY_DOOR:
|
|
if (park.flags.restart || !hal.control.get_state().safety_door_ajar) {
|
|
|
|
bool await_motion = false;
|
|
stateHandler = state_restore;
|
|
sys.parking_state = Parking_Resuming;
|
|
|
|
// Resume door state when parking motion has retracted and door has been closed.
|
|
if (park.flags.motion) {
|
|
|
|
park.flags.restart = Off;
|
|
|
|
// Execute fast restore motion to the pull-out position.
|
|
// Check to ensure the motion doesn't move below pull-out position.
|
|
if (park.restore_target[settings.parking.axis] <= settings.parking.target) {
|
|
float target[N_AXIS];
|
|
memcpy(target, park.restore_target, sizeof(target));
|
|
target[settings.parking.axis] = park.retract_waypoint;
|
|
park.plan_data.feed_rate = settings.parking.rate;
|
|
await_motion = mc_parking_motion(target, &park.plan_data);
|
|
}
|
|
}
|
|
|
|
if (!await_motion) // No motion, proceed to next step immediately.
|
|
stateHandler(EXEC_CYCLE_COMPLETE);
|
|
}
|
|
break;
|
|
|
|
default:
|
|
if (!settings.flags.restore_after_feed_hold) {
|
|
if (!restore_condition.spindle[restore_condition.spindle_num].hal->get_state(restore_condition.spindle[restore_condition.spindle_num].hal).on)
|
|
gc_spindle_off();
|
|
sys.override.spindle_stop.value = 0; // Clear spindle stop override states
|
|
} else {
|
|
|
|
if (restore_condition.spindle[restore_condition.spindle_num].state.on != restore_condition.spindle[restore_condition.spindle_num].hal->get_state(restore_condition.spindle[restore_condition.spindle_num].hal).on) {
|
|
grbl.report.feedback_message(Message_SpindleRestore);
|
|
state_spindle_restore(&restore_condition.spindle[restore_condition.spindle_num], settings.spindle.on_delay);
|
|
}
|
|
|
|
if (restore_condition.coolant.value != hal.coolant.get_state().value) {
|
|
// NOTE: Laser mode will honor this delay. An exhaust system is often controlled by coolant signals.
|
|
coolant_restore(restore_condition.coolant, settings.coolant.on_delay);
|
|
gc_coolant(restore_condition.coolant);
|
|
}
|
|
|
|
sys.override.spindle_stop.value = 0; // Clear spindle stop override states
|
|
|
|
grbl.report.feedback_message(Message_None);
|
|
}
|
|
break;
|
|
}
|
|
|
|
// Restart cycle
|
|
if (!(sys_state & (STATE_SLEEP|STATE_SAFETY_DOOR))) {
|
|
step_control_t step_control = sys.step_control;
|
|
state_set(STATE_IDLE);
|
|
sys.step_control = step_control;
|
|
state_set(STATE_CYCLE);
|
|
}
|
|
|
|
} else if ((rt_exec & EXEC_DOOR_CLOSED) && !hal.control.get_state().safety_door_ajar)
|
|
sys.parking_state = Parking_DoorClosed;
|
|
}
|
|
|
|
// ********************
|
|
// Safety door handlers
|
|
// ********************
|
|
|
|
/*! /brief Waits until plunge motion abort is completed then calls state_await_hold() to restart retraction.
|
|
state_await_hold() is set to handle the cycle complete event.
|
|
*/
|
|
static void state_await_restart_retract (uint_fast16_t rt_exec)
|
|
{
|
|
if (rt_exec & EXEC_CYCLE_COMPLETE) {
|
|
|
|
if (sys.step_control.execute_sys_motion) {
|
|
sys.step_control.execute_sys_motion = Off;
|
|
st_parking_restore_buffer(); // Restore step segment buffer to normal run state.
|
|
}
|
|
|
|
stateHandler = state_await_hold;
|
|
stateHandler(EXEC_CYCLE_COMPLETE);
|
|
}
|
|
}
|
|
|
|
/*! /brief Sets up a feed hold to abort plunge motion.
|
|
state_await_restart_retract() is set to handle the cycle complete event.
|
|
*/
|
|
static void restart_retract (void)
|
|
{
|
|
grbl.report.feedback_message(Message_SafetyDoorAjar);
|
|
|
|
stateHandler = state_await_restart_retract;
|
|
|
|
park.flags.restart = On;
|
|
sys.parking_state = Parking_Retracting;
|
|
|
|
if (sys.step_control.execute_sys_motion) {
|
|
st_update_plan_block_parameters(false); // Notify stepper module to recompute for hold deceleration.
|
|
sys.step_control.execute_hold = On;
|
|
sys.step_control.execute_sys_motion = On;
|
|
} else // else NO_MOTION is active.
|
|
stateHandler(EXEC_CYCLE_COMPLETE);
|
|
}
|
|
|
|
/*! /brief Waits until slow plunge motion is completed then deenergize spindle and coolant and execute fast retract motion.
|
|
state_await_resume() is set to handle the cycle complete event.
|
|
*/
|
|
static void state_await_waypoint_retract (uint_fast16_t rt_exec)
|
|
{
|
|
if (rt_exec & EXEC_CYCLE_COMPLETE) {
|
|
|
|
bool await_motion = false;
|
|
|
|
if (sys.step_control.execute_sys_motion) {
|
|
sys.step_control.execute_sys_motion = Off;
|
|
st_parking_restore_buffer(); // Restore step segment buffer to normal run state.
|
|
}
|
|
|
|
// NOTE: Clear accessory state after retract and after an aborted restore motion.
|
|
park.plan_data.spindle.state.value = 0;
|
|
park.plan_data.spindle.rpm = 0.0f;
|
|
park.plan_data.spindle.hal->set_state(park.plan_data.spindle.hal, park.plan_data.spindle.state, 0.0f); // De-energize
|
|
|
|
if (!settings.safety_door.flags.keep_coolant_on) {
|
|
park.plan_data.condition.coolant.value = 0;
|
|
hal.coolant.set_state(park.plan_data.condition.coolant); // De-energize
|
|
}
|
|
|
|
stateHandler = state_await_resume;
|
|
|
|
// Execute fast parking retract motion to parking target location.
|
|
if (park.flags.motion && park.target[settings.parking.axis] < settings.parking.target) {
|
|
float target[N_AXIS];
|
|
memcpy(target, park.target, sizeof(target));
|
|
target[settings.parking.axis] = settings.parking.target;
|
|
park.plan_data.feed_rate = settings.parking.rate;
|
|
await_motion = mc_parking_motion(target, &park.plan_data);
|
|
}
|
|
|
|
if (!await_motion)
|
|
stateHandler(EXEC_CYCLE_COMPLETE);
|
|
}
|
|
}
|
|
|
|
/*! /brief Waits until fast plunge motion is completed then restore spindle and coolant and execute slow plunge motion.
|
|
state_await_resumed() is set to handle the cycle complete event.
|
|
Note: A safety door event during restoration or motion will halt it and restart the retract sequence.
|
|
*/
|
|
static void state_restore (uint_fast16_t rt_exec)
|
|
{
|
|
if (rt_exec & EXEC_SAFETY_DOOR) {
|
|
if(park.flags.restoring)
|
|
park.flags.restart = On;
|
|
else
|
|
restart_retract();
|
|
}
|
|
|
|
else if (rt_exec & EXEC_CYCLE_COMPLETE) {
|
|
|
|
bool await_motion = false;
|
|
|
|
if (sys.step_control.execute_sys_motion) {
|
|
sys.step_control.execute_sys_motion = Off;
|
|
st_parking_restore_buffer(); // Restore step segment buffer to normal run state.
|
|
}
|
|
|
|
park.flags.restart = Off;
|
|
stateHandler = state_await_resumed;
|
|
|
|
// Restart spindle and coolant, delay to power-up.
|
|
state_restore_conditions(&restore_condition);
|
|
|
|
if(park.flags.restart) {
|
|
// Restart flag was set by a safety door event during
|
|
// conditions restore so restart retract.
|
|
restart_retract();
|
|
return;
|
|
}
|
|
|
|
if (park.flags.motion) {
|
|
|
|
sys.parking_state = Parking_Resuming;
|
|
|
|
// Execute slow plunge motion from pull-out position to resume position.
|
|
|
|
// Regardless if the retract parking motion was a valid/safe motion or not, the
|
|
// restore parking motion should logically be valid, either by returning to the
|
|
// original position through valid machine space or by not moving at all.
|
|
park.plan_data.feed_rate = settings.parking.pullout_rate;
|
|
park.plan_data.condition.coolant = restore_condition.coolant;
|
|
park.plan_data.spindle.state = restore_condition.spindle[restore_condition.spindle_num].state;
|
|
park.plan_data.spindle.rpm = restore_condition.spindle[restore_condition.spindle_num].rpm;
|
|
await_motion = mc_parking_motion(park.restore_target, &park.plan_data);
|
|
}
|
|
|
|
if (!await_motion)
|
|
stateHandler(EXEC_CYCLE_COMPLETE); // No motion, proceed to next step immediately.
|
|
}
|
|
}
|
|
|
|
/*! /brief Waits until slow plunge motion is complete then restart the cycle.
|
|
Note: A safety door event during the motion will halt it and restart the retract sequence.
|
|
*/
|
|
static void state_await_resumed (uint_fast16_t rt_exec)
|
|
{
|
|
if (rt_exec & EXEC_SAFETY_DOOR)
|
|
restart_retract();
|
|
|
|
else if (rt_exec & EXEC_CYCLE_COMPLETE) {
|
|
sys.parking_state = Parking_DoorClosed;
|
|
park.flags.value = 0;
|
|
if (sys.step_control.execute_sys_motion) {
|
|
sys.step_control.flags = 0;
|
|
st_parking_restore_buffer(); // Restore step segment buffer to normal run state.
|
|
}
|
|
state_set(STATE_IDLE);
|
|
state_set(STATE_CYCLE);
|
|
}
|
|
}
|