diff --git a/g2core/canonical_machine.cpp b/g2core/canonical_machine.cpp index a4fd9334..4fbca969 100644 --- a/g2core/canonical_machine.cpp +++ b/g2core/canonical_machine.cpp @@ -190,7 +190,7 @@ void canonical_machine_reset(cmMachine_t *_cm) _cm->homing_state = HOMING_NOT_HOMED; // reset request flags - _cm->queue_flush_state = FLUSH_OFF; + _cm->flush_state = FLUSH_OFF; _cm->end_hold_requested = false; _cm->limit_requested = 0; // resets switch closures that occurred during initialization _cm->safety_interlock_disengaged = 0; // ditto @@ -947,7 +947,7 @@ static void _exec_offset(float *value, bool *flag) } /* - * cm_set_position() - set the position of a single axis in the model, planner and runtime + * cm_set_position_by_axis() - set the position of a single axis in the model, planner and runtime * * This command sets an axis/axes to a position provided as an argument. * This is useful for setting origins for homing, probing, and other operations. @@ -965,7 +965,7 @@ static void _exec_offset(float *value, bool *flag) * Use cm_get_runtime_busy() to be sure the system is quiescent. */ -void cm_set_position(const uint8_t axis, const float position) +void cm_set_position_by_axis(const uint8_t axis, const float position) { // TODO: Interlock involving runtime_busy test cm->gmx.position[axis] = position; @@ -975,6 +975,18 @@ void cm_set_position(const uint8_t axis, const float position) mp_set_steps_to_runtime_position(); } +/* + * cm_reset_position_to_absolute_position() - set all positions to current absolute position in mr + */ + +void cm_reset_position_to_absolute_position(cmMachine_t *_cm) +{ + mpPlanner_t *_mp = (mpPlanner_t *)_cm->mp; + for (uint8_t axis = AXIS_X; axis < AXES; axis++) { + cm_set_position_by_axis(axis, mp_get_runtime_absolute_position(_mp->mr, axis)); + } +} + /*** G28.3 functions and support *** * * cm_set_absolute_origin() - G28.3 - model, planner and queue to runtime diff --git a/g2core/canonical_machine.h b/g2core/canonical_machine.h index ee21aeb8..c517ff3c 100644 --- a/g2core/canonical_machine.h +++ b/g2core/canonical_machine.h @@ -149,7 +149,7 @@ typedef enum { // queue flush state machine FLUSH_OFF = 0, // no queue flush in effect FLUSH_REQUESTED, // flush has been requested but not started yet FLUSH_WAS_RUN // transient state to note that a queue flush has been run -} cmQueueFlushState; +} cmFlushState; /***************************************************************************** * CANONICAL MACHINE STRUCTURES @@ -248,7 +248,7 @@ typedef struct cmMachine { // struct to manage canonical machin cmMotionState motion_state; // momo cmFeedholdState hold_state; // hold: feedhold state machine cmOverrideState mfo_state; // feed override state machine - cmQueueFlushState queue_flush_state; // master queue flush state machine + cmFlushState flush_state; // queue flush state machine uint8_t safety_interlock_disengaged; // set non-zero to start interlock processing (value is input number) uint8_t safety_interlock_reengaged; // set non-zero to end interlock processing (value is input number) @@ -365,7 +365,10 @@ stat_t cm_set_g10_data(const uint8_t P_word, const bool P_flag, // G const uint8_t L_word, const bool L_flag, const float offset[], const bool flag[]); -void cm_set_position(const uint8_t axis, const float position); // set absolute position - single axis +void cm_set_position_by_axis(const uint8_t axis, const float position); // set position to abs pos - single axis +void cm_reset_position_to_absolute_position(cmMachine_t *_cm); // set position to abs pos - all axes +//void cm_set_position(const uint8_t axis, const float position); // set absolute position - single axis +//void cm_set_positions(cmMachine_t *_cm); // set absolute position - all axes stat_t cm_set_absolute_origin(const float origin[], bool flag[]); // G28.3 void cm_set_axis_origin(uint8_t axis, const float position); // G28.3 planner callback diff --git a/g2core/cycle_feedhold.cpp b/g2core/cycle_feedhold.cpp index d294066e..306bb37a 100644 --- a/g2core/cycle_feedhold.cpp +++ b/g2core/cycle_feedhold.cpp @@ -187,9 +187,9 @@ void cm_request_end_hold(void) void cm_request_queue_flush() { // NOTE: this function used to flush input buffers, but this is handled in xio *prior* to queue flush now - if ((cm1.hold_state != FEEDHOLD_OFF) && // don't honor request unless you are in a feedhold - (cm1.queue_flush_state == FLUSH_OFF)) { // ...and only once - cm1.queue_flush_state = FLUSH_REQUESTED; // request planner flush once motion has stopped + if ((cm1.hold_state != FEEDHOLD_OFF) && // don't honor request unless you are in a feedhold + (cm1.flush_state == FLUSH_OFF)) { // ...and only once + cm1.flush_state = FLUSH_REQUESTED; // request planner flush once motion has stopped } } @@ -214,12 +214,12 @@ stat_t cm_feedhold_sequencing_callback() { // invoking a p1 feedhold is a 2 step process - get to the stop, then execute the hold actions if (cm1.hold_state == FEEDHOLD_REQUESTED) { - if (mp_has_runnable_buffer(&mp1)) { // bypass cm_start_hold() to start from here + if (mp_has_runnable_buffer(&mp1)) { // bypass cm_start_hold() to start from here cm_set_motion_state(MOTION_HOLD); - cm1.hold_state = FEEDHOLD_SYNC; // invokes hold from aline execution + cm1.hold_state = FEEDHOLD_SYNC; // invokes hold from aline execution } } - if (cm1.hold_state == FEEDHOLD_ACTIONS_START) { // perform Z lift, spindle & coolant actions + if (cm1.hold_state == FEEDHOLD_ACTIONS_START) { // perform Z lift, spindle & coolant actions _run_p1_hold_entry_actions(); } @@ -231,37 +231,37 @@ stat_t cm_feedhold_sequencing_callback() } // queue flush won't run until the hold is complete and all (subsequent) motion has stopped -/* - if ((cm2.queue_flush_state == FLUSH_REQUESTED) && (mp_runtime_is_idle())) { +// if ((cm2.flush_state == FLUSH_REQUESTED) && (mp_runtime_is_idle())) { + if (cm2.flush_state == FLUSH_REQUESTED) { cm_queue_flush(&cm2); return (STAT_OK); } -*/ - if ((cm1.queue_flush_state == FLUSH_REQUESTED) && - (cm1.hold_state == FEEDHOLD_HOLD) && // only flush once hold is actually holding - (mp_runtime_is_idle())) { // don't flush planner during movement + + if ((cm1.flush_state == FLUSH_REQUESTED) && (cm1.hold_state == FEEDHOLD_HOLD) && + (mp_runtime_is_idle())) { // don't flush planner during movement cm_queue_flush(&cm1); - cm1.end_hold_requested = true; // p1 queue flush always ends the hold + cm1.end_hold_requested = true; // p1 queue flush always ends the hold + qr_request_queue_report(0); // request a queue report, since we've changed the number of buffers available } // exit_hold runs for both ~ and % feedhold ends if (cm1.end_hold_requested) { // Flush must complete before exit_hold runs. Trap possible race condition if flush request was - if (cm1.queue_flush_state == FLUSH_REQUESTED) { // ...received when this callback was running + if (cm1.flush_state == FLUSH_REQUESTED) { // ...received when this callback was running return (STAT_OK); } - if (cm1.hold_state == FEEDHOLD_HOLD) { // don't run end_hold until fully into a hold + if (cm1.hold_state == FEEDHOLD_HOLD) { // don't run end_hold until fully into a hold cm1.end_hold_requested = false; - _run_p1_hold_exit_actions(); // runs once only + _run_p1_hold_exit_actions(); // runs once only } } if (cm1.hold_state == FEEDHOLD_EXIT) { - return(_finalize_p1_hold_exit()); // run multiple times until actions are complete + return(_finalize_p1_hold_exit()); // run multiple times until actions are complete } return (STAT_OK); } - + /*********************************************************************************** * _run_p1_hold_entry_actions() - run actions in p2 that complete the p1 hold * _sync_to_p1_hold_entry_actions_done() - final state change occurs here @@ -287,6 +287,7 @@ static stat_t _run_p1_hold_entry_actions() cm2.hold_state = FEEDHOLD_OFF; cm2.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE; cm2.gm.absolute_override = ABSOLUTE_OVERRIDE_OFF; + cm2.flush_state = FLUSH_OFF; cm2.gm.feed_rate = 0; // clear the target and set the positions to the current hold position @@ -380,11 +381,12 @@ static stat_t _finalize_p1_hold_exit() // execute this block if a queue flush was performed // adjust primary planner positions to runtime positions - if (cm1.queue_flush_state == FLUSH_WAS_RUN) { - for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - cm_set_position(axis, mp_get_runtime_absolute_position(&mr2, axis)); - } - cm1.queue_flush_state = FLUSH_OFF; + if (cm1.flush_state == FLUSH_WAS_RUN) { + cm_reset_position_to_absolute_position(cm); +// for (uint8_t axis = AXIS_X; axis < AXES; axis++) { +// cm_set_position(axis, mp_get_runtime_absolute_position(&mr2, axis)); +// } + cm1.flush_state = FLUSH_OFF; } // resume motion from primary planner or end cycle if no moves in planner @@ -450,14 +452,5 @@ void cm_queue_flush(cmMachine_t *_cm) { cm_abort_arc(_cm); // kill arcs so they don't just create more alines planner_reset((mpPlanner_t *)_cm->mp); // reset primary planner. also resets the mr under the planner - _cm->queue_flush_state = FLUSH_WAS_RUN; - qr_request_queue_report(0); // request a queue report, since we've changed the number of buffers available - -/* - cm_abort_arc(&cm1); // kill arcs so they don't just create more alines - planner_reset((mpPlanner_t *)cm1.mp); // reset primary planner. also resets the mr under the planner - cm1.queue_flush_state = FLUSH_WAS_RUN; - cm1.end_hold_requested = true; // queue flush always ends the hold - qr_request_queue_report(0); // request a queue report, since we've changed the number of buffers available -*/ + _cm->flush_state = FLUSH_WAS_RUN; } diff --git a/g2core/cycle_homing.cpp b/g2core/cycle_homing.cpp index adb1b4ba..b041cbc3 100644 --- a/g2core/cycle_homing.cpp +++ b/g2core/cycle_homing.cpp @@ -321,7 +321,7 @@ static stat_t _homing_axis_setpoint_backoff(int8_t axis) // backoff to zero or static stat_t _homing_axis_set_position(int8_t axis) // set axis zero / max and finish up { if (hm.set_coordinates) { - cm_set_position(axis, hm.setpoint); + cm_set_position_by_axis(axis, hm.setpoint); cm->homed[axis] = true; } else { // handle G28.4 cycle - set position to the point of switch closure diff --git a/g2core/cycle_probing.cpp b/g2core/cycle_probing.cpp index fcee822e..85833094 100644 --- a/g2core/cycle_probing.cpp +++ b/g2core/cycle_probing.cpp @@ -313,7 +313,7 @@ static stat_t _probe_axis_move(const float target[], bool exact_position) { } for (uint8_t axis = AXIS_X; axis < AXES; axis++) { // set all positions - cm_set_position(axis, mp_get_runtime_absolute_position(mr, axis)); + cm_set_position_by_axis(axis, mp_get_runtime_absolute_position(mr, axis)); } // set this BEFORE the motion starts diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp index 65394f47..9360c6c1 100644 --- a/g2core/plan_exec.cpp +++ b/g2core/plan_exec.cpp @@ -547,7 +547,8 @@ stat_t mp_exec_aline(mpBuf_t *bf) if ((cm->cycle_state == CYCLE_HOMING) || (cm->cycle_state == CYCLE_PROBE)) { cm->hold_state = FEEDHOLD_OFF; } else if (cm == &cm2) { // if in p2 hold set up a flush - cm->queue_flush_state = FLUSH_REQUESTED; + cm->hold_state = FEEDHOLD_OFF; + cm->flush_state = FLUSH_REQUESTED; } else { cm->hold_state = FEEDHOLD_ACTIONS_START; // perform Z-lift, spindle, coolant actions } diff --git a/g2core/planner.cpp b/g2core/planner.cpp index d331b828..a7a5877c 100644 --- a/g2core/planner.cpp +++ b/g2core/planner.cpp @@ -255,7 +255,7 @@ void mp_halt_runtime() * still close to the starting point. */ -void mp_set_planner_position(uint8_t axis, const float position) { mp->position[axis] = position; } //+++++ +void mp_set_planner_position(uint8_t axis, const float position) { mp->position[axis] = position; } void mp_set_runtime_position(uint8_t axis, const float position) { mr->position[axis] = position; } void mp_set_steps_to_runtime_position()