From 4d311b0f2a978050e830bcd3617ba37ca24c03fa Mon Sep 17 00:00:00 2001 From: Michael Haberler Date: Fri, 4 Nov 2011 17:21:49 +0100 Subject: [PATCH] M19 orient: add 'wait for orient complete' option Q word: number of seconds to wait until orient completes condition: motion.spindle.is_oriented becomes true within timeout Adapt regression test to use Q-. Thanks to Martin Kaplan for testing. --- configs/sim/orient.hal | 4 +- configs/sim/orient.ui | 32 ++++----------- docs/src/gcode/main.txt | 16 +++++++- src/emc/motion/command.c | 5 ++- src/emc/motion/control.c | 2 + src/emc/motion/motion.h | 11 ++++- src/emc/nml_intf/canon.hh | 1 + src/emc/nml_intf/emc.cc | 20 ++++++++- src/emc/nml_intf/emc.hh | 5 ++- src/emc/nml_intf/emc_nml.hh | 13 ++++++ src/emc/rs274ngc/gcodemodule.cc | 1 + src/emc/rs274ngc/interp_check.cc | 7 ++-- src/emc/rs274ngc/interp_convert.cc | 5 +++ src/emc/rs274ngc/interp_queue.cc | 17 ++++++++ src/emc/rs274ngc/interp_queue.hh | 9 +++- src/emc/sai/saicanon.cc | 5 +++ src/emc/task/emccanon.cc | 9 ++++ src/emc/task/emctaskmain.cc | 66 +++++++++++++++++++++++++++--- src/emc/task/taskintf.cc | 2 + tests/interp/m19/expected | 3 +- tests/interp/m19/test.ngc | 3 +- 21 files changed, 193 insertions(+), 43 deletions(-) diff --git a/configs/sim/orient.hal b/configs/sim/orient.hal index 0c595b405..8c1c13f57 100644 --- a/configs/sim/orient.hal +++ b/configs/sim/orient.hal @@ -6,6 +6,6 @@ net oriented gladevcp.spindle-is-oriented motion.spindle-is-oriented gladevcp.s net locked motion.spindle-locked gladevcp.spindle-locked-led net angle motion.spindle-orient-angle gladevcp.spindle-orient-angle -net fwd motion.spindle-orient-fwd gladevcp.spindle-orient-fwd-led + net brake motion.spindle-brake gladevcp.spindle-brake-led -net fault gladevcp.spindle-orient-fault-s motion.spindle-orient-fault \ No newline at end of file +net fault gladevcp.spindle-orient-fault-s motion.spindle-orient-fault diff --git a/configs/sim/orient.ui b/configs/sim/orient.ui index dd135eb13..a24ce6797 100644 --- a/configs/sim/orient.ui +++ b/configs/sim/orient.ui @@ -1,7 +1,7 @@ - + @@ -87,8 +87,8 @@ True - 1 motion.spindle-orient-angle: %f + 1 2 @@ -96,28 +96,6 @@ 4 - - - True - motion.spindle-orient-fwd: - - - 4 - 5 - - - - - True - 2 - - - 1 - 2 - 4 - 5 - - True @@ -164,6 +142,12 @@ 7 + + + + + + diff --git a/docs/src/gcode/main.txt b/docs/src/gcode/main.txt index f673fc822..4863bcc57 100644 --- a/docs/src/gcode/main.txt +++ b/docs/src/gcode/main.txt @@ -1782,10 +1782,11 @@ regardless of what coolant is on or off. To start spindle orientation mode, program `M19`. - M19 R- P- + M19 R- P- Q- - The optional R- word specifies the desired angle in degrees (default 0). - The optional P- word specifies the rotation direction (default 0). + - The optional Q- word specifies the timeout ins seconds to wait for orient to complete. M19 cancels spindle rotation mode and enters spindle orientation mode. M19 turns off the spindle, unlocks the spindle brake and then starts the @@ -1802,7 +1803,18 @@ Any M3,M4,M5 spindle command will cancel spindle orientation mode and switch back to rotation mode, which is the default. The `[RS274NGC]ORIENT_OFFSET` value from the ini file is added to the -R word to define an aribtrary zero position. +R word to define an arbitrary zero position. + +The Q word commands the spindle orient support to wait for the orient +cycle to complete. If no Q word is given, the orient cycle is +initiated but not waited for in order to allow parallel operations of +e.g. a rapid move and a spindle orient. + +An `M19` command with just a `Q` word, but no `R` and `P` words will +wait for the last M19 cycle to complete, which is considered complete +when the HAL logic asserts `motion.spindle-locked`. + +It is an error to specify a Q value less than 0. M19 is a modal group 7 command (same as M3,M4,M5). diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index ec260f9e3..86057c4d7 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -1499,12 +1499,13 @@ check_stuff ( "before command_handler()" ); case EMCMOT_SPINDLE_ON: rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_ON"); - if (*(emcmot_hal_data->spindle_orient)) + if (*(emcmot_hal_data->spindle_orient)) rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_ORIENT cancelled by SPINDLE_ON"); if (*(emcmot_hal_data->spindle_locked)) rtapi_print_msg(RTAPI_MSG_DBG, "spindle-locked cleared by SPINDLE_ON"); *(emcmot_hal_data->spindle_locked) = 0; *(emcmot_hal_data->spindle_orient) = 0; + emcmotStatus->spindle.orient_state = EMCMOT_ORIENT_NONE; /* if (emcmotStatus->spindle.orient) { */ /* reportError(_("cant turn on spindle during orient in progress")); */ @@ -1535,6 +1536,7 @@ check_stuff ( "before command_handler()" ); rtapi_print_msg(RTAPI_MSG_DBG, "spindle-locked cleared by SPINDLE_OFF"); *(emcmot_hal_data->spindle_locked) = 0; *(emcmot_hal_data->spindle_orient) = 0; + emcmotStatus->spindle.orient_state = EMCMOT_ORIENT_NONE; break; case EMCMOT_SPINDLE_ORIENT: @@ -1548,6 +1550,7 @@ check_stuff ( "before command_handler()" ); /* tpAbort(&emcmotDebug->queue); */ /* SET_MOTION_ERROR_FLAG(1); */ } + emcmotStatus->spindle.orient_state = EMCMOT_ORIENT_IN_PROGRESS; emcmotStatus->spindle.speed = 0; emcmotStatus->spindle.direction = 0; // so far like spindle stop, except opening brake diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 33042a44d..a237a6f10 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -572,6 +572,7 @@ static void process_inputs(void) // signal error, and cancel the orient if (*(emcmot_hal_data->spindle_orient)) { if (*(emcmot_hal_data->spindle_orient_fault)) { + emcmotStatus->spindle.orient_state = EMCMOT_ORIENT_FAULTED; *(emcmot_hal_data->spindle_orient) = 0; emcmotStatus->spindle.orient_fault = *(emcmot_hal_data->spindle_orient_fault); reportError(_("fault %d during orient in progress"), emcmotStatus->spindle.orient_fault); @@ -583,6 +584,7 @@ static void process_inputs(void) *(emcmot_hal_data->spindle_locked) = 1; emcmotStatus->spindle.locked = 1; emcmotStatus->spindle.brake = 1; + emcmotStatus->spindle.orient_state = EMCMOT_ORIENT_COMPLETE; rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_ORIENT complete, spindle locked"); } } diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 72817c0ce..16e794ff6 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -235,6 +235,7 @@ extern "C" { EmcPose tool_offset; /* TLO */ double orientation; /* angle for spindle orient */ char direction; /* CANON_DIRECTION flag for spindle orient */ + double timeout; /* of wait for spindle orient to complete */ unsigned char tail; /* flag count for mutex detect */ } emcmot_command_t; @@ -426,6 +427,13 @@ Suggestion: Split this in to an Error and a Status flag register.. HOME_SEQUENCE_WAIT_JOINTS, } home_sequence_state_t; + typedef enum { + EMCMOT_ORIENT_NONE = 0, + EMCMOT_ORIENT_IN_PROGRESS, + EMCMOT_ORIENT_COMPLETE, + EMCMOT_ORIENT_FAULTED, + } orient_state_t; + /* flags for homing */ #define HOME_IGNORE_LIMITS 1 #define HOME_USE_INDEX 2 @@ -559,7 +567,8 @@ Suggestion: Split this in to an Error and a Status flag register.. int direction; // 0 stopped, 1 forward, -1 reverse int brake; // 0 released, 1 engaged int locked; // spindle lock engaged after orient - int orient_fault; + int orient_fault; // fault code from motion.spindle-orient-fault + int orient_state; // orient_state_t } spindle_status; diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 6254edd8e..eccf7f67f 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -499,6 +499,7 @@ command may be given, but it will have no effect. */ extern void SPINDLE_RETRACT(); extern void ORIENT_SPINDLE(double orientation, int mode); +extern void WAIT_SPINDLE_ORIENT_COMPLETE(double timeout); extern void LOCK_SPINDLE_Z(); extern void USE_SPINDLE_FORCE(); extern void USE_NO_SPINDLE_FORCE(); diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index 386019aec..9caf79f25 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -245,6 +245,9 @@ int emcFormat(NMLTYPE type, void *buffer, CMS * cms) case EMC_SPINDLE_ORIENT_TYPE: ((EMC_SPINDLE_ORIENT *) buffer)->update(cms); break; + case EMC_SPINDLE_WAIT_ORIENT_COMPLETE_TYPE: + ((EMC_SPINDLE_WAIT_ORIENT_COMPLETE *) buffer)->update(cms); + break; case EMC_SPINDLE_STAT_TYPE: ((EMC_SPINDLE_STAT *) buffer)->update(cms); break; @@ -606,6 +609,8 @@ const char *emc_symbol_lookup(long type) return "EMC_SPINDLE_SPEED"; case EMC_SPINDLE_ORIENT_TYPE: return "EMC_SPINDLE_ORIENT"; + case EMC_SPINDLE_WAIT_ORIENT_COMPLETE_TYPE: + return "EMC_SPINDLE_WAIT_ORIENT_COMPLETE"; case EMC_SPINDLE_STAT_TYPE: return "EMC_SPINDLE_STAT"; case EMC_STAT_TYPE: @@ -1457,7 +1462,8 @@ void EMC_SPINDLE_STAT::update(CMS * cms) cms->update(brake); cms->update(increasing); cms->update(enabled); - + cms->update(orient_state); + cms->update(orient_fault); } /* @@ -2114,6 +2120,18 @@ void EMC_SPINDLE_ORIENT::update(CMS * cms) cms->update(mode); } +/* +* NML/CMS Update function for EMC_SPINDLE_WAIT_ORIENT_COMPLETE +* Manually generated by Michael Haberler. +* on Wed Nov 2 18:29:15 CET 2011 +*/ +void EMC_SPINDLE_WAIT_ORIENT_COMPLETE::update(CMS * cms) +{ + + EMC_SPINDLE_CMD_MSG::update(cms); + cms->update(timeout); +} + /* * NML/CMS Update function for EMC_TASK_PLAN_END * Automatically generated by NML CodeGen Java Applet. diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index 29d40efd4..5ced94b97 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -230,6 +230,7 @@ class PM_CARTESIAN; /* removed #define EMC_SPINDLE_DISABLE_TYPE ((NMLTYPE) 1315) */ #define EMC_SPINDLE_SPEED_TYPE ((NMLTYPE) 1316) #define EMC_SPINDLE_ORIENT_TYPE ((NMLTYPE) 1317) +#define EMC_SPINDLE_WAIT_ORIENT_COMPLETE_TYPE ((NMLTYPE) 1318) #define EMC_SPINDLE_STAT_TYPE ((NMLTYPE) 1399) @@ -306,7 +307,8 @@ enum EMC_TASK_EXEC_ENUM { EMC_TASK_EXEC_WAITING_FOR_PAUSE = 6, EMC_TASK_EXEC_WAITING_FOR_MOTION_AND_IO = 7, EMC_TASK_EXEC_WAITING_FOR_DELAY = 8, - EMC_TASK_EXEC_WAITING_FOR_SYSTEM_CMD = 9 + EMC_TASK_EXEC_WAITING_FOR_SYSTEM_CMD = 9, + EMC_TASK_EXEC_WAITING_FOR_SPINDLE_ORIENTED = 10 }; // types for EMC_TASK interpState @@ -527,6 +529,7 @@ extern int emcSpindleAbort(); extern int emcSpindleSpeed(double speed, double factor, double xoffset); extern int emcSpindleOn(double speed, double factor, double xoffset); extern int emcSpindleOrient(double orientation, int direction); +extern int emcSpindleWaitOrientComplete(double timout); extern int emcSpindleOff(); extern int emcSpindleIncrease(); extern int emcSpindleDecrease(); diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index 8aee30433..141beb29a 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -1129,6 +1129,8 @@ class EMC_SPINDLE_STAT:public EMC_SPINDLE_STAT_MSG { int brake; // 0 released, 1 engaged int increasing; // 1 increasing, -1 decreasing, 0 neither int enabled; // non-zero means enabled + int orient_state; + int orient_fault; }; class EMC_MOTION_STAT:public EMC_MOTION_STAT_MSG { @@ -1684,6 +1686,17 @@ class EMC_SPINDLE_ORIENT:public EMC_SPINDLE_CMD_MSG { int mode; }; +class EMC_SPINDLE_WAIT_ORIENT_COMPLETE:public EMC_SPINDLE_CMD_MSG { + public: + EMC_SPINDLE_WAIT_ORIENT_COMPLETE():EMC_SPINDLE_CMD_MSG(EMC_SPINDLE_WAIT_ORIENT_COMPLETE_TYPE, + sizeof(EMC_SPINDLE_WAIT_ORIENT_COMPLETE)) { + }; + + // For internal NML/CMS use only. + void update(CMS * cms); + double timeout; // how long to wait until spindle orient completes; > 0 +}; + class EMC_SPINDLE_ON:public EMC_SPINDLE_CMD_MSG { public: diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 57656b50f..76b8efbc8 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -405,6 +405,7 @@ void SET_SPINDLE_MODE(double) {} void STOP_SPINDLE_TURNING() {} void SET_SPINDLE_SPEED(double rpm) {} void ORIENT_SPINDLE(double d, int i) {} +void WAIT_SPINDLE_ORIENT_COMPLETE(double timeout) {} void PROGRAM_STOP() {} void PROGRAM_END() {} void FINISH() {} diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index 6657bbff5..e6a399e74 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -320,9 +320,10 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block if (block->q_number != -1.0) { CHKS((motion != G_83) && (motion != G_73) && (motion != G_5) && (block->user_m != 1) && (motion != G_76) && - (block->m_modes[5] != 66) && (block->m_modes[5] != 67) && (block->m_modes[5] != 68) && - (block->g_modes[0] != G_10) && (block->m_modes[6] != 61) && (block->g_modes[13] != G_64), - _("Q word with no G5, G10, G64, G73, G76, G83, M66, M67, M68 or user M code that uses it")); + (block->m_modes[5] != 66) && (block->m_modes[5] != 67) && (block->m_modes[5] != 68) && + (block->g_modes[0] != G_10) && (block->m_modes[6] != 61) && (block->g_modes[13] != G_64) && + (block->m_modes[7] != 19), + _("Q word with no G5, G10, G64, G73, G76, G83, M19, M66, M67, M68 or user M code that uses it")); } if (block->r_flag) { diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index ca0d9bfae..2c8cced5f 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -3078,8 +3078,13 @@ int Interp::convert_m(block_pointer block, //!< pointer to a block of RS27 settings->spindle_turning = CANON_STOPPED; } else if ((block->m_modes[7] == 19) && ONCE_M(7)) { settings->spindle_turning = CANON_STOPPED; + if (block->r_flag || block->p_flag) enqueue_ORIENT_SPINDLE(block->r_flag ? (block->r_number + settings->orient_offset) : settings->orient_offset, block->p_flag ? block->p_number : 0); + if (block->q_flag) { + CHKS((block->q_number <= 0.0),(_("Q word with M19 requires a value > 0"))); + enqueue_WAIT_ORIENT_SPINDLE_COMPLETE(block->q_number); + } } else if ((block->m_modes[7] == 70) || (block->m_modes[7] == 73)) { // save state in current stack frame. We borrow the o-word call stack diff --git a/src/emc/rs274ngc/interp_queue.cc b/src/emc/rs274ngc/interp_queue.cc index fcf28ef4c..6f3560692 100644 --- a/src/emc/rs274ngc/interp_queue.cc +++ b/src/emc/rs274ngc/interp_queue.cc @@ -201,6 +201,19 @@ void enqueue_ORIENT_SPINDLE(double orientation, int mode) { qc().push_back(q); } +void enqueue_WAIT_ORIENT_SPINDLE_COMPLETE(double timeout) { + if(qc().empty()) { + if(debug_qc) printf("immediate wait spindle orient complete\n"); + WAIT_SPINDLE_ORIENT_COMPLETE(timeout); + return; + } + queued_canon q; + q.type = QWAIT_ORIENT_SPINDLE_COMPLETE; + q.data.wait_orient_spindle_complete.timeout = timeout; + if(debug_qc) printf("enqueue wait spindle orient complete\n"); + qc().push_back(q); +} + void enqueue_SET_SPINDLE_MODE(double mode) { if(qc().empty()) { if(debug_qc) printf("immediate spindle mode %f\n", mode); @@ -531,6 +544,10 @@ void dequeue_canons(setup_pointer settings) { if(debug_qc) printf("issuing orient spindle\n"); ORIENT_SPINDLE(q.data.orient_spindle.orientation, q.data.orient_spindle.mode); break; + case QWAIT_ORIENT_SPINDLE_COMPLETE: + if(debug_qc) printf("issuing wait orient spindle complete\n"); + WAIT_SPINDLE_ORIENT_COMPLETE(q.data.wait_orient_spindle_complete.timeout); + break; } } qc().clear(); diff --git a/src/emc/rs274ngc/interp_queue.hh b/src/emc/rs274ngc/interp_queue.hh index 0b84307dc..d73475705 100644 --- a/src/emc/rs274ngc/interp_queue.hh +++ b/src/emc/rs274ngc/interp_queue.hh @@ -14,7 +14,8 @@ enum queued_canon_type {QSTRAIGHT_TRAVERSE, QSTRAIGHT_FEED, QARC_FEED, QSET_FEED QMIST_ON, QMIST_OFF, QFLOOD_ON, QFLOOD_OFF, QSTART_SPINDLE_CLOCKWISE, QSTART_SPINDLE_COUNTERCLOCKWISE, QSTOP_SPINDLE_TURNING, QSET_SPINDLE_MODE, QSET_SPINDLE_SPEED, - QCOMMENT, QM_USER_COMMAND,QSTART_CHANGE, QORIENT_SPINDLE}; + QCOMMENT, QM_USER_COMMAND,QSTART_CHANGE, + QORIENT_SPINDLE, QWAIT_ORIENT_SPINDLE_COMPLETE}; struct straight_traverse { int line_number; @@ -71,6 +72,10 @@ struct orient_spindle { int mode; }; +struct wait_orient_spindle_complete { + double timeout; +}; + struct queued_canon { queued_canon_type type; union { @@ -85,6 +90,7 @@ struct queued_canon { struct comment comment; struct mcommand mcommand; struct orient_spindle orient_spindle; + struct wait_orient_spindle_complete wait_orient_spindle_complete; } data; }; @@ -123,6 +129,7 @@ void enqueue_ARC_FEED(setup_pointer settings, int l, void enqueue_M_USER_COMMAND(int index,double p_number,double q_number); void enqueue_START_CHANGE(void); void enqueue_ORIENT_SPINDLE(double orientation, int mode); +void enqueue_WAIT_ORIENT_SPINDLE_COMPLETE(double timeout); void dequeue_canons(setup_pointer settings); void set_endpoint(double x, double y); void set_endpoint_zx(double z, double x); diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index d8ada1f1e..d7a4aa60a 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -609,6 +609,11 @@ void ORIENT_SPINDLE(double orientation, int mode) {PRINT2("ORIENT_SPINDLE(%.4f, %d)\n", orientation,mode); } +void WAIT_SPINDLE_ORIENT_COMPLETE(double timeout) +{ + PRINT1("SPINDLE_WAIT_ORIENT_COMPLETE(%.4f)\n", timeout); +} + void USE_NO_SPINDLE_FORCE() {PRINT0("USE_NO_SPINDLE_FORCE()\n");} diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 5b994be8b..e890b544d 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -1748,6 +1748,15 @@ void ORIENT_SPINDLE(double orientation, int mode) interp_list.append(o); } +void WAIT_SPINDLE_ORIENT_COMPLETE(double timeout) +{ + EMC_SPINDLE_WAIT_ORIENT_COMPLETE o; + + flush_segments(); + o.timeout = timeout; + interp_list.append(o); +} + void USE_SPINDLE_FORCE(void) { /*! \todo FIXME-- unimplemented */ diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index bdb84b799..992b09fec 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -79,6 +79,7 @@ fpu_control_t __fpu_control = _FPU_IEEE & ~(_FPU_MASK_IM | _FPU_MASK_ZM | _FPU_M #include "nml_oi.hh" #include "task.hh" // emcTaskCommand etc #include "taskclass.hh" +#include "motion.h" // EMCMOT_ORIENT_* /* time after which the user interface is declared dead * because it would'nt read any more messages @@ -392,6 +393,7 @@ static EMC_TRAJ_SET_SPINDLESYNC *emcTrajSetSpindlesyncMsg; static EMC_SPINDLE_SPEED *spindle_speed_msg; static EMC_SPINDLE_ORIENT *spindle_orient_msg; +static EMC_SPINDLE_WAIT_ORIENT_COMPLETE *wait_spindle_orient_complete_msg; static EMC_SPINDLE_ON *spindle_on_msg; static EMC_TOOL_PREPARE *tool_prepare_msg; static EMC_TOOL_LOAD_TOOL_TABLE *load_tool_table_msg; @@ -1004,6 +1006,7 @@ static int emcTaskPlan(void) case EMC_TRAJ_SET_SO_ENABLE_TYPE: case EMC_SPINDLE_SPEED_TYPE: case EMC_SPINDLE_ORIENT_TYPE: + case EMC_SPINDLE_WAIT_ORIENT_COMPLETE_TYPE: case EMC_SPINDLE_ON_TYPE: case EMC_SPINDLE_OFF_TYPE: case EMC_SPINDLE_BRAKE_RELEASE_TYPE: @@ -1163,6 +1166,7 @@ static int emcTaskPlan(void) case EMC_TRAJ_SET_SO_ENABLE_TYPE: case EMC_SPINDLE_SPEED_TYPE: case EMC_SPINDLE_ORIENT_TYPE: + case EMC_SPINDLE_WAIT_ORIENT_COMPLETE_TYPE: case EMC_SPINDLE_ON_TYPE: case EMC_SPINDLE_OFF_TYPE: case EMC_SPINDLE_BRAKE_RELEASE_TYPE: @@ -1315,6 +1319,7 @@ static int emcTaskPlan(void) case EMC_TRAJ_SET_SO_ENABLE_TYPE: case EMC_SPINDLE_SPEED_TYPE: case EMC_SPINDLE_ORIENT_TYPE: + case EMC_SPINDLE_WAIT_ORIENT_COMPLETE_TYPE: case EMC_SPINDLE_ON_TYPE: case EMC_SPINDLE_OFF_TYPE: case EMC_SPINDLE_BRAKE_RELEASE_TYPE: @@ -1413,6 +1418,7 @@ static int emcTaskCheckPreconditions(NMLmsg * cmd) case EMC_TRAJ_RIGID_TAP_TYPE: //and this case EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG_TYPE: // and this case EMC_AUX_INPUT_WAIT_TYPE: + case EMC_SPINDLE_WAIT_ORIENT_COMPLETE_TYPE: return EMC_TASK_EXEC_WAITING_FOR_MOTION_AND_IO; break; @@ -1511,6 +1517,7 @@ static int emcTaskCheckPreconditions(NMLmsg * cmd) return EMC_TASK_EXEC_DONE; break; + default: // unrecognized command if (EMC_DEBUG & EMC_DEBUG_TASK_ISSUE) { @@ -1823,6 +1830,11 @@ static int emcTaskIssueCommand(NMLmsg * cmd) } break; + case EMC_SPINDLE_WAIT_ORIENT_COMPLETE_TYPE: + wait_spindle_orient_complete_msg = (EMC_SPINDLE_WAIT_ORIENT_COMPLETE *) cmd; + taskExecDelayTimeout = etime() + wait_spindle_orient_complete_msg->timeout; + break; + case EMC_TRAJ_RIGID_TAP_TYPE: retval = emcTrajRigidTap(((EMC_TRAJ_RIGID_TAP *) cmd)->pos, ((EMC_TRAJ_RIGID_TAP *) cmd)->vel, @@ -1886,7 +1898,7 @@ static int emcTaskIssueCommand(NMLmsg * cmd) retval = emcSpindleOrient(spindle_orient_msg->orientation, spindle_orient_msg->mode); break; - case EMC_SPINDLE_ON_TYPE: + case EMC_SPINDLE_ON_TYPE: spindle_on_msg = (EMC_SPINDLE_ON *) cmd; retval = emcSpindleOn(spindle_on_msg->speed, spindle_on_msg->factor, spindle_on_msg->xoffset); break; @@ -2323,6 +2335,10 @@ static int emcTaskCheckPostconditions(NMLmsg * cmd) return EMC_TASK_EXEC_DONE; break; + case EMC_SPINDLE_WAIT_ORIENT_COMPLETE_TYPE: + return EMC_TASK_EXEC_WAITING_FOR_SPINDLE_ORIENTED; + break; + case EMC_TRAJ_DELAY_TYPE: case EMC_AUX_INPUT_WAIT_TYPE: return EMC_TASK_EXEC_WAITING_FOR_DELAY; @@ -2540,6 +2556,37 @@ static int emcTaskExecute(void) } break; + case EMC_TASK_EXEC_WAITING_FOR_SPINDLE_ORIENTED: + STEPPING_CHECK(); // not sure + switch (emcStatus->motion.spindle.orient_state) { + case EMCMOT_ORIENT_NONE: + case EMCMOT_ORIENT_COMPLETE: + emcStatus->task.execState = EMC_TASK_EXEC_DONE; + emcStatus->task.delayLeft = 0; + emcTaskEager = 1; + rcs_print("wait for orient complete: nothing to do\n"); + break; + + case EMCMOT_ORIENT_IN_PROGRESS: + emcStatus->task.delayLeft = taskExecDelayTimeout - etime(); + if (etime() >= taskExecDelayTimeout) { + emcStatus->task.execState = EMC_TASK_EXEC_ERROR; + emcStatus->task.delayLeft = 0; + emcTaskEager = 1; + emcOperatorError(0, "wait for orient complete: TIMED OUT"); + } + break; + + case EMCMOT_ORIENT_FAULTED: + // actually the code in main() should trap this before we get here + emcStatus->task.execState = EMC_TASK_EXEC_ERROR; + emcStatus->task.delayLeft = 0; + emcTaskEager = 1; + emcOperatorError(0, "wait for orient complete: FAULTED code=%d", + emcStatus->motion.spindle.orient_fault); + } + break; + case EMC_TASK_EXEC_WAITING_FOR_DELAY: STEPPING_CHECK(); // check if delay has passed @@ -3230,6 +3277,7 @@ int main(int argc, char *argv[]) } } + // check for subordinate errors, and halt task if so if (emcStatus->motion.status == RCS_ERROR || ((emcStatus->io.status == RCS_ERROR) && @@ -3249,10 +3297,18 @@ int main(int argc, char *argv[]) emcOperatorError(0, io_error, emcStatus->io.reason); } } - // abort everything - emcTaskAbort(); - emcIoAbort(EMC_ABORT_MOTION_OR_IO_RCS_ERROR); - emcSpindleAbort(); + // motion already should have reported this condition (and set RCS_ERROR?) + // an M19 orient failed to complete within timeout + // if ((emcStatus->motion.status == RCS_ERROR) && + // (emcStatus->motion.spindle.orient_state == EMCMOT_ORIENT_FAULTED) && + // (emcStatus->motion.spindle.orient_fault != 0)) { + // emcOperatorError(0, "wait for orient complete timed out"); + // } + + // abort everything + emcTaskAbort(); + emcIoAbort(EMC_ABORT_MOTION_OR_IO_RCS_ERROR); + emcSpindleAbort(); mdi_execute_abort(); // without emcTaskPlanClose(), a new run command resumes at // aborted line-- feature that may be considered later diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 157206f89..16681776b 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1500,6 +1500,8 @@ int emcMotionUpdate(EMC_MOTION_STAT * stat) stat->spindle.speed = emcmotStatus.spindle.speed; stat->spindle.brake = emcmotStatus.spindle.brake; stat->spindle.direction = emcmotStatus.spindle.direction; + stat->spindle.orient_state = emcmotStatus.spindle.orient_state; + stat->spindle.orient_fault = emcmotStatus.spindle.orient_fault; for (dio = 0; dio < EMC_MAX_DIO; dio++) { stat->synch_di[dio] = emcmotStatus.synch_di[dio]; diff --git a/tests/interp/m19/expected b/tests/interp/m19/expected index 01ff1e309..3b9d3e919 100644 --- a/tests/interp/m19/expected +++ b/tests/interp/m19/expected @@ -3,11 +3,12 @@ N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) N..... SET_XY_ROTATION(0.0000) N..... SET_FEED_REFERENCE(CANON_XYZ) - N..... ORIENT_SPINDLE(42.0000, 0) N..... ORIENT_SPINDLE(87.0000, 0) + N..... SPINDLE_WAIT_ORIENT_COMPLETE(2.0000) N..... ORIENT_SPINDLE(42.0000, 1) N..... ORIENT_SPINDLE(132.0000, 0) N..... ORIENT_SPINDLE(132.0000, 1) + N..... SPINDLE_WAIT_ORIENT_COMPLETE(1.0000) N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) N..... SET_XY_ROTATION(0.0000) N..... SET_FEED_MODE(0) diff --git a/tests/interp/m19/test.ngc b/tests/interp/m19/test.ngc index 06072c3fd..c33de489d 100644 --- a/tests/interp/m19/test.ngc +++ b/tests/interp/m19/test.ngc @@ -1,7 +1,8 @@ M19 -M19 R45 +M19 R45 Q2 M19 P1 M19 R90 P0 M19 R90 P1 +M19 Q1 M2