diff --git a/g2core/canonical_machine.h b/g2core/canonical_machine.h index 1dbd7aed..564b48ec 100644 --- a/g2core/canonical_machine.h +++ b/g2core/canonical_machine.h @@ -111,7 +111,7 @@ typedef enum { typedef enum { // feedhold type parameter FEEDHOLD_TYPE_ACTIONS = 0, // feedhold at max jerk with actions FEEDHOLD_TYPE_NO_ACTIONS, // feedhold at max jerk with no actions - FEEDHOLD_TYPE_SYNC, // feedhold at max jerk with queue flush and sync command + FEEDHOLD_TYPE_COMMAND, // feedhold at max jerk with queue flush and sync command FEEDHOLD_TYPE_FAST, // feedhold at high jerk with no actions. Can resume FEEDHOLD_TYPE_SCRAM // feedhold at high jerk and stop all active devices } cmFeedholdType; diff --git a/g2core/cycle_feedhold.cpp b/g2core/cycle_feedhold.cpp index ccc0e563..d77cabf0 100644 --- a/g2core/cycle_feedhold.cpp +++ b/g2core/cycle_feedhold.cpp @@ -44,9 +44,9 @@ static void _start_queue_flush(void); static void _start_job_kill(void); // Feedhold actions -static stat_t _feedhold_with_actions(void); +static stat_t _feedhold_with_command(void); static stat_t _feedhold_no_actions(void); -static stat_t _feedhold_with_sync(void); +static stat_t _feedhold_with_actions(void); static stat_t _feedhold_restart_with_actions(void); static stat_t _feedhold_restart_no_actions(void); @@ -508,10 +508,9 @@ static void _start_job_kill() void cm_request_feedhold(cmFeedholdType type, cmFeedholdExit exit) { // look for p2 feedhold (feedhold in a feedhold) - if ((cm1.hold_state == FEEDHOLD_HOLD) && - (cm2.hold_state == FEEDHOLD_OFF) && + if ((cm1.hold_state == FEEDHOLD_HOLD) && (cm2.hold_state == FEEDHOLD_OFF) && (cm2.machine_state == MACHINE_CYCLE)) { - op.add_action(_feedhold_with_sync); + op.add_action(_feedhold_with_command); op.add_action(_run_program_stop); cm2.hold_state = FEEDHOLD_SYNC; return; @@ -536,7 +535,7 @@ static void _start_p1_feedhold() switch (cm1.hold_type) { case FEEDHOLD_TYPE_ACTIONS: { op.add_action(_feedhold_with_actions); break; } case FEEDHOLD_TYPE_NO_ACTIONS: { op.add_action(_feedhold_no_actions); break; } - case FEEDHOLD_TYPE_SYNC: { op.add_action(_feedhold_with_sync); break; } + case FEEDHOLD_TYPE_COMMAND: { op.add_action(_feedhold_with_command); break; } default: { } } switch (cm1.hold_exit) { @@ -553,12 +552,12 @@ static void _start_p1_feedhold() // P2 feedholds only allow feedhold sync types if ((cm2.hold_state == FEEDHOLD_REQUESTED) && (cm2.motion_state == MOTION_RUN)) { - op.add_action(_feedhold_with_sync); + op.add_action(_feedhold_with_command); cm2.hold_state = FEEDHOLD_SYNC; } } -static stat_t _feedhold_with_sync() +static stat_t _feedhold_with_command() { if (cm1.hold_state == FEEDHOLD_OFF) { // start a feedhold cm1.hold_state = FEEDHOLD_SYNC; @@ -567,6 +566,7 @@ static stat_t _feedhold_with_sync() return (STAT_EAGAIN); } cm1.hold_state = FEEDHOLD_HOLD; + st_request_exec_move(); return (STAT_OK); } diff --git a/g2core/cycle_probing.cpp b/g2core/cycle_probing.cpp index 87181a07..08ec8843 100644 --- a/g2core/cycle_probing.cpp +++ b/g2core/cycle_probing.cpp @@ -36,6 +36,7 @@ #include "report.h" #include "gpio.h" #include "planner.h" +#include "stepper.h" #include "util.h" #include "xio.h" @@ -193,6 +194,29 @@ uint8_t cm_probing_cycle_callback(void) return (pb.func()); // execute the current probing move } +/*********************************************************************************** + * _probe_move() - function to execute probing moves + * _motion_end_callback() - callback completes when motion has stopped + * + * target[] must be provided in machine canonical coordinates (absolute, mm) + * cm_set_absolute_override() also zeros work offsets, which are restored on exit. + */ + +static void _motion_end_callback(float* vect, bool* flag) +{ + pb.wait_for_motion_end = false; +} + +static stat_t _probe_move(const float target[], const bool flags[]) +{ + cm_set_absolute_override(MODEL, ABSOLUTE_OVERRIDE_ON); + pb.wait_for_motion_end = true; // set this BEFORE the motion starts + cm_straight_feed(target, flags, PROFILE_FAST); // NB: feed rate was set earlier, so it's OK + mp_queue_command(_motion_end_callback, nullptr, nullptr); // the last two arguments are ignored anyway + st_request_forward_plan(); //+++++ + return (STAT_EAGAIN); +} + /*********************************************************************************** * _probing_start() - start the probe or skip it if contact is already active */ @@ -268,28 +292,6 @@ static stat_t _probing_backoff() return (STAT_EAGAIN); } -/*********************************************************************************** - * _probe_move() - function to execute probing moves - * _motion_end_callback() - callback completes when motion has stopped - * - * target[] must be provided in machine canonical coordinates (absolute, mm) - * cm_set_absolute_override() also zeros work offsets, which are restored on exit. - */ - -static void _motion_end_callback(float* vect, bool* flag) -{ - pb.wait_for_motion_end = false; -} - -static stat_t _probe_move(const float target[], const bool flags[]) -{ - cm_set_absolute_override(MODEL, ABSOLUTE_OVERRIDE_ON); - pb.wait_for_motion_end = true; // set this BEFORE the motion starts - cm_straight_feed(target, flags, PROFILE_FAST); // NB: feed rate was set earlier, so it's OK - mp_queue_command(_motion_end_callback, nullptr, nullptr); // the last two arguments are ignored anyway - return (STAT_EAGAIN); -} - /*********************************************************************************** * _probe_restore_settings() - helper for both exits * _probing_exception_exit() - exit for probes that hit an exception diff --git a/g2core/gpio.cpp b/g2core/gpio.cpp index 00cb39e6..418c6a6e 100644 --- a/g2core/gpio.cpp +++ b/g2core/gpio.cpp @@ -143,7 +143,7 @@ struct ioDigitalInputExt { if (in->homing_mode) { if (in->edge == INPUT_EDGE_LEADING) { // we only want the leading edge to fire en_take_encoder_snapshot(); - cm_request_feedhold(FEEDHOLD_TYPE_SYNC, FEEDHOLD_EXIT_STOP); + cm_request_feedhold(FEEDHOLD_TYPE_COMMAND, FEEDHOLD_EXIT_STOP); } return; } @@ -153,8 +153,10 @@ struct ioDigitalInputExt { // We want to capture either way. // Probing tests the start condition for the correct direction ahead of time. // If we see any edge, it's the right one. - en_take_encoder_snapshot(); - cm_request_feedhold(FEEDHOLD_TYPE_SYNC, FEEDHOLD_EXIT_STOP); + if ((in->edge == INPUT_EDGE_LEADING) || (in->edge == INPUT_EDGE_TRAILING)) { + en_take_encoder_snapshot(); + cm_request_feedhold(FEEDHOLD_TYPE_COMMAND, FEEDHOLD_EXIT_STOP); + } return; } @@ -163,10 +165,10 @@ struct ioDigitalInputExt { // trigger the action on leading edges if (in->edge == INPUT_EDGE_LEADING) { if (in->action == INPUT_ACTION_STOP) { - cm_request_feedhold(FEEDHOLD_TYPE_SYNC, FEEDHOLD_EXIT_STOP); + cm_request_feedhold(FEEDHOLD_TYPE_COMMAND, FEEDHOLD_EXIT_STOP); } if (in->action == INPUT_ACTION_FAST_STOP) { - cm_request_feedhold(FEEDHOLD_TYPE_SYNC, FEEDHOLD_EXIT_STOP); + cm_request_feedhold(FEEDHOLD_TYPE_COMMAND, FEEDHOLD_EXIT_STOP); } if (in->action == INPUT_ACTION_HALT) { cm_halt(); // hard stop, including spindle, coolant and heaters diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp index 9cb7fb66..bb170678 100644 --- a/g2core/plan_exec.cpp +++ b/g2core/plan_exec.cpp @@ -247,7 +247,7 @@ stat_t mp_exec_move() // It is possible to try to try to exec from a priming planner if coming off a hold // This occurs if new p1 commands (and were held back) arrived while in a hold -// if (mp->planner_state <= PLANNER_PRIMING) { +// if (mp->planner_state <= MP_BUFFER_BACK_PLANNED) { // st_prep_null(); // return (STAT_NOOP); // } @@ -266,7 +266,7 @@ stat_t mp_exec_move() } if (bf->block_type == BLOCK_TYPE_ALINE) { // cycle auto-start for lines only - +// if ((bf->block_type == BLOCK_TYPE_ALINE) || (bf->block_type == BLOCK_TYPE_COMMAND)) { // first-time operations if (bf->buffer_state != MP_BUFFER_RUNNING) { if ((bf->buffer_state < MP_BUFFER_BACK_PLANNED) && (cm->motion_state == MOTION_RUN)) { @@ -1042,11 +1042,15 @@ static stat_t _exec_aline_feedhold(mpBuf_t *bf) // If probing or homing, exit the move and advance to the _motion_end_callback()'s. // Stop the runtime, clear the run buffer and do not transition to p2 planner. - else if ((cm->cycle_type == CYCLE_HOMING) || (cm->cycle_type == CYCLE_PROBE)) { -// else if (cm->hold_type == FEEDHOLD_TYPE_SYNC) { +// else if ((cm->cycle_type == CYCLE_HOMING) || (cm->cycle_type == CYCLE_PROBE)) { + else if (cm->hold_type == FEEDHOLD_TYPE_COMMAND) { mr->block_state = BLOCK_INACTIVE; // disable the rest of the runtime movement mp_free_run_buffer(); // free buffer and enable finalization move to get loaded - cm->hold_state = FEEDHOLD_OFF; + copy_vector(mp->position, mr->position); + cm->hold_state = FEEDHOLD_HOLD_POINT_REACHED; + +// mp_replan_queue(mp_get_r()); // unplan current forward plan (bf head block), and reset all blocks +// st_request_forward_plan(); // replan the current bf buffer } // In a regular p1 hold. Motion has stopped, so we can rely on positions and other values to be stable diff --git a/g2core/plan_line.cpp b/g2core/plan_line.cpp index 3d814c1b..350c6295 100644 --- a/g2core/plan_line.cpp +++ b/g2core/plan_line.cpp @@ -262,7 +262,6 @@ void mp_plan_block_list() mp->p = mp->p->nx; return; } - bf = _plan_block(bf); // returns next block to plan planned_something = true; mp->p = bf; // DIAGNOSTIC - this is not needed but is set here for debugging purposes diff --git a/g2core/planner.cpp b/g2core/planner.cpp index 943cf30d..9c20f43b 100644 --- a/g2core/planner.cpp +++ b/g2core/planner.cpp @@ -66,16 +66,32 @@ // Allocate planner structures -mpPlanner_t *mp; // currently active planner (global variable) -mpPlanner_t mp1; // primary planning context -mpPlanner_t mp2; // secondary planning context +mpPlanner_t *mp; // currently active planner (global variable) +mpPlanner_t mp1; // primary planning context +mpPlanner_t mp2; // secondary planning context -mpPlannerRuntime_t *mr; // context for planner block runtime -mpPlannerRuntime_t mr1; // primary planner runtime context -mpPlannerRuntime_t mr2; // secondary planner runtime context +mpPlannerRuntime_t *mr; // context for planner block runtime +mpPlannerRuntime_t mr1; // primary planner runtime context +mpPlannerRuntime_t mr2; // secondary planner runtime context -mpBuf_t mp1_queue[PLANNER_QUEUE_SIZE]; // storage allocation for primary planner queue buffers -mpBuf_t mp2_queue[SECONDARY_QUEUE_SIZE]; // storage allocation for secondary planner queue buffers +mpBuf_t mp1_queue[PLANNER_QUEUE_SIZE]; // storage allocation for primary planner queue buffers +mpBuf_t mp2_queue[SECONDARY_QUEUE_SIZE]; // storage allocation for secondary planner queue buffers + +// Local Scope Data and Functions +#define value_vector gm.target // alias for vector of values + +// Execution routines (NB: These are called from the LO interrupt) +static stat_t _exec_dwell(mpBuf_t *bf); +static stat_t _exec_command(mpBuf_t *bf); +static stat_t _exec_json_wait(mpBuf_t *bf); + +// DIAGNOSTICS +//static void _planner_time_accounting(); +static void _audit_buffers(); + +/**************************************************************************************** + * JSON planner objects + */ #define JSON_COMMAND_BUFFER_SIZE 3 @@ -124,23 +140,9 @@ struct _json_commands_t { available++; } }; - _json_commands_t jc; -// Local Scope Data and Functions -#define spindle_speed block_time // local alias for spindle_speed to the time variable -#define value_vector gm.target // alias for vector of values - -//static void _planner_time_accounting(); -static void _audit_buffers(); - -// Execution routines (NB: These are called from the LO interrupt) -static void _exec_json_command(float *value, bool *flag); -static stat_t _exec_dwell(mpBuf_t *bf); -static stat_t _exec_command(mpBuf_t *bf); -static stat_t _exec_json_wait(mpBuf_t *bf); - -/* +/**************************************************************************************** * planner_init() - initialize MP, MR and planner queue buffers * planner_reset() - selective reset MP and MR structures * planner_assert() - test planner assertions, PANIC if violation exists @@ -222,7 +224,7 @@ stat_t planner_assert(const mpPlanner_t *_mp) return (STAT_OK); } -/* +/**************************************************************************************** * mp_halt_runtime() - stop runtime movement immediately */ void mp_halt_runtime() @@ -231,18 +233,18 @@ void mp_halt_runtime() planner_reset(mp); // reset the active planner } -/* +/**************************************************************************************** * mp_set_planner_position() - set planner position for a single axis * mp_set_runtime_position() - set runtime position for a single axis * mp_set_steps_to_runtime_position() - set encoder counts to the runtime position * * Since steps are in motor space you have to run the position vector through inverse - * kinematics to get the right numbers. This means that in a non-Cartesian robot changing - * any position can result in changes to multiple step values. So this operation is provided - * as a single function and always uses the new position vector as an input. + * kinematics to get the right numbers. This means that in a non-Cartesian robot + * changing any position can result in changes to multiple step values. So this operation + * is provided as a single function and always uses the new position vector as an input. * - * Keeping track of position is complicated by the fact that moves exist in several reference - * frames. The scheme to keep this straight is: + * Keeping track of position is complicated by the fact that moves exist in several + * reference frames. The scheme to keep this straight is: * * - mp->position - start and end position for planning * - mr->position - current position of runtime segment @@ -251,9 +253,9 @@ void mp_halt_runtime() * The runtime keeps a lot more data, such as waypoints, step vectors, etc. * See struct mpMoveRuntimeSingleton for details. * - * Note that position is set immediately when called and may not be not an accurate representation - * of the tool position. The motors are still processing the action and the real tool position is - * still close to the starting point. + * Note that position is set immediately when called and may not be not an accurate + * representation of the tool position. The motors are still processing the action + * and the real tool position is still close to the starting point. */ void mp_set_planner_position(uint8_t axis, const float position) { mp->position[axis] = position; } @@ -276,7 +278,7 @@ void mp_set_steps_to_runtime_position() } } -/*********************************************************************************** +/**************************************************************************************** * mp_queue_command() - queue a synchronous Mcode, program control, or other command * _exec_command() - callback to execute command * @@ -332,11 +334,18 @@ stat_t mp_runtime_command(mpBuf_t *bf) return (STAT_OK); } -/*********************************************************************************** +/**************************************************************************************** * mp_json_command() - queue a json command * _exec_json_command() - execute json string */ +static void _exec_json_command(float *value, bool *flag) +{ + char *json_string = jc.read_buffer(); + json_parse_for_exec(json_string, true); // process it + jc.free_buffer(); +} + stat_t mp_json_command(char *json_string) { // Never supposed to fail, since we stopped parsing when we were full @@ -345,15 +354,7 @@ stat_t mp_json_command(char *json_string) return (STAT_OK); } -static void _exec_json_command(float *value, bool *flag) -{ - char *json_string = jc.read_buffer(); - json_parse_for_exec(json_string, true); // process it - jc.free_buffer(); -} - - -/*********************************************************************************** +/**************************************************************************************** * mp_json_wait() - queue a json wait command * _exec_json_wait() - execute json wait string */ @@ -406,8 +407,7 @@ static stat_t _exec_json_wait(mpBuf_t *bf) return (STAT_OK); } - -/*********************************************************************************** +/**************************************************************************************** * mp_dwell() - queue a dwell * _exec_dwell() - dwell execution * @@ -439,7 +439,7 @@ static stat_t _exec_dwell(mpBuf_t *bf) return (STAT_OK); } -/*********************************************************************************** +/**************************************************************************************** * mp_request_out_of_band_dwell() - request a dwell outside of the planner queue * * This command is used to request that a dwell be run outside of the planner. @@ -457,7 +457,7 @@ void mp_request_out_of_band_dwell(float seconds) } } -/*********************************************************************************** +/**************************************************************************************** * Planner helpers * * mp_get_planner_buffers() - return # of available planner buffers @@ -490,7 +490,7 @@ bool mp_is_phat_city_time() return ((mp->plannable_time <= 0.0) || (PHAT_CITY_TIME < mp->plannable_time)); } -/*********************************************************************************** +/**************************************************************************************** * mp_planner_callback() * * mp_planner_callback()'s job is to invoke backward planning intelligently.