Merge pull request #4160 from grandixximo/spindle-atspeed-m5

motion: wait for spindle at-speed after M5 spindle stop

Author: andypugh

src/emc/motion/command.c

@@ -507,6 +507,22 @@ void emcmotCommandHandler_locked(void *arg, long servo_period)
 	    if (GET_MOTION_TELEOP_FLAG()) {
                 axis_jog_abort_all(0);
 	    } else if (GET_MOTION_COORD_FLAG()) {
+		/* If motion was being held waiting for spindle at-speed, the
+		   operator most likely aborted because the machine sat idle
+		   with no obvious reason. Tell them why, loudly. A wait that
+		   never released after a spindle stop almost always means
+		   spindle.N.at-speed is not wired, or does not go true once the
+		   spindle has actually stopped. */
+		if (MOTION_ID_VALID(emcmotInternal->coord_tp.spindle.waiting_for_atspeed)) {
+			for (spindle_num = 0; spindle_num < emcmotConfig->numSpindles; spindle_num++) {
+				if (emcmotStatus->spindle_status[spindle_num].state == 0 && !emcmotStatus->spindle_status[spindle_num].at_speed) {
+					reportError(_("Aborted while waiting for spindle %d at-speed after a spindle stop. "
+						"Check that spindle.%d.at-speed is connected and goes true once the spindle has stopped."),
+						spindle_num, spindle_num);
+					break;
+				}
+			}
+		}
 		tpAbort(&emcmotInternal->coord_tp);
 	    } else {
 		for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) {
@@ -530,6 +546,8 @@ void emcmotCommandHandler_locked(void *arg, long servo_period)
 		SET_JOINT_FAULT_FLAG(joint, 0);
 	    }
 	    emcmotStatus->paused = 0;
+	    /* Drop any pending at-speed barrier so it can't strand a later move. */
+	    emcmotStatus->atspeed_next_feed = 0;
 	    // Clear pins on abort so tests see a clean state
 		if (emcmot_hal_data) {
 				*(emcmot_hal_data->interp_arc_radius) = 0.0;
@@ -1496,6 +1514,14 @@ void emcmotCommandHandler_locked(void *arg, long servo_period)
                 }
             }
 
+	    /* A probe is a feed-type move, so honor any pending at-speed
+	       barrier just like G1: wait for spindle.N.at-speed (spin-up after
+	       M3, or stop after M5) before the probe starts. */
+	    if (emcmotStatus->atspeed_next_feed) {
+		issue_atspeed = 1;
+		emcmotStatus->atspeed_next_feed = 0;
+	    }
+
 	    /* append it to the emcmotInternal->coord_tp */
 	    tpSetId(&emcmotInternal->coord_tp, emcmotCommand->id);
 	    if (-1 == tpAddLine(&emcmotInternal->coord_tp,
@@ -1506,7 +1532,7 @@ void emcmotCommandHandler_locked(void *arg, long servo_period)
 				emcmotCommand->acc,
 				emcmotCommand->ini_maxjerk,
 				emcmotStatus->enables_new,
-				0,
+				issue_atspeed,
 				-1,
 				emcmotCommand->tag)) {
 		reportError(_("can't add probe move"));
@@ -1702,6 +1728,10 @@ void emcmotCommandHandler_locked(void *arg, long servo_period)
 	        *(emcmot_hal_data->spindle[n].spindle_orient) = 0;
 	        emcmotStatus->spindle_status[n].orient_state = EMCMOT_ORIENT_NONE;
         }
+        /* Optionally make the spindle stop an at-speed barrier: the next feed
+           move waits until at-speed reflects the stopped state. Safe when
+           unwired since at-speed defaults to 1 (see motion.c). */
+        emcmotStatus->atspeed_next_feed = emcmotCommand->wait_for_spindle_at_speed;
 	    break;
 
 	case EMCMOT_SPINDLE_ORIENT:

src/emc/motion/motion.c

@@ -776,6 +776,8 @@ static int export_spindle(int num, spindle_hal_t * addr){
     if ((retval = hal_pin_float_newf(HAL_IN, &(addr->spindle_revs), mot_comp_id, "spindle.%d.revs", num)) != 0) return retval;
     if ((retval = hal_pin_float_newf(HAL_IN, &(addr->spindle_speed_in), mot_comp_id, "spindle.%d.speed-in", num)) != 0) return retval;
     if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->spindle_is_atspeed), mot_comp_id, "spindle.%d.at-speed", num)) != 0) return retval;
+    /* Default 1: an unwired at-speed pin must never block motion. Do not
+       change to 0 or machines without at-speed wired would idle forever. */
     *(addr->spindle_is_atspeed) = 1;
     /* restore saved message level */
     rtapi_set_msg_level(msg);

src/emc/nml_intf/canon.hh

@@ -639,7 +639,7 @@ This is usually given in rpm and refers to the rate of spindle
 rotation. If the spindle is already turning and is at a different
 speed, change to the speed given with this command. */
 
-extern void STOP_SPINDLE_TURNING(int spindle);
+extern void STOP_SPINDLE_TURNING(int spindle, int wait_for_atspeed = 1);
 
 /* Stop the spindle from turning. If the spindle is already stopped, this
 command may be given, but it will have no effect. */

src/emc/nml_intf/emc.cc

@@ -734,6 +734,7 @@ void EMC_SPINDLE_OFF::update(CMS * cms)
 {
     EMC_SPINDLE_CMD_MSG::update(cms);
     cms->update(spindle);
+    cms->update(wait_for_spindle_at_speed);
 }
 
 /*

src/emc/nml_intf/emc.hh

@@ -423,7 +423,7 @@ extern int emcSpindleAbort(int spindle);
 extern int emcSpindleSpeed(int spindle, double speed, double factor, double xoffset);
 extern int emcSpindleOn(int spindle, double speed, double factor, double xoffset,int wait_for_atspeed = 1);
 extern int emcSpindleOrient(int spindle, double orientation, int direction);
-extern int emcSpindleOff(int spindle);
+extern int emcSpindleOff(int spindle, int wait_for_atspeed = 0);
 extern int emcSpindleIncrease(int spindle);
 extern int emcSpindleDecrease(int spindle);
 extern int emcSpindleConstant(int spindle);

src/emc/nml_intf/emc_nml.hh

@@ -1801,7 +1801,8 @@ class EMC_SPINDLE_OFF:public EMC_SPINDLE_CMD_MSG {
   public:
     EMC_SPINDLE_OFF()
       : EMC_SPINDLE_CMD_MSG(EMC_SPINDLE_OFF_TYPE, sizeof(EMC_SPINDLE_OFF)),
-        spindle(0)
+        spindle(0),
+        wait_for_spindle_at_speed(0)
     {};
 
     // For internal NML/CMS use only.
@@ -1810,6 +1811,7 @@ class EMC_SPINDLE_OFF:public EMC_SPINDLE_CMD_MSG {
     void update(CMS * cms);
 
     int spindle;    // the spindle to be turned off
+    int wait_for_spindle_at_speed; // wait for at-speed (spindle stopped) before next feed
 };
 
 class EMC_SPINDLE_INCREASE:public EMC_SPINDLE_CMD_MSG {

src/emc/rs274ngc/gcodemodule.cc

@@ -551,7 +551,7 @@ void STOP_SPEED_FEED_SYNCH() {}
 void START_SPINDLE_COUNTERCLOCKWISE(int /*spindle*/, int /*wait_for_at_speed*/) {}
 void START_SPINDLE_CLOCKWISE(int /*spindle*/, int /*wait_for_at_speed*/) {}
 void SET_SPINDLE_MODE(int /*spindle*/, double) {}
-void STOP_SPINDLE_TURNING(int /*spindle*/) {}
+void STOP_SPINDLE_TURNING(int /*spindle*/, int /*wait_for_at_speed*/) {}
 void SET_SPINDLE_SPEED(int /*spindle*/, double /*rpm*/) {}
 void ORIENT_SPINDLE(int /*spindle*/, double /*d*/, int /*i*/) {}
 void WAIT_SPINDLE_ORIENT_COMPLETE(int /*s*/, double /*timeout*/) {}

src/emc/sai/saicanon.cc

@@ -485,7 +485,7 @@ void SET_SPINDLE_SPEED(int spindle, double rpm)
   _sai._spindle_speed[spindle] = rpm;
 }
 
-void STOP_SPINDLE_TURNING(int spindle)
+void STOP_SPINDLE_TURNING(int spindle, int /*wait_for_atspeed*/)
 {
   PRINT("STOP_SPINDLE_TURNING(%i)\n", spindle);
   _sai._spindle_turning[spindle] = CANON_STOPPED;

src/emc/task/emccanon.cc

@@ -3017,12 +3017,13 @@ void SET_SPINDLE_SPEED(int s, double speed_rpm)
     interp_list.append(SPINDLE_SPEED_<EMC_SPINDLE_SPEED>(s, 0, speed_rpm));
 }
 
-void STOP_SPINDLE_TURNING(int s)
+void STOP_SPINDLE_TURNING(int s, int wait_for_atspeed)
 {
     auto emc_spindle_off_msg = std::make_unique<EMC_SPINDLE_OFF>();
 
     flush_segments();
     emc_spindle_off_msg->spindle = s;
+    emc_spindle_off_msg->wait_for_spindle_at_speed = wait_for_atspeed;
     interp_list.append(std::move(emc_spindle_off_msg));
     // Added by atp 6/1/18 not sure this is right. There is a problem that the _second_ S word starts the spindle without M3/M4
     canon.spindle[s].dir = 0;

src/emc/task/emctaskmain.cc

@@ -1995,7 +1995,7 @@ static int emcTaskIssueCommand(NMLmsg * cmd)
 
     case EMC_SPINDLE_OFF_TYPE:
     spindle_off_msg = reinterpret_cast<EMC_SPINDLE_OFF *>(cmd);
-	retval = emcSpindleOff(spindle_off_msg->spindle);
+	retval = emcSpindleOff(spindle_off_msg->spindle, spindle_off_msg->wait_for_spindle_at_speed);
 	break;
 
     case EMC_SPINDLE_BRAKE_RELEASE_TYPE: