// Block until all buffered steps are executed.
void plan_synchronize()
{
while (plan_get_current_block() || sys.cycle_start) {
protocol_execute_runtime(); // Check and execute run-time commands
if (sys.abort) { return; } // Check for system abort
}
}
// Block until all buffered steps are executed or in a cycle state. Works with feed hold
// during a synchronize call, if it should happen. Also, waits for clean cycle end.
void protocol_buffer_synchronize()
{
// If system is queued, ensure cycle resumes if the auto start flag is present.
protocol_auto_cycle_start();
do {
protocol_execute_realtime(); // Check and execute run-time commands
if (sys.abort) { return; } // Check for system abort
} while (plan_get_current_block() || (sys.state == STATE_CYCLE));
}
// Prints real-time data. This function grabs a real-time snapshot of the stepper subprogram
// and the actual location of the CNC machine. Users may change the following function to their
// specific needs, but the desired real-time data report must be as short as possible. This is
// requires as it minimizes the computational overhead and allows grbl to keep running smoothly,
// especially during g-code programs with fast, short line segments and high frequency reports (5-20Hz).
void report_realtime_status()
{
// **Under construction** Bare-bones status report. Provides real-time machine position relative to
// the system power on location (0,0,0) and work coordinate position (G54 and G92 applied). Eventually
// to be added are distance to go on block, processed block id, and feed rate. Also a settings bitmask
// for a user to select the desired real-time data.
uint8_t i;
int32_t current_position[N_AXIS]; // Copy current state of the system position variable
memcpy(current_position,sys.position,sizeof(sys.position));
float print_position[N_AXIS];
// Report current machine state
switch (sys.state) {
case STATE_IDLE: printPgmString(PSTR("<Idle")); break;
case STATE_QUEUED: printPgmString(PSTR("<Queue")); break;
case STATE_CYCLE: printPgmString(PSTR("<Run")); break;
case STATE_HOLD: printPgmString(PSTR("<Hold")); break;
case STATE_HOMING: printPgmString(PSTR("<Home")); break;
case STATE_ALARM: printPgmString(PSTR("<Alarm")); break;
case STATE_CHECK_MODE: printPgmString(PSTR("<Check")); break;
}
// Report machine position
printPgmString(PSTR(",MPos:"));
for (i=0; i< N_AXIS; i++) {
print_position[i] = current_position[i]/settings.steps_per_mm[i];
if (bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)) { print_position[i] *= INCH_PER_MM; }
printFloat(print_position[i]);
printPgmString(PSTR(","));
}
// Report work position
printPgmString(PSTR("WPos:"));
for (i=0; i< N_AXIS; i++) {
if (bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)) {
print_position[i] -= (gc.coord_system[i]+gc.coord_offset[i])*INCH_PER_MM;
} else {
print_position[i] -= gc.coord_system[i]+gc.coord_offset[i];
}
printFloat(print_position[i]);
if (i < (N_AXIS-1)) { printPgmString(PSTR(",")); }
}
#ifdef USE_LINE_NUMBERS
// Report current line number
printPgmString(PSTR(",Ln:"));
int32_t ln=0;
plan_block_t * pb = plan_get_current_block();
if(pb != NULL) {
ln = pb->line_number;
}
printInteger(ln);
#endif
printPgmString(PSTR(">\r\n"));
}
// Call the stepper interrupt until one block is finished
void sim_stepper() {
//printf("sim_stepper()\n");
block_t *current_block= plan_get_current_block();
// If the block buffer is empty, call the stepper interrupt one last time
// to let it handle sys.cycle_start etc.
if(current_block==NULL) {
interrupt_TIMER2_COMPA_vect();
sim_time+= ISR_INTERVAL;
return;
}
sys.state = STATE_CYCLE;
while(current_block==plan_get_current_block()) {
interrupt_TIMER2_COMPA_vect();
sim_time+= ISR_INTERVAL;
// Check to see if we should print some info
if(step_time>0.0) {
if(sim_time>=next_print_time) {
if(end_of_block) {
end_of_block= 0;
fprintf(step_out_file, "# block number %d\n", block_number);
}
fprintf(step_out_file, "%20.15f, %d, %d, %d\n", sim_time, sys.position[X_AXIS], sys.position[Y_AXIS], sys.position[Z_AXIS]);
// Make sure the simulation time doesn't get ahead of next_print_time
while(next_print_time<sim_time) next_print_time+= step_time;
}
}
}
// always print stepper values at the end of a block
if(step_time>0.0) {
fprintf(step_out_file, "%20.15f, %d, %d, %d\n", sim_time, sys.position[X_AXIS], sys.position[Y_AXIS], sys.position[Z_AXIS]);
end_of_block= 1;
block_number++;
}
}
// Prints real-time data. This function grabs a real-time snapshot of the stepper subprogram
// and the actual location of the CNC machine. Users may change the following function to their
// specific needs, but the desired real-time data report must be as short as possible. This is
// requires as it minimizes the computational overhead and allows grbl to keep running smoothly,
// especially during g-code programs with fast, short line segments and high frequency reports (5-20Hz).
void report_realtime_status()
{
// **Under construction** Bare-bones status report. Provides real-time machine position relative to
// the system power on location (0,0,0) and work coordinate position (G54 and G92 applied). Eventually
// to be added are distance to go on block, processed block id, and feed rate. Also a settings bitmask
// for a user to select the desired real-time data.
uint8_t idx;
int32_t current_position[N_AXIS]; // Copy current state of the system position variable
memcpy(current_position,sys.position,sizeof(sys.position));
float print_position[N_AXIS];
// Report current machine state
switch (sys.state) {
case STATE_IDLE: printPgmString(PSTR("<Idle")); break;
case STATE_MOTION_CANCEL: // Report run state.
case STATE_CYCLE: printPgmString(PSTR("<Run")); break;
case STATE_HOLD: printPgmString(PSTR("<Hold")); break;
case STATE_HOMING: printPgmString(PSTR("<Home")); break;
case STATE_ALARM: printPgmString(PSTR("<Alarm")); break;
case STATE_CHECK_MODE: printPgmString(PSTR("<Check")); break;
case STATE_SAFETY_DOOR: printPgmString(PSTR("<Door")); break;
}
// If reporting a position, convert the current step count (current_position) to millimeters.
if (bit_istrue(settings.status_report_mask,(BITFLAG_RT_STATUS_MACHINE_POSITION | BITFLAG_RT_STATUS_WORK_POSITION))) {
system_convert_array_steps_to_mpos(print_position,current_position);
}
// Report machine position
if (bit_istrue(settings.status_report_mask,BITFLAG_RT_STATUS_MACHINE_POSITION)) {
printPgmString(PSTR(",MPos:"));
for (idx=0; idx< N_AXIS; idx++) {
printFloat_CoordValue(print_position[idx]);
if (idx < (N_AXIS-1)) { printPgmString(PSTR(",")); }
}
}
// Report work position
if (bit_istrue(settings.status_report_mask,BITFLAG_RT_STATUS_WORK_POSITION)) {
printPgmString(PSTR(",WPos:"));
for (idx=0; idx< N_AXIS; idx++) {
// Apply work coordinate offsets and tool length offset to current position.
print_position[idx] -= gc_state.coord_system[idx]+gc_state.coord_offset[idx];
if (idx == TOOL_LENGTH_OFFSET_AXIS) { print_position[idx] -= gc_state.tool_length_offset; }
printFloat_CoordValue(print_position[idx]);
if (idx < (N_AXIS-1)) { printPgmString(PSTR(",")); }
}
}
// Returns the number of active blocks are in the planner buffer.
if (bit_istrue(settings.status_report_mask,BITFLAG_RT_STATUS_PLANNER_BUFFER)) {
printPgmString(PSTR(",Buf:"));
print_uint8_base10(plan_get_block_buffer_count());
}
// Report serial read buffer status
if (bit_istrue(settings.status_report_mask,BITFLAG_RT_STATUS_SERIAL_RX)) {
printPgmString(PSTR(",RX:"));
print_uint8_base10(serial_get_rx_buffer_count());
}
#ifdef USE_LINE_NUMBERS
// Report current line number
printPgmString(PSTR(",Ln:"));
int32_t ln=0;
plan_block_t * pb = plan_get_current_block();
if(pb != NULL) {
ln = pb->line_number;
}
printInteger(ln);
#endif
#ifdef REPORT_REALTIME_RATE
// Report realtime rate
printPgmString(PSTR(",F:"));
printFloat_RateValue(st_get_realtime_rate());
#endif
if (bit_istrue(settings.status_report_mask,BITFLAG_RT_STATUS_LIMIT_PINS)) {
printPgmString(PSTR(",Lim:"));
print_unsigned_int8(limits_get_state(),2,N_AXIS);
}
#ifdef REPORT_CONTROL_PIN_STATE
printPgmString(PSTR(",Ctl:"));
print_uint8_base2(CONTROL_PIN & CONTROL_MASK);
#endif
printPgmString(PSTR(">\r\n"));
}
//Main Timer for total movement time and block handling
void __ISR(_TIMER_1_VECTOR, ipl3) _InterruptHandler_TMR1(void)
{
// clear the interrupt flag
mT1ClearIntFlag();
if(current_block == Null)
{
current_block = plan_get_current_block();
if(current_block != Null)
{
if(current_block->activeAxisCount == 3)
{
if(current_block->minStepAxis < N_AXIS) // If they are not all equal // TODO: If any of the step counts are equal we dont need Timer4
{
switch(current_block->minStepAxis) // Need to configure OC Module to be Single Pulse Output and other two OC modules to be continuous pulse
{
case X_AXIS:
BSP_Timer4Start((uint16_t)current_block->steppingFreq[X_AXIS]);// Use Timer4 Interrupt to trigger single output pulse
OpenOC2((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER3_SRC|OC_SINGLE_PULSE), (ReadPeriod3()>>1), ReadPeriod3()); // X_AXIS = Single Pulse
BSP_Timer2Start((uint16_t)current_block->steppingFreq[Y_AXIS]);
OpenOC1((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER2_SRC|OC_CONTINUE_PULSE), (ReadPeriod2()>>1), ReadPeriod2()); // Y_AXIS = Continuous Pulse
BSP_Timer3Start((uint16_t)current_block->steppingFreq[Z_AXIS]);
OpenOC3((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER3_SRC|OC_CONTINUE_PULSE), (ReadPeriod3()>>1), ReadPeriod3()); // Z_AXIS = Continuous Pulse
break;
case Y_AXIS:
BSP_Timer4Start((uint16_t)current_block->steppingFreq[Y_AXIS]);// Use Timer4 Interrupt to trigger single output pulse
OpenOC1((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER3_SRC|OC_SINGLE_PULSE), (ReadPeriod3()>>1), ReadPeriod3()); // Y_AXIS = Single Pulse
BSP_Timer2Start((uint16_t)current_block->steppingFreq[X_AXIS]);
OpenOC2((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER2_SRC|OC_CONTINUE_PULSE), (ReadPeriod2()>>1), ReadPeriod2()); // X_AXIS = Continuous Pulse
BSP_Timer3Start((uint16_t)current_block->steppingFreq[Z_AXIS]);
OpenOC3((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER3_SRC|OC_CONTINUE_PULSE), (ReadPeriod3()>>1), ReadPeriod3()); // Z_AXIS = Continuous Pulse
break;
case Z_AXIS:
BSP_Timer4Start((uint16_t)current_block->steppingFreq[Z_AXIS]);// Use Timer4 Interrupt to trigger single output pulse
OpenOC3((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER3_SRC|OC_SINGLE_PULSE), (ReadPeriod3()>>1), ReadPeriod3()); // Z_AXIS = Single Pulse
BSP_Timer3Start((uint16_t)current_block->steppingFreq[X_AXIS]);
OpenOC2((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER3_SRC|OC_CONTINUE_PULSE), (ReadPeriod3()>>1), ReadPeriod3()); // X_AXIS = Continuous Pulse
BSP_Timer2Start((uint16_t)current_block->steppingFreq[Y_AXIS]);
OpenOC1((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER2_SRC|OC_CONTINUE_PULSE), (ReadPeriod2()>>1), ReadPeriod2()); // Y_AXIS = Continuous Pulse
break;
default:
// error
break;
}
}
else
{
BSP_Timer2Start((uint16_t)current_block->steppingFreq[X_AXIS]); // All Steps Counts are equal so just use on timer
OpenOC1((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER2_SRC|OC_CONTINUE_PULSE), (ReadPeriod2()>>1), ReadPeriod2()); // Y_AXIS = Continuous Pulse
OpenOC2((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER2_SRC|OC_CONTINUE_PULSE), (ReadPeriod2()>>1), ReadPeriod2()); // X_AXIS = Continuous Pulse
OpenOC3((OC_ON|OC_IDLE_STOP|OC_TIMER_MODE16 \
|OC_TIMER2_SRC|OC_CONTINUE_PULSE), (ReadPeriod2()>>1), ReadPeriod2()); // Z_AXIS = Continuous Pulse
}
BSP_AxisEnable(Y_AXIS, current_block->direction_bits[Y_AXIS]);
BSP_AxisEnable(X_AXIS, current_block->direction_bits[X_AXIS]);
BSP_AxisEnable(Z_AXIS, current_block->direction_bits[Z_AXIS]);
}
else if (current_block->activeAxisCount == 2) // 2 Axis Enabled
// Executes run-time commands, when required. This is called from various check points in the main
// program, primarily where there may be a while loop waiting for a buffer to clear space or any
// point where the execution time from the last check point may be more than a fraction of a second.
// This is a way to execute realtime commands asynchronously (aka multitasking) with grbl's g-code
// parsing and planning functions. This function also serves as an interface for the interrupts to
// set the system realtime flags, where only the main program handles them, removing the need to
// define more computationally-expensive volatile variables. This also provides a controlled way to
// execute certain tasks without having two or more instances of the same task, such as the planner
// recalculating the buffer upon a feedhold or override.
// NOTE: The sys_rt_exec_state variable flags are set by any process, step or serial interrupts, pinouts,
// limit switches, or the main program.
void protocol_execute_realtime()
{
uint8_t rt_exec; // Temp variable to avoid calling volatile multiple times.
do { // If system is suspended, suspend loop restarts here.
// Check and execute alarms.
rt_exec = sys_rt_exec_alarm; // Copy volatile sys_rt_exec_alarm.
if (rt_exec) { // Enter only if any bit flag is true
// System alarm. Everything has shutdown by something that has gone severely wrong. Report
// the source of the error to the user. If critical, Grbl disables by entering an infinite
// loop until system reset/abort.
sys.state = STATE_ALARM; // Set system alarm state
if (rt_exec & EXEC_ALARM_HARD_LIMIT) {
report_alarm_message(ALARM_HARD_LIMIT_ERROR);
} else if (rt_exec & EXEC_ALARM_SOFT_LIMIT) {
report_alarm_message(ALARM_SOFT_LIMIT_ERROR);
} else if (rt_exec & EXEC_ALARM_ABORT_CYCLE) {
report_alarm_message(ALARM_ABORT_CYCLE);
} else if (rt_exec & EXEC_ALARM_PROBE_FAIL) {
report_alarm_message(ALARM_PROBE_FAIL);
} else if (rt_exec & EXEC_ALARM_HOMING_FAIL) {
report_alarm_message(ALARM_HOMING_FAIL);
}
// Halt everything upon a critical event flag. Currently hard and soft limits flag this.
if (rt_exec & EXEC_CRITICAL_EVENT) {
report_feedback_message(MESSAGE_CRITICAL_EVENT);
bit_false_atomic(sys_rt_exec_state,EXEC_RESET); // Disable any existing reset
do {
// Nothing. Block EVERYTHING until user issues reset or power cycles. Hard limits
// typically occur while unattended or not paying attention. Gives the user time
// to do what is needed before resetting, like killing the incoming stream. The
// same could be said about soft limits. While the position is not lost, the incoming
// stream could be still engaged and cause a serious crash if it continues afterwards.
// TODO: Allow status reports during a critical alarm. Still need to think about implications of this.
// if (sys_rt_exec_state & EXEC_STATUS_REPORT) {
// report_realtime_status();
// bit_false_atomic(sys_rt_exec_state,EXEC_STATUS_REPORT);
// }
} while (bit_isfalse(sys_rt_exec_state,EXEC_RESET));
}
bit_false_atomic(sys_rt_exec_alarm,0xFF); // Clear all alarm flags
}
// Check amd execute realtime commands
rt_exec = sys_rt_exec_state; // Copy volatile sys_rt_exec_state.
if (rt_exec) { // Enter only if any bit flag is true
// Execute system abort.
if (rt_exec & EXEC_RESET) {
sys.abort = true; // Only place this is set true.
return; // Nothing else to do but exit.
}
// Execute and serial print status
if (rt_exec & EXEC_STATUS_REPORT) {
report_realtime_status();
bit_false_atomic(sys_rt_exec_state,EXEC_STATUS_REPORT);
}
// Execute hold states.
// NOTE: The math involved to calculate the hold should be low enough for most, if not all,
// operational scenarios. Once hold is initiated, the system enters a suspend state to block
// all main program processes until either reset or resumed.
if (rt_exec & (EXEC_MOTION_CANCEL | EXEC_FEED_HOLD | EXEC_SAFETY_DOOR)) {
// TODO: CHECK MODE? How to handle this? Likely nothing, since it only works when IDLE and then resets Grbl.
// State check for allowable states for hold methods.
if ((sys.state == STATE_IDLE) || (sys.state & (STATE_CYCLE | STATE_HOMING | STATE_MOTION_CANCEL | STATE_HOLD | STATE_SAFETY_DOOR))) {
// If in CYCLE state, all hold states immediately initiate a motion HOLD.
if (sys.state == STATE_CYCLE) {
st_update_plan_block_parameters(); // Notify stepper module to recompute for hold deceleration.
sys.suspend = SUSPEND_ENABLE_HOLD; // Initiate holding cycle with flag.
}
// If IDLE, Grbl is not in motion. Simply indicate suspend ready state.
if (sys.state == STATE_IDLE) { sys.suspend = SUSPEND_ENABLE_READY; }
// Execute and flag a motion cancel with deceleration and return to idle. Used primarily by probing cycle
// to halt and cancel the remainder of the motion.
if (rt_exec & EXEC_MOTION_CANCEL) {
// MOTION_CANCEL only occurs during a CYCLE, but a HOLD and SAFETY_DOOR may been initiated beforehand
// to hold the CYCLE. If so, only flag that motion cancel is complete.
if (sys.state == STATE_CYCLE) { sys.state = STATE_MOTION_CANCEL; }
sys.suspend |= SUSPEND_MOTION_CANCEL; // Indicate motion cancel when resuming. Special motion complete.
}
// Execute a feed hold with deceleration, only during cycle.
if (rt_exec & EXEC_FEED_HOLD) {
// Block SAFETY_DOOR state from prematurely changing back to HOLD.
if (bit_isfalse(sys.state,STATE_SAFETY_DOOR)) { sys.state = STATE_HOLD; }
}
// Execute a safety door stop with a feed hold, only during a cycle, and disable spindle/coolant.
// NOTE: Safety door differs from feed holds by stopping everything no matter state, disables powered
// devices (spindle/coolant), and blocks resuming until switch is re-engaged. The power-down is
// executed here, if IDLE, or when the CYCLE completes via the EXEC_CYCLE_STOP flag.
if (rt_exec & EXEC_SAFETY_DOOR) {
report_feedback_message(MESSAGE_SAFETY_DOOR_AJAR);
// If already in active, ready-to-resume HOLD, set CYCLE_STOP flag to force de-energize.
// NOTE: Only temporarily sets the 'rt_exec' variable, not the volatile 'rt_exec_state' variable.
if (sys.suspend & SUSPEND_ENABLE_READY) { bit_true(rt_exec,EXEC_CYCLE_STOP); }
sys.suspend |= SUSPEND_ENERGIZE;
sys.state = STATE_SAFETY_DOOR;
}
}
bit_false_atomic(sys_rt_exec_state,(EXEC_MOTION_CANCEL | EXEC_FEED_HOLD | EXEC_SAFETY_DOOR));
}
// Execute a cycle start by starting the stepper interrupt to begin executing the blocks in queue.
if (rt_exec & EXEC_CYCLE_START) {
// Block if called at same time as the hold commands: feed hold, motion cancel, and safety door.
// Ensures auto-cycle-start doesn't resume a hold without an explicit user-input.
if (!(rt_exec & (EXEC_FEED_HOLD | EXEC_MOTION_CANCEL | EXEC_SAFETY_DOOR))) {
// Cycle start only when IDLE or when a hold is complete and ready to resume.
// NOTE: SAFETY_DOOR is implicitly blocked. It reverts to HOLD when the door is closed.
if ((sys.state == STATE_IDLE) || ((sys.state & (STATE_HOLD | STATE_MOTION_CANCEL)) && (sys.suspend & SUSPEND_ENABLE_READY))) {
// Re-energize powered components, if disabled by SAFETY_DOOR.
if (sys.suspend & SUSPEND_ENERGIZE) {
// Delayed Tasks: Restart spindle and coolant, delay to power-up, then resume cycle.
if (gc_state.modal.spindle != SPINDLE_DISABLE) {
spindle_set_state(gc_state.modal.spindle, gc_state.spindle_speed);
delay_ms(SAFETY_DOOR_SPINDLE_DELAY); // TODO: Blocking function call. Need a non-blocking one eventually.
}
if (gc_state.modal.coolant != COOLANT_DISABLE) {
coolant_set_state(gc_state.modal.coolant);
delay_ms(SAFETY_DOOR_COOLANT_DELAY); // TODO: Blocking function call. Need a non-blocking one eventually.
}
// TODO: Install return to pre-park position.
}
// Start cycle only if queued motions exist in planner buffer and the motion is not canceled.
if (plan_get_current_block() && bit_isfalse(sys.suspend,SUSPEND_MOTION_CANCEL)) {
sys.state = STATE_CYCLE;
st_prep_buffer(); // Initialize step segment buffer before beginning cycle.
st_wake_up();
} else { // Otherwise, do nothing. Set and resume IDLE state.
sys.state = STATE_IDLE;
}
sys.suspend = SUSPEND_DISABLE; // Break suspend state.
}
}
bit_false_atomic(sys_rt_exec_state,EXEC_CYCLE_START);
}
// Reinitializes the cycle plan and stepper system after a feed hold for a resume. Called by
// realtime command execution in the main program, ensuring that the planner re-plans safely.
// NOTE: Bresenham algorithm variables are still maintained through both the planner and stepper
// cycle reinitializations. The stepper path should continue exactly as if nothing has happened.
// NOTE: EXEC_CYCLE_STOP is set by the stepper subsystem when a cycle or feed hold completes.
if (rt_exec & EXEC_CYCLE_STOP) {
if (sys.state & (STATE_HOLD | STATE_SAFETY_DOOR)) {
// Hold complete. Set to indicate ready to resume. Remain in HOLD or DOOR states until user
// has issued a resume command or reset.
if (sys.suspend & SUSPEND_ENERGIZE) { // De-energize system if safety door has been opened.
spindle_stop();
coolant_stop();
// TODO: Install parking motion here.
}
bit_true(sys.suspend,SUSPEND_ENABLE_READY);
} else { // Motion is complete. Includes CYCLE, HOMING, and MOTION_CANCEL states.
sys.suspend = SUSPEND_DISABLE;
sys.state = STATE_IDLE;
}
bit_false_atomic(sys_rt_exec_state,EXEC_CYCLE_STOP);
}
}
// Overrides flag byte (sys.override) and execution should be installed here, since they
// are realtime and require a direct and controlled interface to the main stepper program.
// Reload step segment buffer
if (sys.state & (STATE_CYCLE | STATE_HOLD | STATE_MOTION_CANCEL | STATE_SAFETY_DOOR | STATE_HOMING)) { st_prep_buffer(); }
// If safety door was opened, actively check when safety door is closed and ready to resume.
// NOTE: This unlocks the SAFETY_DOOR state to a HOLD state, such that CYCLE_START can activate a resume.
if (sys.state == STATE_SAFETY_DOOR) {
if (bit_istrue(sys.suspend,SUSPEND_ENABLE_READY)) {
#ifndef DEFAULTS_TRINAMIC
if (!(system_check_safety_door_ajar())) {
sys.state = STATE_HOLD; // Update to HOLD state to indicate door is closed and ready to resume.
}
#endif
}
}
} while(sys.suspend); // Check for system suspend state before exiting.
}
// Prints real-time data. This function grabs a real-time snapshot of the stepper subprogram
// and the actual location of the CNC machine. Users may change the following function to their
// specific needs, but the desired real-time data report must be as short as possible. This is
// requires as it minimizes the computational overhead and allows grbl to keep running smoothly,
// especially during g-code programs with fast, short line segments and high frequency reports (5-20Hz).
void report_realtime_status()
{
// **Under construction** Bare-bones status report. Provides real-time machine position relative to
// the system power on location (0,0,0) and work coordinate position (G54 and G92 applied). Eventually
// to be added are distance to go on block, processed block id, and feed rate. Also a settings bitmask
// for a user to select the desired real-time data.
uint8_t i;
int32_t current_position[N_AXIS]; // Copy current state of the system position variable
memcpy(current_position,sys.position,sizeof(sys.position));
float print_position[N_AXIS];
// Report current machine state
switch (sys.state) {
case STATE_IDLE: printPgmString(PSTR("<Idle")); break;
case STATE_QUEUED: printPgmString(PSTR("<Queue")); break;
case STATE_CYCLE: printPgmString(PSTR("<Run")); break;
case STATE_HOLD: printPgmString(PSTR("<Hold")); break;
case STATE_HOMING: printPgmString(PSTR("<Home")); break;
case STATE_ALARM: printPgmString(PSTR("<Alarm")); break;
case STATE_CHECK_MODE: printPgmString(PSTR("<Check")); break;
}
// Report machine position
if (bit_istrue(settings.status_report_mask,BITFLAG_RT_STATUS_MACHINE_POSITION)) {
printPgmString(PSTR(",MPos:"));
// print_position[X_AXIS] = 0.5*current_position[X_AXIS]/settings.steps_per_mm[X_AXIS];
// print_position[Z_AXIS] = 0.5*current_position[Y_AXIS]/settings.steps_per_mm[Y_AXIS];
// print_position[Y_AXIS] = print_position[X_AXIS]-print_position[Z_AXIS]);
// print_position[X_AXIS] -= print_position[Z_AXIS];
// print_position[Z_AXIS] = current_position[Z_AXIS]/settings.steps_per_mm[Z_AXIS];
for (i=0; i< N_AXIS; i++) {
print_position[i] = current_position[i]/settings.steps_per_mm[i];
printFloat_CoordValue(print_position[i]);
if (i < (N_AXIS-1)) { printPgmString(PSTR(",")); }
}
}
// Report work position
if (bit_istrue(settings.status_report_mask,BITFLAG_RT_STATUS_WORK_POSITION)) {
printPgmString(PSTR(",WPos:"));
for (i=0; i< N_AXIS; i++) {
print_position[i] -= gc_state.coord_system[i]+gc_state.coord_offset[i];
if (i == TOOL_LENGTH_OFFSET_AXIS) { print_position[i] -= gc_state.tool_length_offset; }
printFloat_CoordValue(print_position[i]);
if (i < (N_AXIS-1)) { printPgmString(PSTR(",")); }
}
}
// Returns the number of active blocks are in the planner buffer.
if (bit_istrue(settings.status_report_mask,BITFLAG_RT_STATUS_PLANNER_BUFFER)) {
printPgmString(PSTR(",Buf:"));
print_uint8_base10(plan_get_block_buffer_count());
}
// Report serial read buffer status
if (bit_istrue(settings.status_report_mask,BITFLAG_RT_STATUS_SERIAL_RX)) {
printPgmString(PSTR(",RX:"));
print_uint8_base10(serial_get_rx_buffer_count());
}
#ifdef USE_LINE_NUMBERS
// Report current line number
printPgmString(PSTR(",Ln:"));
int32_t ln=0;
plan_block_t * pb = plan_get_current_block();
if(pb != NULL) {
ln = pb->line_number;
}
printInteger(ln);
#endif
#ifdef REPORT_REALTIME_RATE
// Report realtime rate
printPgmString(PSTR(",F:"));
printFloat_RateValue(st_get_realtime_rate());
#endif
printPgmString(PSTR(">\r\n"));
}
void st_prep_buffer()
{
if (segment_buffer_tail != segment_next_head) // Check if we need to fill the buffer.
{
// Determine if we need to load a new planner block or if the block has been replanned.
if (pl_block == NULL)
{
if( !(pl_block = plan_get_current_block()) ) // Query planner for a queued block
return; // No planner blocks. Exit.
// Check if the segment buffer completed the last planner block. If so, load the Bresenham
// data for the block. If not, we are still mid-block and the velocity profile was updated.
// Increment stepper common data index to store new planner block data.
if ( ++prep.st_block_index == (SEGMENT_BUFFER_SIZE-1) ) { prep.st_block_index = 0; }
// Initialize segment buffer data for generating the segments.
prep.current_speed = sqrt(pl_block->entry_speed_sqr);
//---------------------------------------------------------------------------------------
// Compute the velocity profile of a new planner block based on its entry and exit speeds
double inv_2_accel = 0.5/pl_block->acceleration;
// Compute or recompute velocity profile parameters of the prepped planner block.
prep.accelerate_until = pl_block->millimeters;
prep.exit_speed = plan_get_exec_block_exit_speed();
double exit_speed_sqr = prep.exit_speed*prep.exit_speed;
double intersect_distance = 0.5*(pl_block->millimeters+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr));
if (intersect_distance > 0.0)
{
if (intersect_distance < pl_block->millimeters) // Either trapezoid or triangle types
{
// NOTE: For acceleration-cruise and cruise-only types, following calculation will be 0.0.
prep.decelerate_after = inv_2_accel*(pl_block->nominal_speed_sqr-exit_speed_sqr);
if (prep.decelerate_after < intersect_distance) // Trapezoid type
{
prep.maximum_speed = sqrt(pl_block->nominal_speed_sqr);
if (pl_block->entry_speed_sqr == pl_block->nominal_speed_sqr)
{
// Cruise-deceleration or cruise-only type.
}
else
{
// Full-trapezoid or acceleration-cruise types
prep.accelerate_until -= inv_2_accel*(pl_block->nominal_speed_sqr-pl_block->entry_speed_sqr);
}
}
else
{
// Triangle type
prep.accelerate_until = intersect_distance;
prep.decelerate_after = intersect_distance;
prep.maximum_speed = sqrt(2.0*pl_block->acceleration*intersect_distance+exit_speed_sqr);
}
}
else
{
// Deceleration-only type
prep.decelerate_after = pl_block->millimeters;
prep.maximum_speed = prep.current_speed;
}
}
else
{
// Acceleration-only type
prep.accelerate_until = 0.0;
prep.decelerate_after = 0.0;
prep.maximum_speed = prep.exit_speed;
}
}
if( pl_block->millimeters-prep.accelerate_until )
{
//calc A
segment_up3d_t a_seg;
// Acceleration-cruise, acceleration-deceleration ramp junction, or end of block.
double time_var = 2.0*(pl_block->millimeters-prep.accelerate_until)/(prep.current_speed+prep.maximum_speed);
_st_create_up3d_seg_a( &a_seg, time_var, prep.current_speed, prep.maximum_speed);
//subtract A block distance
_st_subtract_plsteps( &a_seg );
//emit A block
_st_store_up3d_seg( &a_seg );
}
segment_up3d_t d_seg = {0};
if( prep.decelerate_after )
{
//calc D
double time_var = 2.0*(prep.decelerate_after)/(prep.maximum_speed+prep.exit_speed);
_st_create_up3d_seg_a( &d_seg, time_var, prep.maximum_speed, prep.exit_speed);
//subtract D block distance
_st_subtract_plsteps( &d_seg );
}
if( pl_block->steps[0] || pl_block->steps[1] || pl_block->steps[2] )
{
//calc C
segment_up3d_t c_seg;
_st_create_up3d_seg_c( &c_seg, prep.maximum_speed );
//emit C
_st_store_up3d_seg( &c_seg );
}
//emit D block
_st_store_up3d_seg( &d_seg );
pl_block = NULL; // Set pointer to indicate check and load next planner block.
plan_discard_current_block();
}
}
