diff --git a/g2core/canonical_machine.cpp b/g2core/canonical_machine.cpp index 4fbca969..d81375dc 100644 --- a/g2core/canonical_machine.cpp +++ b/g2core/canonical_machine.cpp @@ -948,21 +948,22 @@ static void _exec_offset(float *value, bool *flag) /* * cm_set_position_by_axis() - set the position of a single axis in the model, planner and runtime + * cm_reset_position_to_absolute_position() - set all positions to current absolute position in mr * - * 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. + * 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. * - * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - * !!!!! DO NOT CALL THIS FUNCTION WHILE IN A MACHINING CYCLE !!!!! - * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + * !!!!! DO NOT CALL THESE FUNCTIONS WHILE IN A MACHINING CYCLE !!!!! + * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! * - * More specifically, do not call this function if there are any moves in the planner or - * if the runtime is moving. The system must be quiescent or you will introduce positional - * errors. This is true because the planned / running moves have a different reference frame - * than the one you are now going to set. These functions should only be called during - * initialization sequences and during cycles (such as homing cycles) when you know there - * are no more moves in the planner and that all motion has stopped. - * Use cm_get_runtime_busy() to be sure the system is quiescent. + * More specifically, do not call these functions if there are any moves in the planner or + * if the runtime is moving. The system must be quiescent or you will introduce positional + * errors. This is true because the planned / running moves have a different reference frame + * than the one you are now going to set. These functions should only be called during + * initialization sequences and during cycles (such as homing cycles) when you know there + * are no more moves in the planner and that all motion has stopped. + * You can use cm_get_runtime_busy() to be sure the system is quiescent. */ void cm_set_position_by_axis(const uint8_t axis, const float position) @@ -975,10 +976,6 @@ void cm_set_position_by_axis(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; diff --git a/g2core/canonical_machine.h b/g2core/canonical_machine.h index c517ff3c..fc6f7892 100644 --- a/g2core/canonical_machine.h +++ b/g2core/canonical_machine.h @@ -108,8 +108,7 @@ typedef enum { } cmMotionState; typedef enum { // feedhold state machine -// FEEDHOLD_FLUSH = -2, // set when p2 feedhold is ready to flush p2 queue - FEEDHOLD_EXIT = -1, // set when feedhold is due to exit + FEEDHOLD_P1_EXIT = -1, // set when p1 feedhold is due to exit FEEDHOLD_OFF = 0, // no feedhold in effect FEEDHOLD_REQUESTED, // feedhold has been requested but not started yet FEEDHOLD_SYNC, // start hold - sync to latest aline segment @@ -119,7 +118,8 @@ typedef enum { // feedhold state machine FEEDHOLD_STOPPING, // waiting to complete deceleration once planner motion stops FEEDHOLD_ACTIONS_START, // enter secondary planner and perform feedhold actions (once) FEEDHOLD_ACTIONS_WAIT, // wait for feedhold actions to complete - FEEDHOLD_HOLD // holding (steady state) Must be last state + FEEDHOLD_HOLD, // holding (steady state) Must be last state + FEEDHOLD_P2_EXIT // set when p2 feedhold is finishing } cmFeedholdState; typedef enum { // applies to cm->homing_state @@ -367,8 +367,6 @@ stat_t cm_set_g10_data(const uint8_t P_word, const bool P_flag, // G 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 306bb37a..46b86644 100644 --- a/g2core/cycle_feedhold.cpp +++ b/g2core/cycle_feedhold.cpp @@ -223,20 +223,26 @@ stat_t cm_feedhold_sequencing_callback() _run_p1_hold_entry_actions(); } + // p2 feedhold states - feedhold in feedhold if (cm2.hold_state == FEEDHOLD_REQUESTED) { if (mp_has_runnable_buffer(&mp2)) { cm_set_motion_state(MOTION_HOLD); cm2.hold_state = FEEDHOLD_SYNC; } } - - // queue flush won't run until the hold is complete and all (subsequent) motion has stopped -// if ((cm2.flush_state == FLUSH_REQUESTED) && (mp_runtime_is_idle())) { - if (cm2.flush_state == FLUSH_REQUESTED) { + if (cm2.hold_state == FEEDHOLD_P2_EXIT) { + cm2.hold_state = FEEDHOLD_OFF; +// copy_vector (mr1.position, mr2.position); + cm_reset_position_to_absolute_position(&cm2); // get the absolute position before destroying it +// cm_reset_position_to_absolute_position(&cm1); cm_queue_flush(&cm2); + cm_set_motion_state(MOTION_STOP); + cm_cycle_end(); + sr_request_status_report(SR_REQUEST_IMMEDIATE); return (STAT_OK); } + // queue flush won't run until the hold is complete and all (subsequent) motion has stopped 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); @@ -256,7 +262,7 @@ stat_t cm_feedhold_sequencing_callback() _run_p1_hold_exit_actions(); // runs once only } } - if (cm1.hold_state == FEEDHOLD_EXIT) { + if (cm1.hold_state == FEEDHOLD_P1_EXIT) { return(_finalize_p1_hold_exit()); // run multiple times until actions are complete } return (STAT_OK); @@ -364,13 +370,13 @@ static stat_t _run_p1_hold_exit_actions() // LATER: if value == true return static void _sync_to_p1_hold_exit_actions_done(float* vect, bool* flag) { - cm1.hold_state = FEEDHOLD_EXIT; // penultimate state before transitioning to FEEDHOLD_OFF + cm1.hold_state = FEEDHOLD_P1_EXIT; // penultimate state before transitioning to FEEDHOLD_OFF } static stat_t _finalize_p1_hold_exit() { // skip out if not ready to finalize the exit - if (cm1.hold_state != FEEDHOLD_EXIT) { + if (cm1.hold_state != FEEDHOLD_P1_EXIT) { return (STAT_NOOP); // ??? return (STAT_EAGAIN); } @@ -380,12 +386,9 @@ static stat_t _finalize_p1_hold_exit() mr = mp->mr; // execute this block if a queue flush was performed - // adjust primary planner positions to runtime positions + // adjust p1 planner positions to runtime positions 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; } diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp index 9360c6c1..9a5e90e1 100644 --- a/g2core/plan_exec.cpp +++ b/g2core/plan_exec.cpp @@ -547,8 +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->hold_state = FEEDHOLD_OFF; - cm->flush_state = FLUSH_REQUESTED; +// cm_reset_position_to_absolute_position(&cm2); + cm->hold_state = FEEDHOLD_P2_EXIT; } else { cm->hold_state = FEEDHOLD_ACTIONS_START; // perform Z-lift, spindle, coolant actions }