From 8e5d0384dc09f25ee290d73dae4fb043bb544ed9 Mon Sep 17 00:00:00 2001 From: Alden Hart Date: Wed, 12 Apr 2017 07:48:51 -0400 Subject: [PATCH] Fixed case where Z movement was not transferred back to p1 planner in event of a job kill. See cycle_feedhold.cpp, line 450; Comments and some cleanup --- g2core/cycle_feedhold.cpp | 121 +++++++++++++++++--------------------- g2core/g2core.cppproj | 4 +- g2core/plan_exec.cpp | 2 +- 3 files changed, 58 insertions(+), 69 deletions(-) diff --git a/g2core/cycle_feedhold.cpp b/g2core/cycle_feedhold.cpp index 1517ffea..311c2aef 100644 --- a/g2core/cycle_feedhold.cpp +++ b/g2core/cycle_feedhold.cpp @@ -273,7 +273,7 @@ stat_t cm_operation_runner_callback() _start_cycle_restart(); } - // run the operation or operation continuation + // run the operation or operation continuation (callback) return (op.run_operation()); } @@ -305,7 +305,6 @@ stat_t cm_feedhold_command_blocker() static stat_t _run_program_stop() { cm_cycle_end(); // end cycle and run program stop -// cm_program_stop(); return (STAT_OK); } @@ -315,20 +314,9 @@ static stat_t _run_program_end() return (STAT_OK); } -static stat_t _run_alarm() -{ - return (STAT_OK); -} - -static stat_t _run_shutdown() -{ - return (STAT_OK); -} - -static stat_t _run_interlock() -{ - return (STAT_OK); -} +static stat_t _run_alarm() { return (STAT_OK); } +static stat_t _run_shutdown() { return (STAT_OK); } +static stat_t _run_interlock() { return (STAT_OK); } /**************************************************************************************** * cm_request_cycle_start() - set request enum only @@ -377,11 +365,11 @@ static void _start_cycle_restart() /**************************************************************************************** * cm_request_queue_flush() - set request enum only - * _start_queue_flush() - run a queue flush from a % - * _restart_flush() - run a queue flush from an action + * _start_queue_flush() - run a queue flush from a % + * _run_queue_flush() - run a queue flush from an action * - * cm_request_queue_flush() should be called concurrently with xio_flush_to_command() - * cm_request_queue_flush(); xio_flush_to_command(); + * cm_request_queue_flush() should be called concurrently with xio_flush_to_command(), like this: + * { cm_request_queue_flush(); xio_flush_to_command(); } */ void cm_request_queue_flush() @@ -408,6 +396,10 @@ static void _start_queue_flush() } } +// _run_queue_flush() should not be called until motion has stopped. +// It is completely synchronous so it can be called directly; +// it does not need to be part of an operation(). + static stat_t _run_queue_flush() // typically runs from cm1 planner { cm_abort_arc(cm); // kill arcs so they don't just create more alines @@ -420,11 +412,11 @@ static stat_t _run_queue_flush() // typically runs from cm1 planner /**************************************************************************************** * cm_request_job_kill() - Control-D handler - set request flag only by ^d - * _run_job_kill_final() - perform the job kill. queue flush, enter alarm with no movement + * _run_job_kill() - perform the job kill. queue flush, program_end * _start_job_kill() - invoke the job kill function, which may start from various states * - * cm_request_job_kill() should be called concurrently with xio_flush_to_command() - * cm_request_job_kill(); xio_flush_to_command(); + * cm_request_job_kill() should be called concurrently with xio_flush_to_command(), like this: + * { cm_request_job_kill(); xio_flush_to_command(); } * * Job kill cases: Actions: * (0) job kill from ALARM, SHUTDOWN, PANIC no action, end request @@ -443,22 +435,30 @@ void cm_request_job_kill() cm1.job_kill_state = JOB_KILL_REQUESTED; } -// _run_job_kill() is completely synchronous so it can be called -// directly and does not need to be part of an opertion(). +// _run_job_kill() should not be called until motion has stopped. +// It is completely synchronous so it can be called directly; +// it does not need to be part of an operation(). static stat_t _run_job_kill() { - // switch to p1 (may already be in it) - cm = &cm1; // return to primary planner (p1) - mp = (mpPlanner_t *)cm->mp; // cm->mp is a void pointer - mr = mp->mr; + // if in p2 switch to p1 and copy actual position back to p1 + if (cm == &cm2) { + cm = &cm1; // return to primary planner (p1) + mp = (mpPlanner_t *)cm->mp; // cm->mp is a void pointer + mr = mp->mr; + + copy_vector(cm1.gmx.position, mr2.position); // transfer actual position back to p1 + copy_vector(cm1.gm.target, mr2.position); + copy_vector(mp1.position, mr2.position); + copy_vector(mr1.position, mr2.position); + } _run_queue_flush(); coolant_control_immediate(COOLANT_OFF, COOLANT_BOTH); // stop coolant spindle_control_immediate(SPINDLE_OFF); // stop spindle - cm_set_motion_state(MOTION_STOP); // set to stop and set the active model + cm_set_motion_state(MOTION_STOP); // set to stop and set the active model cm->hold_state = FEEDHOLD_OFF; cm_program_end(); @@ -480,14 +480,10 @@ static void _start_job_kill() return; } case MACHINE_CYCLE: { // Case 2's -// if (cm1.motion_state == MOTION_RUN) { // +++++ bandaid - if (cm1.hold_state == FEEDHOLD_OFF) { // Case 2a - in cycle and not in a hold - op.add_action(_feedhold_no_actions); - // op.add_action(_run_job_kill); - } -// } else { -// cm1.hold_state = FEEDHOLD_HOLD; // +++++ bandaid -// } + if (cm1.hold_state == FEEDHOLD_OFF) { // Case 2a - in cycle and not in a hold + op.add_action(_feedhold_no_actions); +// op.add_action(_run_job_kill); + } if (cm1.hold_state == FEEDHOLD_HOLD) { // Case 2c - in a finished hold _run_job_kill(); } @@ -513,11 +509,10 @@ void cm_request_feedhold(cmFeedholdType type, cmFeedholdExit exit) { // Can only initiate a feedhold if you are in a machining cycle, running, and not already in a feedhold + // +++++ This needs to be extended to allow HOLDs to be requested when motion has stopped +++++ if ((cm1.hold_state == FEEDHOLD_OFF) && (cm1.machine_state == MACHINE_CYCLE) && (cm1.motion_state == MOTION_RUN)) { -// if ((cm1.hold_state == FEEDHOLD_OFF) && (cm1.machine_state == MACHINE_CYCLE)) { - cm1.hold_type = type; cm1.hold_exit = exit; cm1.hold_profile = ((type == FEEDHOLD_TYPE_ACTIONS) || (type == FEEDHOLD_TYPE_HOLD)) ? @@ -537,7 +532,6 @@ void cm_request_feedhold(cmFeedholdType type, cmFeedholdExit exit) case FEEDHOLD_EXIT_INTERLOCK: { op.add_action(_run_interlock); break; } default: {} } -// cm1.hold_state = FEEDHOLD_SYNC; // may be redundant, unless default {} was triggered return; } @@ -555,7 +549,7 @@ void cm_request_feedhold(cmFeedholdType type, cmFeedholdExit exit) } /* -static void _start_feedhold() +static void _start_p2_feedhold() { // P2 feedholds only allow skip types if ((cm2.hold_state == FEEDHOLD_REQUESTED) && (cm2.motion_state == MOTION_RUN)) { @@ -610,22 +604,16 @@ static stat_t _feedhold_no_actions() // initiate the feedhold if (cm1.hold_state == FEEDHOLD_OFF) { // start a feedhold cm1.hold_type = FEEDHOLD_TYPE_HOLD; -// cm1.hold_exit = FEEDHOLD_EXIT_STOP; // default exit for NO_ACTIONS is STOP... -//+++ add these lines - if (cm1.motion_state == MOTION_RUN) { - cm1.hold_state = FEEDHOLD_SYNC; // ... STOP can be overridden by setting hold_exit after this function - } else { - _check_motion_stopped(); - cm->hold_state = FEEDHOLD_HOLD; - } -// cm1.hold_state = FEEDHOLD_SYNC; // ... STOP can be overridden by setting hold_exit after this function -//++++ to here - } +// cm1.hold_exit = FEEDHOLD_EXIT_STOP; // default exit for NO_ACTIONS is STOP... - // if motion has already stopped declare that you are in a feedhold -// if (cm1.motion_state == MOTION_STOP) { -// _check_motion_stopped(); -// } + if (cm1.motion_state == MOTION_STOP) { // if motion has already stopped declare that you are in a feedhold + _check_motion_stopped(); + cm1.hold_state = FEEDHOLD_HOLD; + } else { + cm1.hold_state = FEEDHOLD_SYNC; // ... STOP can be overridden by setting hold_exit after this function + return (STAT_EAGAIN); + } + } // wait until feedhold reaches the hold point if (cm1.hold_state < FEEDHOLD_MOTION_STOPPED) { @@ -649,17 +637,17 @@ static stat_t _feedhold_with_actions() // Execute Case (5) // if entered while OFF start a feedhold if (cm1.hold_state == FEEDHOLD_OFF) { cm1.hold_type = FEEDHOLD_TYPE_ACTIONS; -// cm1.hold_exit = FEEDHOLD_EXIT_STOP; // default exit for ACTIONS is STOP... - cm1.hold_state = FEEDHOLD_SYNC; // ... STOP can be overridden by setting hold_exit after this function - return (STAT_EAGAIN); +// cm1.hold_exit = FEEDHOLD_EXIT_STOP; // default exit for ACTIONS is STOP... + if (cm1.motion_state == MOTION_STOP) { // if motion has already stopped declare that you are in a feedhold + _check_motion_stopped(); + cm1.hold_state = FEEDHOLD_HOLD; + } else { + cm1.hold_state = FEEDHOLD_SYNC; // ... STOP can be overridden by setting hold_exit after this function + return (STAT_EAGAIN); + } } - // if motion has already stopped declare that you are in a feedhold -// if (cm1.motion_state == MOTION_STOP) { -// _check_motion_stopped(); -// } - - // Check to run first-time code + // Code to run once motion has stopped if (cm1.hold_state == FEEDHOLD_MOTION_STOPPED) { cm->hold_state = FEEDHOLD_HOLD_ACTIONS_PENDING; // next state @@ -731,6 +719,7 @@ static void _feedhold_restart_actions_done_callback(float* vect, bool* flag) sr_request_status_report(SR_REQUEST_IMMEDIATE); } +//+++++ Make this more robust so it handles being called before reaching HOLD state static stat_t _feedhold_restart_no_actions() { if (cm1.hold_state == FEEDHOLD_OFF) { diff --git a/g2core/g2core.cppproj b/g2core/g2core.cppproj index b8ee2360..0d5b8f5b 100644 --- a/g2core/g2core.cppproj +++ b/g2core/g2core.cppproj @@ -73,7 +73,7 @@ SWD com.atmel.avrdbg.tool.atmelice - J41800036434 + J41800030015 Atmel-ICE True @@ -100,7 +100,7 @@ True true - J41800036434 + J41800030015 0x284E0A60 10000000 diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp index a6a261d6..ace7d9cd 100644 --- a/g2core/plan_exec.cpp +++ b/g2core/plan_exec.cpp @@ -1042,7 +1042,7 @@ static stat_t _exec_aline_feedhold(mpBuf_t *bf) } else { // Otherwise setup the block to complete motion (regardless of how hold will ultimately be exited) bf->length = get_axis_vector_length(mr->position, mr->target); // update bf w/remaining length in move bf->block_state = BLOCK_INITIAL_ACTION; // tell _exec to re-use the bf buffer - bf->buffer_state = MP_BUFFER_BACK_PLANNED; // so it can be forward planned again + bf->buffer_state = MP_BUFFER_BACK_PLANNED; // revert from RUNNING so it can be forward planned again bf->plannable = true; // needed so block can be re-planned } mr->reset(); // reset MR for next use and for forward planning