Fix synched spindle moves exceeding linear axis limits

lib/hallib/sim_spindle_encoder.hal

@@ -5,26 +5,39 @@ setp sim_spindle.scale 0.01666667
 loadrt limit2 names=limit_speed
 loadrt lowpass names=spindle_mass
 loadrt near names=near_speed
+loadrt quant names=sim_spindle_encoder,spindle_vel_est
 
-# this limit doesnt make any sense to me:
+# Bound spindle acceleration to something reasonable
 setp limit_speed.maxv 5000.0 # rpm/second
 
 # encoder reset control
 # hook up motion controller's sync output
 net spindle-index-enable motion.spindle-index-enable <=> sim_spindle.index-enable
 
+# Set resolution of spindle encoder in counts / revolution (Note: this value is 4 * LPR or PPR)
+setp sim_spindle_encoder.resolution 64.0
+
+# Simulated spindle encoder
+net spindle-pos-raw sim_spindle.position-fb => sim_spindle_encoder.in
+
 # report our revolution count to the motion controller
-net spindle-pos sim_spindle.position-fb => motion.spindle-revs
+net spindle-pos sim_spindle_encoder.out => motion.spindle-revs
 
 # simulate spindle mass
 setp spindle_mass.gain .07
 
-# spindle speed control
+# Limit spindle speed by our maximum value
 net spindle-speed-cmd  motion.spindle-speed-out => limit_speed.in
-net spindle-speed-limited limit_speed.out => sim_spindle.velocity-cmd spindle_mass.in
 
-# for spindle velocity estimate
-net spindle-rpm-filtered spindle_mass.out motion.spindle-speed-in near_speed.in2
+# Simulate spindle mass with a low-pass filter on the velocity command
+net spindle-speed-limited limit_speed.out => spindle_mass.in
+
+# Feed the simulated spindle speed into the sim spindle to generate a position signal
+net spindle-rpm-filtered spindle_mass.out => sim_spindle.velocity-cmd near_speed.in2 spindle_vel_est.in
+
+# Approximate velocity quantization assuming 360 RPM, 30kHz base thread, 100 PPR encoder
+setp spindle_vel_est.resolution 8.0
+net spindle-rpm-est spindle_vel_est.out => motion.spindle-speed-in
 
 # at-speed detection
 setp near_speed.scale 1.1
@@ -37,3 +50,5 @@ addf limit_speed servo-thread
 addf spindle_mass servo-thread
 addf near_speed servo-thread
 addf sim_spindle servo-thread
+addf sim_spindle_encoder servo-thread
+addf spindle_vel_est servo-thread

lib/python/rs274/glcanon.py

@@ -49,6 +49,8 @@ def __init__(self, colors, geometry, is_foam=0):
         self.arcfeed = []; self.arcfeed_append = self.arcfeed.append
         # dwell list - [line number, color, pos x, pos y, pos z, plane]
         self.dwells = []; self.dwells_append = self.dwells.append
+        # spindle-synched feed list - (line number, (position deltas), S, fpr)
+        self.feed_synched = []
         self.choice = None
         self.feedrate = 1
         self.lo = (0,) * 9
@@ -162,6 +164,33 @@ def calc_extents(self):
                 self.min_extents_notool[0], self.min_extents_notool[1], min_z
             self.max_extents_notool = \
                 self.max_extents_notool[0], self.max_extents_notool[1], max_z
+
+    def calc_velocity(self, delta, axis_max_vel):
+        """Using techniques from getStraightVelocity() in emccanon.cc, given 9
+        axis deltas and velocity limits, calculate max velocity of a
+        straight move; deltas should be absolute; invalid axes should be 0
+        """
+        # Clean up tiny values
+        delta = tuple([(0.0 if i<1e-7 else i) for i in delta])
+        # Fastest time of coordinated move is the maximum time of any
+        # one axis to perform move at axis max velocity
+        tmax = max([(i[0]/i[1] if i[1] else 0.0)
+                     for i in zip(delta, axis_max_vel)])
+        # Total distance is the hypotenuse of a set of three axes;
+        # which set depends on the type of move
+        if sum(delta[0:3]) > 0:
+            # Linear XYZ with or without ABC or UVW
+            dtot = math.sqrt(sum(i*i for i in delta[0:3]))
+        elif sum(delta[6:9]) > 0:
+            # Linear UVW without XYZ and with or without ABC
+            dtot = math.sqrt(sum(i*i for i in delta[6:9]))
+        else:
+            # Angular-only
+            dtot = math.sqrt(sum(i*i for i in delta[3:6]))
+        # Max velocity = total distance / fastest time
+        max_vel = dtot/tmax
+        return max_vel
+
     def tool_offset(self, xo, yo, zo, ao, bo, co, uo, vo, wo):
         self.first_move = True
         x, y, z, a, b, c, u, v, w = self.lo
@@ -230,6 +259,18 @@ def straight_feed(self, x,y,z, a,b,c, u, v, w):
         self.lo = l
     straight_probe = straight_feed
 
+    def straight_feed_synched(self, x,y,z, a,b,c, u,v,w, s, fpr):
+        """For spindle-synched straight feeds, also collect data needed to
+        check if the commanded spindle rate and feed per revolution
+        will violate any axis MAX_VELOCITY constraints"""
+        if self.suppress > 0: return
+        # save segment start and record straight feed segment
+        lo = self.lo
+        self.straight_feed(x,y,z, a,b,c, u, v, w)
+        # record axis distances, spindle speed and feed per revolution
+        delta = tuple([abs(i[0]-i[1]) for i in zip(lo, self.lo)])
+        self.feed_synched.append((self.lineno, delta, s, fpr))
+
     def user_defined_function(self, i, p, q):
         if self.suppress > 0: return
         color = self.colors['m1xx']

src/emc/motion-logger/motion-logger.c

@@ -572,12 +572,12 @@ int main(int argc, char* argv[]) {
 
             case EMCMOT_SPINDLE_ON:
                 log_print("SPINDLE_ON speed=%f, css_factor=%f, xoffset=%f\n", c->vel, c->ini_maxvel, c->acc);
-                emcmotStatus->spindle.speed = c->vel;
+                emcmotStatus->spindle_cmd.velocity_rpm_out = c->vel;
                 break;
 
             case EMCMOT_SPINDLE_OFF:
                 log_print("SPINDLE_OFF\n");
-                emcmotStatus->spindle.speed = 0;
+                emcmotStatus->spindle_cmd.velocity_rpm_out = 0;
                 break;
 
             case EMCMOT_SPINDLE_INCREASE:

src/emc/motion/command.c

@@ -891,7 +891,7 @@ check_stuff ( "before command_handler()" );
                 issue_atspeed = 1;
                 emcmotStatus->atspeed_next_feed = 0;
             }
-            if(!is_feed_type(emcmotCommand->motion_type) && emcmotStatus->spindle.css_factor) {
+            if(!is_feed_type(emcmotCommand->motion_type) && emcmotStatus->spindle_cmd.css_factor) {
                 emcmotStatus->atspeed_next_feed = 1;
             }
 	    /* append it to the emcmotDebug->tp */
@@ -1522,38 +1522,32 @@ check_stuff ( "before command_handler()" );
 		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;
+        emcmotStatus->spindle_cmd.orient_state = EMCMOT_ORIENT_NONE;
 
 	    /* if (emcmotStatus->spindle.orient) { */
 	    /* 	reportError(_("cant turn on spindle during orient in progress")); */
 	    /* 	emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND; */
 	    /* 	tpAbort(&emcmotDebug->tp); */
 	    /* 	SET_MOTION_ERROR_FLAG(1); */
 	    /* } else { */
-	    emcmotStatus->spindle.speed = emcmotCommand->vel;
-	    emcmotStatus->spindle.css_factor = emcmotCommand->ini_maxvel;
-	    emcmotStatus->spindle.xoffset = emcmotCommand->acc;
-	    if (emcmotCommand->vel >= 0) {
-		emcmotStatus->spindle.direction = 1;
-	    } else {
-		emcmotStatus->spindle.direction = -1;
-	    }
-	    emcmotStatus->spindle.brake = 0; //disengage brake
+        emcmotStatus->spindle_cmd.velocity_rpm_out = emcmotCommand->vel;
+        emcmotStatus->spindle_cmd.css_factor = emcmotCommand->ini_maxvel;
+        emcmotStatus->spindle_cmd.xoffset = emcmotCommand->acc;
+        emcmotStatus->spindle_cmd.brake = 0; //disengage brake
 	    emcmotStatus->atspeed_next_feed = 1;
 	    break;
 
 	case EMCMOT_SPINDLE_OFF:
 	    rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_OFF");
-	    emcmotStatus->spindle.speed = 0;
-	    emcmotStatus->spindle.direction = 0;
-	    emcmotStatus->spindle.brake = 1; // engage brake
+        emcmotStatus->spindle_cmd.velocity_rpm_out = 0;
+        emcmotStatus->spindle_cmd.brake = 1; // engage brake
 	    if (*(emcmot_hal_data->spindle_orient))
 		rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_ORIENT cancelled by SPINDLE_OFF");
 	    if (*(emcmot_hal_data->spindle_locked))
 		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;
+        emcmotStatus->spindle_cmd.orient_state = EMCMOT_ORIENT_NONE;
 	    break;
 
 	case EMCMOT_SPINDLE_ORIENT:
@@ -1567,50 +1561,48 @@ check_stuff ( "before command_handler()" );
 		/* tpAbort(&emcmotDebug->tp); */
 		/* SET_MOTION_ERROR_FLAG(1); */
 	    }
-	    emcmotStatus->spindle.orient_state = EMCMOT_ORIENT_IN_PROGRESS;
-	    emcmotStatus->spindle.speed = 0;
-	    emcmotStatus->spindle.direction = 0;
+        emcmotStatus->spindle_cmd.orient_state = EMCMOT_ORIENT_IN_PROGRESS;
+        emcmotStatus->spindle_cmd.velocity_rpm_out = 0;
 	    // so far like spindle stop, except opening brake
-	    emcmotStatus->spindle.brake = 0; // open brake
+        emcmotStatus->spindle_cmd.brake = 0; // open brake
 
 	    *(emcmot_hal_data->spindle_orient_angle) = emcmotCommand->orientation;
 	    *(emcmot_hal_data->spindle_orient_mode) = emcmotCommand->mode;
 	    *(emcmot_hal_data->spindle_locked) = 0;
 	    *(emcmot_hal_data->spindle_orient) = 1;
 
 	    // mirror in spindle status
-	    emcmotStatus->spindle.orient_fault = 0; // this pin read during spindle-orient == 1 
-	    emcmotStatus->spindle.locked = 0;
+        emcmotStatus->spindle_cmd.orient_fault = 0; // this pin read during spindle-orient == 1
+        emcmotStatus->spindle_cmd.locked = 0;
 	    break;
 
 	case EMCMOT_SPINDLE_INCREASE:
 	    rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_INCREASE");
-	    if (emcmotStatus->spindle.speed > 0) {
-		emcmotStatus->spindle.speed += 100; //FIXME - make the step a HAL parameter
-	    } else if (emcmotStatus->spindle.speed < 0) {
-		emcmotStatus->spindle.speed -= 100;
+        if (emcmotStatus->spindle_cmd.velocity_rpm_out > 0) {
+        emcmotStatus->spindle_cmd.velocity_rpm_out += 100; //FIXME - make the step a HAL parameter
+        } else if (emcmotStatus->spindle_cmd.velocity_rpm_out < 0) {
+        emcmotStatus->spindle_cmd.velocity_rpm_out -= 100;
 	    }
 	    break;
 
 	case EMCMOT_SPINDLE_DECREASE:
 	    rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_DECREASE");
-	    if (emcmotStatus->spindle.speed > 100) {
-		emcmotStatus->spindle.speed -= 100; //FIXME - make the step a HAL parameter
-	    } else if (emcmotStatus->spindle.speed < -100) {
-		emcmotStatus->spindle.speed += 100;
+        if (emcmotStatus->spindle_cmd.velocity_rpm_out > 100) {
+        emcmotStatus->spindle_cmd.velocity_rpm_out -= 100; //FIXME - make the step a HAL parameter
+        } else if (emcmotStatus->spindle_cmd.velocity_rpm_out < -100) {
+        emcmotStatus->spindle_cmd.velocity_rpm_out += 100;
 	    }
 	    break;
 
 	case EMCMOT_SPINDLE_BRAKE_ENGAGE:
 	    rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_BRAKE_ENGAGE");
-	    emcmotStatus->spindle.speed = 0;
-	    emcmotStatus->spindle.direction = 0;
-	    emcmotStatus->spindle.brake = 1;
+        emcmotStatus->spindle_cmd.velocity_rpm_out = 0;
+        emcmotStatus->spindle_cmd.brake = 1;
 	    break;
 
 	case EMCMOT_SPINDLE_BRAKE_RELEASE:
 	    rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_BRAKE_RELEASE");
-	    emcmotStatus->spindle.brake = 0;
+        emcmotStatus->spindle_cmd.brake = 0;
 	    break;
 
 	case EMCMOT_SET_JOINT_COMP:

src/emc/motion/control.c

@@ -375,17 +375,33 @@ static void process_probe_inputs(void) {
     old_probeVal = emcmotStatus->probeVal;
 }
 
+// TODO generalize to higher order diff with an array
+static double backward_difference(double curr, double prev, double dt)
+{
+    return (curr - prev) / dt;
+}
+
+
 static void process_inputs(void)
 {
     int joint_num;
     double abs_ferror, tmp, scale;
     joint_hal_t *joint_data;
     emcmot_joint_t *joint;
     unsigned char enables;
+
     /* read spindle angle (for threading, etc) */
-    emcmotStatus->spindleRevs = *emcmot_hal_data->spindle_revs;
-    emcmotStatus->spindleSpeedIn = *emcmot_hal_data->spindle_speed_in;
+    double prev_revs = emcmotStatus->spindle_fb.position_rev;
+    emcmotStatus->spindle_fb.position_rev = *emcmot_hal_data->spindle_revs;
+    *emcmot_hal_data->spindle_speed_in_estimate = backward_difference(emcmotStatus->spindle_fb.position_rev,
+                                                                      prev_revs,
+                                                                      emcmotConfig->trajCycleTime) * 60.0;
+    emcmotStatus->spindle_fb.velocity_rpm = *emcmot_hal_data->spindle_speed_in;
     emcmotStatus->spindle_is_atspeed = *emcmot_hal_data->spindle_is_atspeed;
+    // Minimum gain is zero (no position error correction), maximum gain is 1 (correct at maximum acceleration)
+    emcmotStatus->spindle_tracking_gain = fmax(fmin(*emcmot_hal_data->spindle_tracking_gain, 1.0), 0.0);
+    emcmotStatus->pos_tracking_mode = *emcmot_hal_data->pos_tracking_mode;
+
     /* compute net feed and spindle scale factors */
     if ( emcmotStatus->motion_state == EMCMOT_MOTION_COORD ) {
 	/* use the enables that were queued with the current move */
@@ -524,19 +540,19 @@ 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;
+	    emcmotStatus->spindle_cmd.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);
+	    emcmotStatus->spindle_cmd.orient_fault = *(emcmot_hal_data->spindle_orient_fault);
+	    reportError(_("fault %d during orient in progress"), emcmotStatus->spindle_cmd.orient_fault);
 	    emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
 	    tpAbort(&emcmotDebug->tp);
 	    SET_MOTION_ERROR_FLAG(1);
 	} else if (*(emcmot_hal_data->spindle_is_oriented)) {
 	    *(emcmot_hal_data->spindle_orient) = 0;
 	    *(emcmot_hal_data->spindle_locked) = 1;
-	    emcmotStatus->spindle.locked = 1;
-	    emcmotStatus->spindle.brake = 1;
-	    emcmotStatus->spindle.orient_state = EMCMOT_ORIENT_COMPLETE;
+	    emcmotStatus->spindle_cmd.locked = 1;
+	    emcmotStatus->spindle_cmd.brake = 1;
+	    emcmotStatus->spindle_cmd.orient_state = EMCMOT_ORIENT_COMPLETE;
 	    rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_ORIENT complete, spindle locked");
 	}
     }
@@ -1713,16 +1729,16 @@ static void output_to_hal(void)
     *(emcmot_hal_data->teleop_mode) = GET_MOTION_TELEOP_FLAG();
     *(emcmot_hal_data->coord_error) = GET_MOTION_ERROR_FLAG();
     *(emcmot_hal_data->on_soft_limit) = emcmotStatus->on_soft_limit;
-    if(emcmotStatus->spindle.css_factor) {
-	double denom = fabs(emcmotStatus->spindle.xoffset - emcmotStatus->carte_pos_cmd.tran.x);
+    if(emcmotStatus->spindle_cmd.css_factor) {
+	double denom = fabs(emcmotStatus->spindle_cmd.xoffset - emcmotStatus->carte_pos_cmd.tran.x);
 	double speed;
         double maxpositive;
-        if(denom > 0) speed = emcmotStatus->spindle.css_factor / denom;
-	else speed = emcmotStatus->spindle.speed;
+        if(denom > 0) speed = emcmotStatus->spindle_cmd.css_factor / denom;
+    else speed = emcmotStatus->spindle_cmd.velocity_rpm_out;
 
 	speed = speed * emcmotStatus->net_spindle_scale;
 
-        maxpositive = fabs(emcmotStatus->spindle.speed);
+        maxpositive = fabs(emcmotStatus->spindle_cmd.velocity_rpm_out);
         // cap speed to G96 D...
         if(speed < -maxpositive)
             speed = -maxpositive;
@@ -1732,16 +1748,16 @@ static void output_to_hal(void)
 	*(emcmot_hal_data->spindle_speed_out) = speed;
 	*(emcmot_hal_data->spindle_speed_out_rps) = speed/60.;
     } else {
-	*(emcmot_hal_data->spindle_speed_out) = emcmotStatus->spindle.speed * emcmotStatus->net_spindle_scale;
-	*(emcmot_hal_data->spindle_speed_out_rps) = emcmotStatus->spindle.speed * emcmotStatus->net_spindle_scale / 60.;
+    *(emcmot_hal_data->spindle_speed_out) = emcmotStatus->spindle_cmd.velocity_rpm_out * emcmotStatus->net_spindle_scale;
+    *(emcmot_hal_data->spindle_speed_out_rps) = emcmotStatus->spindle_cmd.velocity_rpm_out * emcmotStatus->net_spindle_scale / 60.;
     }
 	*(emcmot_hal_data->spindle_speed_out_abs) = fabs(*(emcmot_hal_data->spindle_speed_out));
 	*(emcmot_hal_data->spindle_speed_out_rps_abs) = fabs(*(emcmot_hal_data->spindle_speed_out_rps));
-    *(emcmot_hal_data->spindle_speed_cmd_rps) = emcmotStatus->spindle.speed / 60.;
-    *(emcmot_hal_data->spindle_on) = ((emcmotStatus->spindle.speed * emcmotStatus->net_spindle_scale) != 0) ? 1 : 0;
+    *(emcmot_hal_data->spindle_speed_cmd_rps) = emcmotStatus->spindle_cmd.velocity_rpm_out / 60.;
+    *(emcmot_hal_data->spindle_on) = ((emcmotStatus->spindle_cmd.velocity_rpm_out * emcmotStatus->net_spindle_scale) != 0) ? 1 : 0;
     *(emcmot_hal_data->spindle_forward) = (*emcmot_hal_data->spindle_speed_out > 0) ? 1 : 0;
     *(emcmot_hal_data->spindle_reverse) = (*emcmot_hal_data->spindle_speed_out < 0) ? 1 : 0;
-    *(emcmot_hal_data->spindle_brake) = (emcmotStatus->spindle.brake != 0) ? 1 : 0;
+    *(emcmot_hal_data->spindle_brake) = (emcmotStatus->spindle_cmd.brake != 0) ? 1 : 0;
 
     *(emcmot_hal_data->program_line) = emcmotStatus->id;
     *(emcmot_hal_data->motion_type) = emcmotStatus->motionType;
@@ -1774,22 +1790,22 @@ static void output_to_hal(void)
     emcmot_hal_data->debug_bit_0 = joints[1].free_tp_active;
     emcmot_hal_data->debug_bit_1 = emcmotStatus->enables_new & AF_ENABLED;
     emcmot_hal_data->debug_float_0 = emcmotStatus->net_feed_scale;
-    emcmot_hal_data->debug_float_1 = emcmotStatus->spindleRevs;
-    emcmot_hal_data->debug_float_2 = emcmotStatus->spindleSpeedIn;
+    emcmot_hal_data->debug_float_1 = emcmotStatus->spindle_fb.position_rev;
+    emcmot_hal_data->debug_float_2 = emcmotStatus->spindle_fb.velocity_rpm;
     emcmot_hal_data->debug_float_3 = emcmotStatus->net_spindle_scale;
     emcmot_hal_data->debug_s32_0 = emcmotStatus->overrideLimitMask;
     emcmot_hal_data->debug_s32_1 = emcmotStatus->tcqlen;
 
     /* two way handshaking for the spindle encoder */
-    if(emcmotStatus->spindle_index_enable && !old_motion_index) {
+    if(emcmotStatus->spindle_fb.index_enable && !old_motion_index) {
         *emcmot_hal_data->spindle_index_enable = 1;
     }
 
     if(!*emcmot_hal_data->spindle_index_enable && old_hal_index) {
-        emcmotStatus->spindle_index_enable = 0;
+        emcmotStatus->spindle_fb.index_enable = 0;
     }
 
-    old_motion_index = emcmotStatus->spindle_index_enable;
+    old_motion_index = emcmotStatus->spindle_fb.index_enable;
     old_hal_index = *emcmot_hal_data->spindle_index_enable;
 
     *(emcmot_hal_data->tooloffset_x) = emcmotStatus->tool_offset.tran.x;