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

@@ -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 
+net fault gladevcp.spindle-orient-fault-s motion.spindle-orient-fault 

configs/sim/orient.ui

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <interface>
-  <requires lib="gtk+" version="2.16"/>
   <!-- interface-requires gladevcp 0.0 -->
+  <requires lib="gtk+" version="2.16"/>
   <!-- interface-naming-policy project-wide -->
   <object class="GtkWindow" id="window1">
     <child>
@@ -87,37 +87,15 @@
                 <child>
                   <object class="HAL_Label" id="spindle-orient-angle">
                     <property name="visible">True</property>
-                    <property name="label_pin_type">1</property>
                     <property name="text_template">motion.spindle-orient-angle: %f</property>
+                    <property name="label_pin_type">1</property>
                   </object>
                   <packing>
                     <property name="right_attach">2</property>
                     <property name="top_attach">3</property>
                     <property name="bottom_attach">4</property>
                   </packing>
                 </child>
-                <child>
-                  <object class="GtkLabel" id="label3">
-                    <property name="visible">True</property>
-                    <property name="label" translatable="yes">motion.spindle-orient-fwd:</property>
-                  </object>
-                  <packing>
-                    <property name="top_attach">4</property>
-                    <property name="bottom_attach">5</property>
-                  </packing>
-                </child>
-                <child>
-                  <object class="HAL_LED" id="spindle-orient-fwd-led">
-                    <property name="visible">True</property>
-                    <property name="led_shape">2</property>
-                  </object>
-                  <packing>
-                    <property name="left_attach">1</property>
-                    <property name="right_attach">2</property>
-                    <property name="top_attach">4</property>
-                    <property name="bottom_attach">5</property>
-                  </packing>
-                </child>
                 <child>
                   <object class="HAL_LED" id="spindle-brake-led">
                     <property name="visible">True</property>
@@ -164,6 +142,12 @@
                     <property name="bottom_attach">7</property>
                   </packing>
                 </child>
+                <child>
+                  <placeholder/>
+                </child>
+                <child>
+                  <placeholder/>
+                </child>
               </object>
             </child>
           </object>

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). 
 

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

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");
 	}
     }

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;
 
 

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();

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.

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();

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:

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() {}

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) {

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

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();

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);