diff --git a/lib/hallib/sim_spindle_encoder.hal b/lib/hallib/sim_spindle_encoder.hal index 20ae6b95f0b..de8b128f6ac 100644 --- a/lib/hallib/sim_spindle_encoder.hal +++ b/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 diff --git a/lib/python/rs274/glcanon.py b/lib/python/rs274/glcanon.py index 6552e4b6a2f..9e352f82d0b 100644 --- a/lib/python/rs274/glcanon.py +++ b/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'] diff --git a/src/emc/motion-logger/motion-logger.c b/src/emc/motion-logger/motion-logger.c index 81a3e8db7fb..fead99ebc45 100644 --- a/src/emc/motion-logger/motion-logger.c +++ b/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: diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 7754aa16158..ffdb3f0715c 100644 --- a/src/emc/motion/command.c +++ b/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,7 +1522,7 @@ 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")); */ @@ -1530,30 +1530,24 @@ check_stuff ( "before command_handler()" ); /* 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,11 +1561,10 @@ 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; @@ -1579,38 +1572,37 @@ check_stuff ( "before command_handler()" ); *(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: diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 8e7416549b5..3745db091ce 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -375,6 +375,13 @@ 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; @@ -382,10 +389,19 @@ static void process_inputs(void) 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; diff --git a/src/emc/motion/mot_priv.h b/src/emc/motion/mot_priv.h index f82e36403c3..265a93c1c58 100644 --- a/src/emc/motion/mot_priv.h +++ b/src/emc/motion/mot_priv.h @@ -90,6 +90,8 @@ typedef struct { hal_bit_t *spindle_is_atspeed; hal_float_t *spindle_revs; hal_bit_t *spindle_inhibit; /* RPI: set TRUE to stop spindle (non maskable)*/ + hal_float_t *spindle_tracking_gain; //!< Controls position tracking accuracy from least to most aggressive [0.0-1.0] + hal_s32_t *pos_tracking_mode; hal_float_t *adaptive_feed; /* RPI: adaptive feedrate, 0.0 to 1.0 */ hal_bit_t *feed_hold; /* RPI: set TRUE to stop motion maskable with g53 P1*/ hal_bit_t *feed_inhibit; /* RPI: set TRUE to stop motion (non maskable)*/ @@ -147,7 +149,8 @@ typedef struct { hal_float_t *spindle_speed_out_rps_abs; /* spindle speed output absolute*/ hal_float_t *spindle_speed_cmd_rps; /* spindle speed command without SO applied */ hal_float_t *spindle_speed_in; /* spindle speed measured */ - + hal_float_t *spindle_speed_in_estimate; /* spindle speed measured */ + // spindle orient hal_float_t *spindle_orient_angle; /* out: desired spindle angle, degrees */ hal_s32_t *spindle_orient_mode; /* out: 0: least travel; 1: cw; 2: ccw */ diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index a47ce7b5842..86d5dfdc4d0 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -317,11 +317,18 @@ static int init_hal_io(void) *(emcmot_hal_data->spindle_orient) = 0; -// if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->inpos_output), mot_comp_id, "motion.motion-inpos")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_revs), mot_comp_id, "motion.spindle-revs")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_speed_in), mot_comp_id, "motion.spindle-speed-in")) < 0) goto error; + // Inspired by pid.c "dummysig" pins that estimate velocity internally + emcmot_hal_data->spindle_speed_in_estimate = emcmot_hal_data->spindle_speed_in; + if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_is_atspeed), mot_comp_id, "motion.spindle-at-speed")) < 0) goto error; *emcmot_hal_data->spindle_is_atspeed = 1; + + if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_tracking_gain), mot_comp_id, "motion.spindle-tracking-gain")) < 0) goto error; + *emcmot_hal_data->spindle_tracking_gain = 1.0; + if ((retval = hal_pin_s32_newf(HAL_IN, &(emcmot_hal_data->pos_tracking_mode), mot_comp_id, "motion.pos-tracking-mode")) < 0) goto error; + if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->adaptive_feed), mot_comp_id, "motion.adaptive-feed")) < 0) goto error; *(emcmot_hal_data->adaptive_feed) = 1.0; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_hold), mot_comp_id, "motion.feed-hold")) < 0) goto error; @@ -907,7 +914,7 @@ static int init_comm_buffers(void) emcmotStatus->activeDepth = 0; emcmotStatus->paused = 0; emcmotStatus->overrideLimitMask = 0; - emcmotStatus->spindle.speed = 0.0; + emcmotStatus->spindle_cmd.velocity_rpm_out = 0.0; SET_MOTION_INPOS_FLAG(1); SET_MOTION_ENABLE_FLAG(0); emcmotConfig->kinematics_type = kinType; diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 5c5d7c93b5b..f3f84aa256a 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -544,17 +544,21 @@ Suggestion: Split this in to an Error and a Status flag register.. double big_vel; /* used for "debouncing" velocity */ } emcmot_joint_t; -/* This structure contains only the "status" data associated with - a joint. "Status" data is that data that should be reported to - user space on a continuous basis. An array of these structs is - part of the main status structure, and is filled in with data - copied from the emcmot_joint_t structs every servo period. - - For now this struct contains more data than it really needs, but - paring it down will take time (and probably needs to be done one - or two items at a time, with much testing). My main goal right - now is to get get the large joint struct out of status. - +typedef enum { + SPINDLE_REVERSE=-1, + SPINDLE_STOPPED=0, + SPINDLE_FORWARD=1 +} spindle_direction_code_t; + +/** + * This structure contains only the "status" data associated with a joint. + * "Status" data is that data that should be reported to user space on a + * continuous basis. An array of these structs is part of the main status + * structure, and is filled in with data copied from the emcmot_joint_t structs + * every servo period. For now this struct contains more data than it really + * needs, but paring it down will take time (and probably needs to be done one + * or two items at a time, with much testing). My main goal right now is to + * get get the large joint struct out of status. */ typedef struct { @@ -580,16 +584,21 @@ Suggestion: Split this in to an Error and a Status flag register.. typedef struct { - double speed; // spindle speed in RPMs + double velocity_rpm_out; double css_factor; double xoffset; - int direction; // 0 stopped, 1 forward, -1 reverse int brake; // 0 released, 1 engaged int locked; // spindle lock engaged after orient int orient_fault; // fault code from motion.spindle-orient-fault int orient_state; // orient_state_t - } spindle_status; + } spindle_cmd_status; + typedef struct { + double position_rev; + double velocity_rpm; + int index_enable; /* hooked to a canon encoder index-enable */ + int synced; /* we are doing spindle-synced motion */ + } spindle_fb_status; /********************************* STATUS STRUCTURE @@ -647,12 +656,11 @@ Suggestion: Split this in to an Error and a Status flag register.. unsigned char probe_type; EmcPose probedPos; /* Axis positions stored as soon as possible after last probeTripped */ - int spindle_index_enable; /* hooked to a canon encoder index-enable */ - int spindleSync; /* we are doing spindle-synced motion */ - double spindleRevs; /* position of spindle in revolutions */ - double spindleSpeedIn; /* velocity of spindle in revolutions per minute */ - spindle_status spindle; /* data types for spindle status */ + spindle_cmd_status spindle_cmd; /* Spindle command output from motion */ + spindle_fb_status spindle_fb; /* Spindle feedback input to motion */ + double spindle_tracking_gain; // external control of position trakcing aggressiveness + int pos_tracking_mode; int synch_di[EMCMOT_MAX_DIO]; /* inputs to the motion controller, queried by g-code */ int synch_do[EMCMOT_MAX_DIO]; /* outputs to the motion controller, queried by g-code */ diff --git a/src/emc/nml_intf/interpl.hh b/src/emc/nml_intf/interpl.hh index 185ac1ed279..79cade5989d 100644 --- a/src/emc/nml_intf/interpl.hh +++ b/src/emc/nml_intf/interpl.hh @@ -19,6 +19,8 @@ #include +struct NMLmsg; + #define MAX_NML_COMMAND_SIZE 1000 // these go on the interp list diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 1d56ae5c797..e11d1e1094c 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -144,6 +144,11 @@ static bool metric; static double _pos_x, _pos_y, _pos_z, _pos_a, _pos_b, _pos_c, _pos_u, _pos_v, _pos_w; EmcPose tool_offset; +// Track spindle synched motion +static int spindle_synched_motion = 0; +static double spindle_synched_feed_per_revolution; +static double spindle_speed_programmed = 0.0; + static InterpBase *pinterp; #define interp_new (*pinterp) @@ -225,9 +230,13 @@ void STRAIGHT_FEED(int line_number, if(metric) { x /= 25.4; y /= 25.4; z /= 25.4; u /= 25.4; v /= 25.4; w /= 25.4; } maybe_new_line(line_number); if(interp_error) return; - PyObject *result = - callmethod(callback, "straight_feed", "fffffffff", - x, y, z, a, b, c, u, v, w); + PyObject *result = spindle_synched_motion ? + callmethod(callback, "straight_feed_synched", "fffffffffff", + x, y, z, a, b, c, u, v, w, + spindle_speed_programmed, + spindle_synched_feed_per_revolution / (metric?25.4:1.0)) : + callmethod(callback, "straight_feed", "fffffffff", + x, y, z, a, b, c, u, v, w); if(result == NULL) interp_error ++; Py_XDECREF(result); } @@ -398,14 +407,23 @@ void SET_FEED_REFERENCE(double reference) { } void SET_CUTTER_RADIUS_COMPENSATION(double radius) {} void START_CUTTER_RADIUS_COMPENSATION(int direction) {} void STOP_CUTTER_RADIUS_COMPENSATION(int direction) {} -void START_SPEED_FEED_SYNCH() {} -void START_SPEED_FEED_SYNCH(double sync, bool vel) {} -void STOP_SPEED_FEED_SYNCH() {} +void START_SPEED_FEED_SYNCH() { + spindle_synched_motion = 1; +} +void START_SPEED_FEED_SYNCH(double sync, bool vel) { + spindle_synched_motion = 1; + spindle_synched_feed_per_revolution = sync; +} +void STOP_SPEED_FEED_SYNCH() { + spindle_synched_motion = 0; +} void START_SPINDLE_COUNTERCLOCKWISE() {} void START_SPINDLE_CLOCKWISE() {} void SET_SPINDLE_MODE(double) {} void STOP_SPINDLE_TURNING() {} -void SET_SPINDLE_SPEED(double rpm) {} +void SET_SPINDLE_SPEED(double rpm) { + spindle_speed_programmed = rpm; +} void ORIENT_SPINDLE(double d, int i) {} void WAIT_SPINDLE_ORIENT_COMPLETE(double timeout) {} void PROGRAM_STOP() {} @@ -707,6 +725,8 @@ static PyObject *parse_file(PyObject *self, PyObject *args) { metric=false; interp_error = 0; last_sequence_number = -1; + spindle_synched_motion = 0; + spindle_speed_programmed = 0.0; _pos_x = _pos_y = _pos_z = _pos_a = _pos_b = _pos_c = 0; _pos_u = _pos_v = _pos_w = 0; diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index f4d76c90454..90e7646b1f2 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -52,8 +52,11 @@ #define canon_debug(...) #endif +static bool limitSpindleSpeed(double rpm); +static bool limitSpindleSpeedFromVel(double nominal_vel, double max_vel); + static int debug_velacc = 0; -static double css_maximum, css_numerator; // both always positive +static double css_maximum; // both always positive static int spindle_dir = 0; static const double tiny = 1e-7; @@ -97,9 +100,9 @@ static int rotary_unlock_for_traverse = -1; #define TO_PROG_ANG(deg) (deg) /* macros for converting program units to internal (mm/deg) units */ -#define FROM_PROG_LEN(prog) ((prog) * (lengthUnits == CANON_UNITS_INCHES ? 25.4 : lengthUnits == CANON_UNITS_CM ? 10.0 : 1.0)) #define FROM_PROG_ANG(prog) (prog) + /* Certain axes are periodic. Hardcode this for now */ #define IS_PERIODIC(axisnum) \ ((axisnum) == 3 || (axisnum) == 4 || (axisnum) == 5) @@ -137,6 +140,7 @@ static void flush_segments(void); in the 6-axis canon.hh. So, we declare them here now. */ extern void CANON_ERROR(const char *fmt, ...) __attribute__((format(printf,1,2))); +extern void CANON_MSG(const char *fmt, ...) __attribute__((format(printf,1,2))); /* Origin offsets, length units, and active plane are all maintained @@ -159,6 +163,19 @@ static CANON_POSITION g92Offset(0.0, 0.0, 0.0, static CANON_UNITS lengthUnits = CANON_UNITS_MM; static CANON_PLANE activePlane = CANON_PLANE_XY; +inline static double FROM_PROG_LEN(double prog_len) +{ + switch (lengthUnits) { + case CANON_UNITS_INCHES: + return prog_len * 25.4; + case CANON_UNITS_CM: + return prog_len * 10.0; + case CANON_UNITS_MM: + return prog_len; + } + return prog_len; +} + static int feed_mode = 0; static int synched = 0; @@ -439,7 +456,7 @@ static double canonMotionTolerance = 0.0; static double canonNaivecamTolerance = 0.0; /* Spindle speed is saved here */ -static double spindleSpeed = 0.0; // always positive +static double spindleSpeed_rpm = 0.0; // always positive /* Prepped tool is saved here */ static int preppedTool = 0; @@ -454,6 +471,8 @@ static bool block_delete = ON; //set enabled by default (previous EMC behaviour) Feed rate is saved here; values are in mm/sec or deg/sec. It will be initially set in INIT_CANON() below. */ +static double uuPerRev_vel = 0.0; +static double uuPerRev_pos = 0.0; static double currentLinearFeedRate = 0.0; static double currentAngularFeedRate = 0.0; @@ -464,6 +483,30 @@ static double currentAngularFeedRate = 0.0; static int cartesian_move = 0; static int angular_move = 0; +enum FeedRateType +{ + FEED_LINEAR, + FEED_ANGULAR +}; + +/** + * Get the nominal feed rate currently active. + * If spindle sync is active, this function estimates the equivalent feed rate under ideal conditions. + */ +static double getActiveFeedRate(FeedRateType angular) +{ + double uuPerRev = feed_mode ? uuPerRev_vel : uuPerRev_pos; + double uu_per_sec = uuPerRev * spindleSpeed_rpm / 60.0; + + //KLUDGE relies on external state here + if (!angular) { + return synched ? FROM_PROG_LEN(uu_per_sec) : currentLinearFeedRate; + } else { + return synched ? FROM_PROG_ANG(uu_per_sec) : currentAngularFeedRate; + } +} + + static double toExtVel(double vel) { if (cartesian_move && !angular_move) { return TO_EXT_LEN(vel); @@ -490,7 +533,7 @@ static void send_g5x_msg(int index) { set_g5x_msg.origin = to_ext_pose(g5xOffset); if(css_maximum) { - SET_SPINDLE_SPEED(spindleSpeed); + SET_SPINDLE_SPEED(spindleSpeed_rpm); } interp_list.append(set_g5x_msg); } @@ -505,7 +548,7 @@ static void send_g92_msg(void) { set_g92_msg.origin = to_ext_pose(g92Offset); if(css_maximum) { - SET_SPINDLE_SPEED(spindleSpeed); + SET_SPINDLE_SPEED(spindleSpeed_rpm); } interp_list.append(set_g92_msg); } @@ -561,29 +604,30 @@ void SET_FEED_MODE(int mode) { flush_segments(); feed_mode = mode; if(feed_mode == 0) STOP_SPEED_FEED_SYNCH(); + canon_debug("setting feed mode %d\n", mode); } void SET_FEED_RATE(double rate) { if(feed_mode) { - START_SPEED_FEED_SYNCH(rate, 1); - currentLinearFeedRate = rate; + START_SPEED_FEED_SYNCH(rate, 1); + uuPerRev_vel = rate; } else { - /* convert from /min to /sec */ - rate /= 60.0; + /* convert from /min to /sec */ + rate /= 60.0; - /* convert to traj units (mm & deg) if needed */ - double newLinearFeedRate = FROM_PROG_LEN(rate), - newAngularFeedRate = FROM_PROG_ANG(rate); + /* convert to traj units (mm & deg) if needed */ + double newLinearFeedRate = FROM_PROG_LEN(rate), + newAngularFeedRate = FROM_PROG_ANG(rate); - if(newLinearFeedRate != currentLinearFeedRate - || newAngularFeedRate != currentAngularFeedRate) - flush_segments(); + if(newLinearFeedRate != currentLinearFeedRate + || newAngularFeedRate != currentAngularFeedRate) + flush_segments(); - currentLinearFeedRate = newLinearFeedRate; - currentAngularFeedRate = newAngularFeedRate; + currentLinearFeedRate = newLinearFeedRate; + currentAngularFeedRate = newAngularFeedRate; } } @@ -732,8 +776,8 @@ static AccelData getStraightAcceleration(CANON_POSITION pos) } static VelData getStraightVelocity(double x, double y, double z, - double a, double b, double c, - double u, double v, double w) + double a, double b, double c, + double u, double v, double w) { double dx, dy, dz, da, db, dc, du, dv, dw; double tx, ty, tz, ta, tb, tc, tu, tv, tw; @@ -742,7 +786,7 @@ static VelData getStraightVelocity(double x, double y, double z, /* If we get a move to nowhere (!cartesian_move && !angular_move) we might as well go there at the currentLinearFeedRate... */ - out.vel = currentLinearFeedRate; + out.vel = getActiveFeedRate(FEED_LINEAR); out.tmax = 0; out.dtot = 0; @@ -801,7 +845,7 @@ static VelData getStraightVelocity(double x, double y, double z, out.dtot = sqrt(du * du + dv * dv + dw * dw); if (out.tmax <= 0.0) { - out.vel = currentLinearFeedRate; + out.vel = getActiveFeedRate(FEED_LINEAR); } else { out.vel = out.dtot / out.tmax; } @@ -815,7 +859,7 @@ static VelData getStraightVelocity(double x, double y, double z, out.dtot = sqrt(da * da + db * db + dc * dc); if (out.tmax <= 0.0) { - out.vel = currentAngularFeedRate; + out.vel = getActiveFeedRate(FEED_ANGULAR); } else { out.vel = out.dtot / out.tmax; } @@ -851,7 +895,7 @@ static VelData getStraightVelocity(double x, double y, double z, out.dtot = sqrt(du * du + dv * dv + dw * dw); if (out.tmax <= 0.0) { - out.vel = currentLinearFeedRate; + out.vel = getActiveFeedRate(FEED_LINEAR); } else { out.vel = out.dtot / out.tmax; } @@ -897,22 +941,13 @@ static void flush_segments(void) { #endif VelData linedata = getStraightVelocity(x, y, z, a, b, c, u, v, w); - double vel = linedata.vel; + double ini_maxvel = linedata.vel; - if (cartesian_move && !angular_move) { - if (vel > currentLinearFeedRate) { - vel = currentLinearFeedRate; - } - } else if (!cartesian_move && angular_move) { - if (vel > currentAngularFeedRate) { - vel = currentAngularFeedRate; - } - } else if (cartesian_move && angular_move) { - if (vel > currentLinearFeedRate) { - vel = currentLinearFeedRate; - } - } + double nominal_vel = getActiveFeedRate(static_cast(!cartesian_move && angular_move)); + canon_debug("in flush_segments: got vel %f, nominal_vel %f\n", vel, nominal_vel); + double vel = std::min(ini_maxvel, nominal_vel); + limitSpindleSpeedFromVel(nominal_vel, ini_maxvel); EMC_TRAJ_LINEAR_MOVE linearMoveMsg; linearMoveMsg.feed_mode = feed_mode; @@ -932,7 +967,7 @@ static void flush_segments(void) { linearMoveMsg.end.c = TO_EXT_ANG(c); linearMoveMsg.vel = toExtVel(vel); - linearMoveMsg.ini_maxvel = toExtVel(linedata.vel); + linearMoveMsg.ini_maxvel = toExtVel(ini_maxvel); AccelData lineaccdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w); double acc = lineaccdata.acc; linearMoveMsg.acc = toExtAcc(acc); @@ -1057,7 +1092,7 @@ void STRAIGHT_TRAVERSE(int line_number, int old_feed_mode = feed_mode; if(feed_mode) - STOP_SPEED_FEED_SYNCH(); + STOP_SPEED_FEED_SYNCH(); if(vel && acc) { interp_list.set_line_number(line_number); @@ -1065,7 +1100,7 @@ void STRAIGHT_TRAVERSE(int line_number, } if(old_feed_mode) - START_SPEED_FEED_SYNCH(currentLinearFeedRate, 1); + START_SPEED_FEED_SYNCH(uuPerRev_vel, 1); canonUpdateEndPoint(x, y, z, a, b, c, u, v, w); } @@ -1134,7 +1169,6 @@ void STRAIGHT_PROBE(int line_number, double u, double v, double w, unsigned char probe_type) { - double ini_maxvel, vel, acc; EMC_TRAJ_PROBE probeMsg; from_prog(x,y,z,a,b,c,u,v,w); @@ -1143,24 +1177,14 @@ void STRAIGHT_PROBE(int line_number, flush_segments(); VelData veldata = getStraightVelocity(x, y, z, a, b, c, u, v, w); - ini_maxvel = vel = veldata.vel; + double ini_maxvel = veldata.vel; - if (cartesian_move && !angular_move) { - if (vel > currentLinearFeedRate) { - vel = currentLinearFeedRate; - } - } else if (!cartesian_move && angular_move) { - if (vel > currentAngularFeedRate) { - vel = currentAngularFeedRate; - } - } else if (cartesian_move && angular_move) { - if (vel > currentLinearFeedRate) { - vel = currentLinearFeedRate; - } - } + double nominal_vel = getActiveFeedRate(static_cast(!cartesian_move && angular_move)); + double vel = std::min(ini_maxvel, nominal_vel); + limitSpindleSpeedFromVel(nominal_vel, ini_maxvel); AccelData accdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w); - acc = accdata.acc; + double acc = accdata.acc; probeMsg.vel = toExtVel(vel); probeMsg.ini_maxvel = toExtVel(ini_maxvel); @@ -1233,11 +1257,16 @@ void STOP_CUTTER_RADIUS_COMPENSATION() // nothing need be done here } - - void START_SPEED_FEED_SYNCH(double feed_per_revolution, bool velocity_mode) { + canon_debug("Setting sync, feed_per_revolution = %f, velocity_mode = %d\n", feed_per_revolution, velocity_mode); flush_segments(); + // KLUDGE often we pass these values in from internal state, redundant and bug-prone + if (velocity_mode) { + uuPerRev_vel = feed_per_revolution; + } else { + uuPerRev_pos = feed_per_revolution; + } EMC_TRAJ_SET_SPINDLESYNC spindlesyncMsg; spindlesyncMsg.feed_per_revolution = TO_EXT_LEN(FROM_PROG_LEN(feed_per_revolution)); spindlesyncMsg.velocity_mode = velocity_mode; @@ -1776,8 +1805,13 @@ void ARC_FEED(int line_number, double a_max = total_xyz_length / tt_max; // Limit velocity by maximum - double vel = MIN(currentLinearFeedRate, v_max); - canon_debug("current F = %f\n",currentLinearFeedRate); + double nominal_vel = getActiveFeedRate(FEED_LINEAR); + double vel = MIN(nominal_vel, v_max); + + // Make sure spindle speed is within range (for spindle_sync motion only) + limitSpindleSpeedFromVel(nominal_vel, v_max); + + canon_debug("current F = %f\n", nominal_vel); canon_debug("vel = %f\n",vel); canon_debug("v_max = %f\n",v_max); @@ -1852,6 +1886,30 @@ void SET_SPINDLE_MODE(double css_max) { css_maximum = fabs(css_max); } + +/** + * Hack to add spindle speed information to an existing spindle msg. + * Using a template gives us static polymorphism, since all the spindle + * messages derive from EMC_SPINDLE_SPEED. + */ +template +void buildSpindleCmd(SpindleMsg & msg, double spindle_rpm) +{ + double css_numerator = 0.0; + if(css_maximum > 0.0) { + if(lengthUnits == CANON_UNITS_INCHES) + css_numerator = 12 / (2.0 * M_PI) * spindle_rpm * TO_EXT_LEN(25.4); + else + css_numerator = 1000 / (2.0 * M_PI) * spindle_rpm * TO_EXT_LEN(1); + msg.speed = spindle_dir * css_maximum; + msg.factor = spindle_dir * css_numerator; + msg.xoffset = TO_EXT_LEN(g5xOffset.x + g92Offset.x + currentToolOffset.tran.x); + } else { + msg.speed = spindle_dir * spindle_rpm; + } + +} + void START_SPINDLE_CLOCKWISE() { EMC_SPINDLE_ON emc_spindle_on_msg; @@ -1859,18 +1917,7 @@ void START_SPINDLE_CLOCKWISE() flush_segments(); spindle_dir = 1; - if(css_maximum) { - if(lengthUnits == CANON_UNITS_INCHES) - css_numerator = 12 / (2 * M_PI) * spindleSpeed * TO_EXT_LEN(25.4); - else - css_numerator = 1000 / (2 * M_PI) * spindleSpeed * TO_EXT_LEN(1); - emc_spindle_on_msg.speed = spindle_dir * css_maximum; - emc_spindle_on_msg.factor = spindle_dir * css_numerator; - emc_spindle_on_msg.xoffset = TO_EXT_LEN(g5xOffset.x + g92Offset.x + currentToolOffset.tran.x); - } else { - emc_spindle_on_msg.speed = spindle_dir * spindleSpeed; - css_numerator = 0; - } + buildSpindleCmd(emc_spindle_on_msg, spindleSpeed_rpm); interp_list.append(emc_spindle_on_msg); } @@ -1880,45 +1927,55 @@ void START_SPINDLE_COUNTERCLOCKWISE() flush_segments(); spindle_dir = -1; + buildSpindleCmd(emc_spindle_on_msg, spindleSpeed_rpm); - if(css_maximum) { - if(lengthUnits == CANON_UNITS_INCHES) - css_numerator = 12 / (2 * M_PI) * spindleSpeed * TO_EXT_LEN(25.4); - else - css_numerator = 1000 / (2 * M_PI) * spindleSpeed * TO_EXT_LEN(1); - emc_spindle_on_msg.speed = spindle_dir * css_maximum; - emc_spindle_on_msg.factor = spindle_dir * css_numerator; - emc_spindle_on_msg.xoffset = TO_EXT_LEN(g5xOffset.x + g92Offset.x + currentToolOffset.tran.x); - } else { - emc_spindle_on_msg.speed = spindle_dir * spindleSpeed; - css_numerator = 0; - } interp_list.append(emc_spindle_on_msg); } void SET_SPINDLE_SPEED(double r) { // speed is in RPMs everywhere - spindleSpeed = fabs(r); // interp will never send negative anyway ... + spindleSpeed_rpm = fabs(r); // interp will never send negative anyway ... EMC_SPINDLE_SPEED emc_spindle_speed_msg; flush_segments(); - if(css_maximum) { - if(lengthUnits == CANON_UNITS_INCHES) - css_numerator = 12 / (2 * M_PI) * spindleSpeed * TO_EXT_LEN(25.4); - else - css_numerator = 1000 / (2 * M_PI) * spindleSpeed * TO_EXT_LEN(1); - emc_spindle_speed_msg.speed = spindle_dir * css_maximum; - emc_spindle_speed_msg.factor = spindle_dir * css_numerator; - emc_spindle_speed_msg.xoffset = TO_EXT_LEN(g5xOffset.x + g92Offset.x + currentToolOffset.tran.x); - } else { - emc_spindle_speed_msg.speed = spindle_dir * spindleSpeed; - css_numerator = 0; + buildSpindleCmd(emc_spindle_speed_msg, spindleSpeed_rpm); + interp_list.append(emc_spindle_speed_msg); + +} + +/** + * Hack way to immediately update the spindle speed without flushing segments first (typically called within flush segments as a way to limit spindle speed during threading). + * @warning does NOT update the interpreter state. + */ +static bool limitSpindleSpeed(double rpm) +{ + // Don't care about fractional RPM here, the limit speed has a safety factor anyway + double limit_rpm = ceil(fabs(rpm)); + if (spindleSpeed_rpm <= limit_rpm) { + // spindle speed within range, do nothing + return false; } + EMC_SPINDLE_SPEED emc_spindle_speed_msg; + CANON_MSG("Reducing spindle speed from %0.0f to %0.0f for synched motion\n", spindleSpeed_rpm, limit_rpm); + + buildSpindleCmd(emc_spindle_speed_msg, limit_rpm); interp_list.append(emc_spindle_speed_msg); - + return true; +} + +static bool limitSpindleSpeedFromVel(double nominal_vel, double max_vel) +{ + if (!synched || nominal_vel <= 0.0) { + return false; + } + const static double SPINDLE_SYNCH_MARGIN = 0.05; + + // Scale down spindle RPM in proportion to the maximum allowed spindle velocity, with a safety factor + double maxSpindleRPM = spindleSpeed_rpm * max_vel / nominal_vel * (1.0 - SPINDLE_SYNCH_MARGIN); + return limitSpindleSpeed(maxSpindleRPM); } void STOP_SPINDLE_TURNING() @@ -2020,7 +2077,7 @@ void USE_TOOL_LENGTH_OFFSET(EmcPose offset) set_offset_msg.offset.w = TO_EXT_LEN(currentToolOffset.w); if(css_maximum) { - SET_SPINDLE_SPEED(spindleSpeed); + SET_SPINDLE_SPEED(spindleSpeed_rpm); } interp_list.append(set_offset_msg); } @@ -2098,7 +2155,7 @@ void CHANGE_TOOL(int slot) interp_list.append(linearMoveMsg); if(old_feed_mode) - START_SPEED_FEED_SYNCH(currentLinearFeedRate, 1); + START_SPEED_FEED_SYNCH(uuPerRev_vel, 1); canonUpdateEndPoint(x, y, z, a, b, c, u, v, w); } @@ -2534,12 +2591,14 @@ void INIT_CANON() activePlane = CANON_PLANE_XY; canonUpdateEndPoint(0, 0, 0, 0, 0, 0, 0, 0, 0); SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, 0); - spindleSpeed = 0.0; + spindleSpeed_rpm = 0.0; preppedTool = 0; cartesian_move = 0; angular_move = 0; currentLinearFeedRate = 0.0; currentAngularFeedRate = 0.0; + uuPerRev_vel = 0.0; + uuPerRev_pos = 0.0; ZERO_EMC_POSE(currentToolOffset); /* to set the units, note that GET_EXTERNAL_LENGTH_UNITS() returns @@ -2565,20 +2624,39 @@ void CANON_ERROR(const char *fmt, ...) va_list ap; EMC_OPERATOR_ERROR operator_error_msg; - flush_segments(); - operator_error_msg.id = 0; if (fmt != NULL) { - va_start(ap, fmt); - vsnprintf(operator_error_msg.error,sizeof(operator_error_msg.error), fmt, ap); - va_end(ap); + va_start(ap, fmt); + vsnprintf(operator_error_msg.error,sizeof(operator_error_msg.error), fmt, ap); + va_end(ap); } else { - operator_error_msg.error[0] = 0; + operator_error_msg.error[0] = 0; } interp_list.append(operator_error_msg); } +/** + * KLUDGE like CANON_ERROR but for sending an info message to the user. + */ +void CANON_MSG(const char *fmt, ...) +{ + va_list ap; + EMC_OPERATOR_TEXT operator_text_msg; + + operator_text_msg.id = 0; + if (fmt != NULL) { + va_start(ap, fmt); + vsnprintf(operator_text_msg.text,sizeof(operator_text_msg.text), fmt, ap); + va_end(ap); + } else { + operator_text_msg.text[0] = 0; + } + + interp_list.append(operator_text_msg); +} + + /* GET_EXTERNAL_TOOL_TABLE(int pocket) @@ -2687,7 +2765,7 @@ double GET_EXTERNAL_FEED_RATE() // We're in G95 "Units per Revolution" mode, so // currentLinearFeedRate is the FPR and we should just return // it, unchanged. - feed = currentLinearFeedRate; + feed = uuPerRev_vel; } else { // We're in G94 "Units per Minute" mode so unhork // currentLinearFeedRate before returning it, by converting @@ -2756,7 +2834,7 @@ int GET_EXTERNAL_FLOOD() double GET_EXTERNAL_SPEED() { // speed is in RPMs everywhere - return spindleSpeed; + return spindleSpeed_rpm; } CANON_DIRECTION GET_EXTERNAL_SPINDLE() @@ -3250,12 +3328,12 @@ int UNLOCK_ROTARY(int line_number, int axis) { // issue it int old_feed_mode = feed_mode; if(feed_mode) - STOP_SPEED_FEED_SYNCH(); + STOP_SPEED_FEED_SYNCH(); interp_list.set_line_number(line_number); interp_list.append(m); // no need to update endpoint if(old_feed_mode) - START_SPEED_FEED_SYNCH(currentLinearFeedRate, 1); + START_SPEED_FEED_SYNCH(uuPerRev_vel, 1); // now, the next move is the real indexing move, so be ready rotary_unlock_for_traverse = axis; diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 0e9a8cf46d5..f7cd1d4685d 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1395,7 +1395,7 @@ int emcSpindleAbort(void) int emcSpindleSpeed(double speed, double css_factor, double offset) { - if (emcmotStatus.spindle.speed == 0) + if (emcmotStatus.spindle_cmd.velocity_rpm_out == 0) return 0; //spindle stopped, not updating speed return emcSpindleOn(speed, css_factor, offset); @@ -1505,12 +1505,21 @@ int emcMotionUpdate(EMC_MOTION_STAT * stat) stat->echo_serial_number = localMotionEchoSerialNumber; stat->debug = emcmotConfig.debug; - stat->spindle.enabled = emcmotStatus.spindle.speed != 0; - 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; + stat->spindle.enabled = emcmotStatus.spindle_cmd.velocity_rpm_out != 0; + stat->spindle.speed = emcmotStatus.spindle_cmd.velocity_rpm_out; + stat->spindle.brake = emcmotStatus.spindle_cmd.brake; + + // Report spindle direction based on commanded velocity + if (stat->spindle.speed > 0.0) { + stat->spindle.direction = SPINDLE_FORWARD; + } else if (stat->spindle.speed < 0.0){ + stat->spindle.direction = SPINDLE_REVERSE; + } else { + stat->spindle.direction = SPINDLE_STOPPED; + } + + stat->spindle.orient_state = emcmotStatus.spindle_cmd.orient_state; + stat->spindle.orient_fault = emcmotStatus.spindle_cmd.orient_fault; for (dio = 0; dio < EMC_MAX_DIO; dio++) { stat->synch_di[dio] = emcmotStatus.synch_di[dio]; diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index 52ae4561c32..b64eb21ef5b 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -1824,3 +1824,29 @@ double pmCircleEffectiveMinRadius(PmCircle const * const circle) effective_radius); return effective_radius; } + +double findTrapezoidalDesiredVel(double a_max, + double dx, + double v_final, + double v_current, + double cycle_time) +{ + double dt = fmax(cycle_time, TP_TIME_EPSILON); + // Discriminant is 3 terms (when final velocity is non-zero) + double discr_term1 = pmSq(v_final); + double discr_term2 = a_max * (2.0 * dx - v_current * dt); + double tmp_adt = a_max * dt * 0.5; + double discr_term3 = pmSq(tmp_adt); + double discr = discr_term1 + discr_term2 + discr_term3; + + //Start with -B/2 portion of quadratic formula + double maxnewvel = -tmp_adt; + + //If the discriminant term brings our velocity above zero, add it to the total + //We can ignore the calculation otherwise because negative velocities are clipped to zero + if (discr > discr_term3) { + maxnewvel += pmSqrt(discr); + } + + return maxnewvel; +} diff --git a/src/emc/tp/blendmath.h b/src/emc/tp/blendmath.h index 1ce9e4934fa..742a2cc7b4b 100644 --- a/src/emc/tp/blendmath.h +++ b/src/emc/tp/blendmath.h @@ -148,6 +148,12 @@ int findAccelScale(PmCartesian const * const acc, PmCartesian const * const bounds, PmCartesian * const scale); +double findTrapezoidalDesiredVel(double a_max, + double dx, + double v_final, + double currentvel, + double cycle_time); + int pmCartCartParallel(PmCartesian const * const v1, PmCartesian const * const v2, double tol); diff --git a/src/emc/tp/math_util.h b/src/emc/tp/math_util.h new file mode 100644 index 00000000000..127fd22466f --- /dev/null +++ b/src/emc/tp/math_util.h @@ -0,0 +1,15 @@ +#ifndef MATH_UTIL_H +#define MATH_UTIL_H + +inline long max(long y, long x) { + return y > x ? y : x; +} +inline long min(long y, long x) { + return y < x ? y : x; +} + +inline double signum(double x) { + return (x > 0.0) ? 1.0 : (x < 0.0) ? -1.0 : 0.0; +} + +#endif // MATH_UTIL_H diff --git a/src/emc/tp/tc_types.h b/src/emc/tp/tc_types.h index 58a1bf038fa..3d3fbac770c 100644 --- a/src/emc/tp/tc_types.h +++ b/src/emc/tp/tc_types.h @@ -106,7 +106,7 @@ typedef struct { PmCartesian abc; PmCartesian uvw; double reversal_target; - double spindlerevs_at_reversal; + double spindlepos_at_reversal; RIGIDTAP_STATE state; } PmRigidTap; @@ -116,6 +116,7 @@ typedef struct { double target; // actual segment length double progress; // where are we in the segment? 0..target double nominal_length; + double progress_at_sync; // When did we sync up with the spindle? //Velocity double reqvel; // vel requested by F word, calc'd by task diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index b7a018ea35d..ea90cf0472b 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -21,6 +21,7 @@ #include "motion_types.h" #include "spherical_arc.h" #include "blendmath.h" +#include "math_util.h" //KLUDGE Don't include all of emc.hh here, just hand-copy the TERM COND //definitions until we can break the emc constants out into a separate file. //#include "emc.hh" @@ -67,6 +68,8 @@ STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT con STATIC int tpAdjustAccelForTangent(TP_STRUCT const * const, TC_STRUCT * const tc, double normal_acc_scale); + +STATIC int tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc); /** * @section tpcheck Internal state check functions. * These functions compartmentalize some of the messy state checks. @@ -81,9 +84,14 @@ STATIC int tpAdjustAccelForTangent(TP_STRUCT const * const, */ STATIC int tcCheckLastParabolic(TC_STRUCT * const tc, TC_STRUCT const * const prev_tc) { + if (!tc) {return TP_ERR_FAIL;} + if (prev_tc && prev_tc->term_cond == TC_TERM_COND_PARABOLIC) { - tp_debug_print("prev segment parabolic, flagging blend_prev\n"); + tp_debug_print("Found parabolic blend between %d and %d, flagging blend_prev\n", + prev_tc->id, tc->id); tc->blend_prev = 1; + } else { + tc->blend_prev = 0; } return TP_ERR_OK; } @@ -223,13 +231,33 @@ STATIC inline double tpGetRealTargetVel(TP_STRUCT const * const tp, return fmin(v_target * tpGetFeedScale(tp,tc), tpGetMaxTargetVel(tp, tc)); } +STATIC inline double getMaxFeedScale(TC_STRUCT const * tc) +{ + //All reasons to disable feed override go here + if (tc && tc->synchronized == TC_SYNC_POSITION ) { + return 1.0; + } else { + return emcmotConfig->maxFeedScale; + } +} + +STATIC inline double getMaxBlendFeedScale(TC_STRUCT const * prev_tc, TC_STRUCT const * tc) +{ + //All reasons to disable feed override go here + if ((tc && tc->synchronized == TC_SYNC_POSITION) || + (prev_tc && prev_tc->synchronized == TC_SYNC_POSITION)) { + return 1.0; + } else { + return emcmotConfig->maxFeedScale; + } +} /** * Get the worst-case target velocity for a segment based on the trajectory planner state. */ STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT const * const tc) { - double max_scale = emcmotConfig->maxFeedScale; + double max_scale = getMaxFeedScale(tc); if (tc->is_blending) { //KLUDGE: Don't allow feed override to keep blending from overruning max velocity max_scale = fmin(max_scale, 1.0); @@ -303,9 +331,14 @@ STATIC inline double tpGetScaledAccel(TP_STRUCT const * const tp, } /** - * Convert the 2-part spindle position and sign to a signed double. + * Convert a raw spindle position into a "progress" position in the current spindle direction. + * In other words, how far has the spindle moved in the indicated direction? + * + * @param spindle_pos signed raw spindle position (typically from motion) + * @param spindle_dir commanded spindle direction + * @return Spindle progress, POSITIVE if the position and direction have the same sign, NEGATIVE otherwise. */ -STATIC inline double tpGetSignedSpindlePosition(double spindle_pos, int spindle_dir) { +STATIC inline double getSpindleProgress(double spindle_pos, int spindle_dir) { if (spindle_dir < 0.0) { spindle_pos*=-1.0; } @@ -386,7 +419,7 @@ int tpClear(TP_STRUCT * const tp) tp->pausing = 0; tp->synchronized = 0; tp->uu_per_rev = 0.0; - emcmotStatus->spindleSync = 0; + emcmotStatus->spindle_fb.synced = 0; emcmotStatus->current_vel = 0.0; emcmotStatus->requested_vel = 0.0; emcmotStatus->distance_to_go = 0.0; @@ -418,7 +451,6 @@ int tpInit(TP_STRUCT * const tp) tp->wDotMax = 0.0; tp->spindle.offset = 0.0; - tp->spindle.revs = 0.0; tp->spindle.waiting_for_index = MOTION_INVALID_ID; tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID; @@ -811,7 +843,7 @@ STATIC int tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, tc, &acc_bound, &vel_bound, - emcmotConfig->maxFeedScale); + getMaxBlendFeedScale(prev_tc, tc)); if (res_init != TP_ERR_OK) { tp_debug_print("blend init failed with code %d, aborting blend arc\n", @@ -930,9 +962,6 @@ STATIC int tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, tp_debug_print("Passed all tests, updating segments\n"); - //Cleanup any mess from parabolic - tc->blend_prev = 0; - //TODO refactor to pass consume to connect function if (param.consume) { //Since we're consuming the previous segment, pop the last line off of the queue @@ -974,7 +1003,7 @@ STATIC int tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, tc, &acc_bound, &vel_bound, - emcmotConfig->maxFeedScale); + getMaxBlendFeedScale(prev_tc, tc)); if (res_init != TP_ERR_OK) { tp_debug_print("blend init failed with code %d, aborting blend arc\n", res_init); @@ -1089,7 +1118,6 @@ STATIC int tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, tcSetLineXYZ(tc, &line2_temp); //Cleanup any mess from parabolic - tc->blend_prev = 0; tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT); return TP_ERR_OK; } @@ -1124,7 +1152,7 @@ STATIC int tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, tc, &acc_bound, &vel_bound, - emcmotConfig->maxFeedScale); + getMaxBlendFeedScale(prev_tc, tc)); if (res_init != TP_ERR_OK) { tp_debug_print("blend init failed with code %d, aborting blend arc\n", @@ -1149,8 +1177,6 @@ STATIC int tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, return TP_ERR_FAIL; } - - int res_param = blendComputeParameters(¶m); int res_points = blendFindPoints3(&points_approx, &geom, ¶m); @@ -1258,10 +1284,7 @@ STATIC int tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, tcSetCircleXYZ(prev_tc, &circ1_temp); tcSetCircleXYZ(tc, &circ2_temp); - //Cleanup any mess from parabolic - tc->blend_prev = 0; tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT); - return TP_ERR_OK; } @@ -1287,7 +1310,7 @@ STATIC int tpCreateLineLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc tc, &acc_bound, &vel_bound, - emcmotConfig->maxFeedScale); + getMaxBlendFeedScale(prev_tc, tc)); if (res_init != TP_ERR_OK) { tp_debug_print("blend init failed with code %d, aborting blend arc\n", @@ -1376,14 +1399,21 @@ STATIC inline int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc return TP_ERR_OK; } -STATIC int tpCheckCanonType(TC_STRUCT * const prev_tc, TC_STRUCT const * const tc) +STATIC int handleModeChange(TC_STRUCT * const prev_tc, TC_STRUCT const * const tc) { if (!tc || !prev_tc) { return TP_ERR_FAIL; } if ((prev_tc->canon_motion_type == EMC_MOTION_TYPE_TRAVERSE) ^ (tc->canon_motion_type == EMC_MOTION_TYPE_TRAVERSE)) { - tp_debug_print("Can't blend between rapid and feed move, aborting arc\n"); + tp_debug_print("Blending disabled: can't blend between rapid and feed motions\n"); + tcSetTermCond(prev_tc, TC_TERM_COND_STOP); + } + if (prev_tc->synchronized != TC_SYNC_POSITION && + tc->synchronized == TC_SYNC_POSITION) { + tp_debug_print("Blending disabled: changing spindle sync mode from %d to %d\n", + prev_tc->synchronized, + tc->synchronized); tcSetTermCond(prev_tc, TC_TERM_COND_STOP); } return TP_ERR_OK; @@ -1400,6 +1430,46 @@ STATIC int tpSetupSyncedIO(TP_STRUCT * const tp, TC_STRUCT * const tc) { } } +STATIC int tcUpdateArcLengthFit(TC_STRUCT * const tc) +{ + if (!tc) {return -1;} + + switch (tc->motion_type) { + case TC_CIRCULAR: + findSpiralArcLengthFit(&tc->coords.circle.xyz, &tc->coords.circle.fit); + break; + case TC_LINEAR: + case TC_RIGIDTAP: + case TC_SPHERICAL: + default: + break; + } + return 0; +} + +STATIC int tpFinalizeAndEnqueue(TP_STRUCT * const tp, TC_STRUCT * const tc) +{ + //TODO refactor this into its own function + TC_STRUCT *prev_tc; + prev_tc = tcqLast(&tp->queue); + handleModeChange(prev_tc, tc); + if (emcmotConfig->arcBlendEnable){ + tpHandleBlendArc(tp, tc); + tcUpdateArcLengthFit(tc); + } + tcFlagEarlyStop(prev_tc, tc); + // KLUDGE order is important here, the parabolic blend check has to + // happen after all other steps that affect the terminal condition + tcCheckLastParabolic(tc, prev_tc); + tcFinalizeLength(prev_tc); + + int retval = tpAddSegmentToQueue(tp, tc, true); + //Run speed optimization (will abort safely if there are no tangent segments) + tpRunOptimization(tp); + + return retval; +} + /** * Adds a rigid tap cycle to the motion queue. @@ -1451,14 +1521,7 @@ int tpAddRigidTap(TP_STRUCT * const tp, EmcPose end, double vel, double ini_maxv // Force exact stop mode after rigid tapping regardless of TP setting tcSetTermCond(&tc, TC_TERM_COND_STOP); - TC_STRUCT *prev_tc; - //Assume non-zero error code is failure - prev_tc = tcqLast(&tp->queue); - tcFinalizeLength(prev_tc); - tcFlagEarlyStop(prev_tc, &tc); - int retval = tpAddSegmentToQueue(tp, &tc, true); - tpRunOptimization(tp); - return retval; + return tpFinalizeAndEnqueue(tp, &tc); } STATIC blend_type_t tpCheckBlendArcType(TP_STRUCT const * const tp, @@ -1725,8 +1788,8 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp, // Calculate instantaneous acceleration required for change in direction // from v1 to v2, assuming constant speed - double v_max1 = fmin(prev_tc->maxvel, prev_tc->reqvel * emcmotConfig->maxFeedScale); - double v_max2 = fmin(tc->maxvel, tc->reqvel * emcmotConfig->maxFeedScale); + double v_max1 = fmin(prev_tc->maxvel, prev_tc->reqvel * getMaxFeedScale(prev_tc)); + double v_max2 = fmin(tc->maxvel, tc->reqvel * getMaxFeedScale(tc)); double v_max = fmin(v_max1, v_max2); tp_debug_print("tangent v_max = %f\n",v_max); @@ -1847,6 +1910,7 @@ STATIC int tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) { if (res_create == TP_ERR_OK) { //Need to do this here since the length changed + tcCheckLastParabolic(&blend_tc, prev_tc); tpAddSegmentToQueue(tp, &blend_tc, false); } else { return res_create; @@ -1905,22 +1969,7 @@ int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, double v // For linear move, set rotary axis settings tc.indexrotary = indexrotary; - //TODO refactor this into its own function - TC_STRUCT *prev_tc; - prev_tc = tcqLast(&tp->queue); - tpCheckCanonType(prev_tc, &tc); - if (emcmotConfig->arcBlendEnable){ - tpHandleBlendArc(tp, &tc); - } - tcCheckLastParabolic(&tc, prev_tc); - tcFinalizeLength(prev_tc); - tcFlagEarlyStop(prev_tc, &tc); - - int retval = tpAddSegmentToQueue(tp, &tc, true); - //Run speed optimization (will abort safely if there are no tangent segments) - tpRunOptimization(tp); - - return retval; + return tpFinalizeAndEnqueue(tp, &tc); } @@ -1997,22 +2046,7 @@ int tpAddCircle(TP_STRUCT * const tp, v_max_actual, acc); - TC_STRUCT *prev_tc; - prev_tc = tcqLast(&tp->queue); - - tpCheckCanonType(prev_tc, &tc); - if (emcmotConfig->arcBlendEnable){ - tpHandleBlendArc(tp, &tc); - findSpiralArcLengthFit(&tc.coords.circle.xyz, &tc.coords.circle.fit); - } - tcCheckLastParabolic(&tc, prev_tc); - tcFinalizeLength(prev_tc); - tcFlagEarlyStop(prev_tc, &tc); - - int retval = tpAddSegmentToQueue(tp, &tc, true); - - tpRunOptimization(tp); - return retval; + return tpFinalizeAndEnqueue(tp, &tc); } @@ -2145,13 +2179,13 @@ STATIC void tpDebugCycleInfo(TP_STRUCT const * const tp, TC_STRUCT const * const double tc_finalvel = tpGetRealFinalVel(tp, tc, nexttc); /* Debug Output */ - tc_debug_print("tc state: vr = %f, vf = %f, maxvel = %f\n", + tc_debug_print("tc state: vr = %f, vf = %f, maxvel = %f, ", tc_target_vel, tc_finalvel, tc->maxvel); - tc_debug_print(" currentvel = %f, fs = %f, tc = %f, term = %d\n", + tc_debug_print("currentvel = %f, fs = %f, dt = %f, term = %d, ", tc->currentvel, tpGetFeedScale(tp,tc), tc->cycle_time, tc->term_cond); - tc_debug_print(" acc = %f,T = %f, P = %f\n", acc, + tc_debug_print("acc = %f, T = %f, P = %f, ", acc, tc->target, tc->progress); - tc_debug_print(" motion type %d\n", tc->motion_type); + tc_debug_print("motion_type = %d\n", tc->motion_type); if (tc->on_final_decel) { rtapi_print(" on final decel\n"); @@ -2189,30 +2223,7 @@ void tpCalculateTrapezoidalAccel(TP_STRUCT const * const tp, TC_STRUCT * const t double dx = tc->target - tc->progress; double maxaccel = tpGetScaledAccel(tp, tc); - double discr_term1 = pmSq(tc_finalvel); - double discr_term2 = maxaccel * (2.0 * dx - tc->currentvel * tc->cycle_time); - double tmp_adt = maxaccel * tc->cycle_time * 0.5; - double discr_term3 = pmSq(tmp_adt); - - double discr = discr_term1 + discr_term2 + discr_term3; - - // Descriminant is a little more complicated with final velocity term. If - // descriminant < 0, we've overshot (or are about to). Do the best we can - // in this situation -#ifdef TP_PEDANTIC - if (discr < 0.0) { - rtapi_print_msg(RTAPI_MSG_ERR, - "discriminant %f < 0 in velocity calculation!\n", discr); - } -#endif - //Start with -B/2 portion of quadratic formula - double maxnewvel = -tmp_adt; - - //If the discriminant term brings our velocity above zero, add it to the total - //We can ignore the calculation otherwise because negative velocities are clipped to zero - if (discr > discr_term3) { - maxnewvel += pmSqrt(discr); - } + double maxnewvel = findTrapezoidalDesiredVel(maxaccel, dx, tc_finalvel, tc->currentvel, tc->cycle_time); // Find bounded new velocity based on target velocity // Note that we use a separate variable later to check if we're on final decel @@ -2291,6 +2302,54 @@ void tpToggleDIOs(TC_STRUCT * const tc) { } } +//KLUDGE compine this with the version in taskintf +static inline spindle_direction_code_t getSpindleCmdDir() +{ + if (emcmotStatus->spindle_cmd.velocity_rpm_out > 0.0) { + return SPINDLE_FORWARD; + } else if (emcmotStatus->spindle_cmd.velocity_rpm_out < 0.0) { + return SPINDLE_REVERSE; + } else { + return SPINDLE_STOPPED; + } +} + +static inline double findSpindleDisplacement(double new_pos, + double old_pos, + spindle_direction_code_t spindle_cmd_dir) +{ + // Difference assuming spindle "forward" direction + double forward_diff = new_pos - old_pos; + + switch(spindle_cmd_dir) { + case SPINDLE_STOPPED: + case SPINDLE_FORWARD: + return forward_diff; + case SPINDLE_REVERSE: + return -forward_diff; + } + //no default on purpose, compiler should warn on missing states + return 0; +} + +/** + * Helper function to compare commanded and actual spindle velocity. + * If the signs of velocity don't match, then the spindle is reversing direction. + */ +static inline bool spindleReversing() +{ + return (signum(emcmotStatus->spindle_fb.velocity_rpm) != signum(emcmotStatus->spindle_cmd.velocity_rpm_out)); +} + +static inline bool cmdReverseSpindle() +{ + static bool reversed = false; + // Flip sign on commanded velocity + emcmotStatus->spindle_cmd.velocity_rpm_out *= -1; + // (Optional) set an internal flag so we know if the spindle is reversed from the user command + reversed = !reversed; + return reversed; +} /** * Handle special cases for rigid tapping. @@ -2301,26 +2360,24 @@ void tpToggleDIOs(TC_STRUCT * const tc) { STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, TC_STRUCT * const tc) { - static double old_spindlepos; - double new_spindlepos = emcmotStatus->spindleRevs; - if (emcmotStatus->spindle.direction < 0) new_spindlepos = -new_spindlepos; + double spindle_pos = emcmotStatus->spindle_fb.position_rev; switch (tc->coords.rigidtap.state) { case TAPPING: tc_debug_print("TAPPING\n"); if (tc->progress >= tc->coords.rigidtap.reversal_target) { // command reversal - emcmotStatus->spindle.speed *= -1.0; + cmdReverseSpindle(); tc->coords.rigidtap.state = REVERSING; } break; case REVERSING: tc_debug_print("REVERSING\n"); - if (new_spindlepos < old_spindlepos) { + if (!spindleReversing()) { PmCartesian start, end; PmCartLine *aux = &tc->coords.rigidtap.aux_xyz; // we've stopped, so set a new target at the original position - tc->coords.rigidtap.spindlerevs_at_reversal = new_spindlepos + tp->spindle.offset; + tc->coords.rigidtap.spindlepos_at_reversal = spindle_pos; pmCartLinePoint(&tc->coords.rigidtap.xyz, tc->progress, &start); end = tc->coords.rigidtap.xyz.start; @@ -2333,19 +2390,19 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, tc->coords.rigidtap.state = RETRACTION; } - old_spindlepos = new_spindlepos; - tc_debug_print("Spindlepos = %f\n", new_spindlepos); + tc_debug_print("Spindlepos = %f\n", spindle_pos); break; case RETRACTION: tc_debug_print("RETRACTION\n"); if (tc->progress >= tc->coords.rigidtap.reversal_target) { - emcmotStatus->spindle.speed *= -1; + // Flip spindle direction againt to start final reversal + cmdReverseSpindle(); tc->coords.rigidtap.state = FINAL_REVERSAL; } break; case FINAL_REVERSAL: tc_debug_print("FINAL_REVERSAL\n"); - if (new_spindlepos > old_spindlepos) { + if (!spindleReversing()) { PmCartesian start, end; PmCartLine *aux = &tc->coords.rigidtap.aux_xyz; pmCartLinePoint(aux, tc->progress, &start); @@ -2359,7 +2416,6 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, tc->coords.rigidtap.state = FINAL_PLACEMENT; } - old_spindlepos = new_spindlepos; break; case FINAL_PLACEMENT: tc_debug_print("FINAL_PLACEMENT\n"); @@ -2493,7 +2549,7 @@ STATIC int tpCompleteSegment(TP_STRUCT * const tp, // spindle position so the next synced move can be in // the right place. if(tc->synchronized != TC_SYNC_NONE) { - tp->spindle.offset += tc->target / tc->uu_per_rev; + tp->spindle.offset += (tc->target - tc->progress_at_sync) / tc->uu_per_rev; } else { tp->spindle.offset = 0.0; } @@ -2542,7 +2598,7 @@ STATIC int tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc, tp->synchronized = 0; tp->spindle.waiting_for_index = MOTION_INVALID_ID; tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID; - emcmotStatus->spindleSync = 0; + emcmotStatus->spindle_fb.synced = 0; tpResume(tp); return TP_ERR_STOPPED; } //FIXME consistent error codes @@ -2587,15 +2643,14 @@ STATIC int tpCheckAtSpeed(TP_STRUCT * const tp, TC_STRUCT * const tc) } if (MOTION_ID_VALID(tp->spindle.waiting_for_index)) { - if (emcmotStatus->spindle_index_enable) { + if (emcmotStatus->spindle_fb.index_enable) { /* haven't passed index yet */ return TP_ERR_WAITING; } else { /* passed index, start the move */ - emcmotStatus->spindleSync = 1; + emcmotStatus->spindle_fb.synced = 1; tp->spindle.waiting_for_index = MOTION_INVALID_ID; tc->sync_accel = 1; - tp->spindle.revs = 0; } } @@ -2643,7 +2698,7 @@ STATIC int tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { // Do at speed checks that only happen once int needs_atspeed = tc->atspeed || - (tc->synchronized == TC_SYNC_POSITION && !(emcmotStatus->spindleSync)); + (tc->synchronized == TC_SYNC_POSITION && !(emcmotStatus->spindle_fb.synced)); if ( needs_atspeed && !(emcmotStatus->spindle_is_atspeed)) { tp->spindle.waiting_for_atspeed = tc->id; @@ -2659,13 +2714,14 @@ STATIC int tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { return TP_ERR_WAITING; } - // Temporary debug message - tp_debug_print("Activate tc id = %d target_vel = %f req_vel = %f final_vel = %f length = %f\n", + tp_debug_print("Activate tc id = %d target_vel = %f req_vel = %f final_vel = %f length = %f max_acc = %f term_cond = %d\n", tc->id, tc->target_vel, tc->reqvel, tc->finalvel, - tc->target); + tc->target, + tc->maxaccel, + tc->term_cond); tc->active = 1; //Do not change initial velocity here, since tangent blending already sets this up @@ -2673,12 +2729,12 @@ STATIC int tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { tc->blending_next = 0; tc->on_final_decel = 0; - if (TC_SYNC_POSITION == tc->synchronized && !(emcmotStatus->spindleSync)) { - tp_debug_print("Setting up position sync\n"); + if (TC_SYNC_POSITION == tc->synchronized && !(emcmotStatus->spindle_fb.synced)) { + tp_debug_print(" Setting up position sync\n"); // if we aren't already synced, wait tp->spindle.waiting_for_index = tc->id; // ask for an index reset - emcmotStatus->spindle_index_enable = 1; + emcmotStatus->spindle_fb.index_enable = 1; tp->spindle.offset = 0.0; rtapi_print_msg(RTAPI_MSG_DBG, "Waiting on sync...\n"); return TP_ERR_WAITING; @@ -2693,7 +2749,7 @@ STATIC int tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { * Update requested velocity to follow the spindle's velocity (scaled by feed rate). */ STATIC void tpSyncVelocityMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT const * nexttc) { - double speed = emcmotStatus->spindleSpeedIn; + double speed = emcmotStatus->spindle_fb.velocity_rpm; double pos_error = fabs(speed) * tc->uu_per_rev; // Account for movement due to parabolic blending with next segment if(nexttc) { @@ -2710,38 +2766,38 @@ STATIC void tpSyncVelocityMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_ST STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT * const nexttc ) { - double spindle_pos = tpGetSignedSpindlePosition(emcmotStatus->spindleRevs, - emcmotStatus->spindle.direction); - tp_debug_print("Spindle at %f\n",spindle_pos); - double spindle_vel, target_vel; - double oldrevs = tp->spindle.revs; + // Start with raw spindle position and our saved offset + double spindle_pos = emcmotStatus->spindle_fb.position_rev; + // If we're backing out of a hole during rigid tapping, our spindle "displacement" is + // measured relative to spindle position at the bottom of the hole. + // Otherwise, we use the stored spindle offset. + double local_spindle_offset = tp->spindle.offset; if ((tc->motion_type == TC_RIGIDTAP) && (tc->coords.rigidtap.state == RETRACTION || tc->coords.rigidtap.state == FINAL_REVERSAL)) { - tp->spindle.revs = tc->coords.rigidtap.spindlerevs_at_reversal - - spindle_pos; - } else { - tp->spindle.revs = spindle_pos; + local_spindle_offset = tc->coords.rigidtap.spindlepos_at_reversal; } - double pos_desired = (tp->spindle.revs - tp->spindle.offset) * tc->uu_per_rev; - double pos_error = pos_desired - tc->progress; + // Note that this quantity should be non-negative under normal conditions. + double spindle_displacement = findSpindleDisplacement(spindle_pos, + local_spindle_offset, + getSpindleCmdDir()); - if(nexttc) { - pos_error -= nexttc->progress; - } + tc_debug_print("spindle_displacement %f raw_pos %f", spindle_displacement, spindle_pos); + const double spindle_vel = emcmotStatus->spindle_fb.velocity_rpm / 60.0; if(tc->sync_accel) { // detect when velocities match, and move the target accordingly. // acceleration will abruptly stop and we will be on our new target. // FIX: this is driven by TP cycle time, not the segment cycle time - double dt = fmax(tp->cycleTime, TP_TIME_EPSILON); - spindle_vel = tp->spindle.revs / ( dt * tc->sync_accel++); - target_vel = spindle_vel * tc->uu_per_rev; + // Experiment: try syncing with averaged spindle speed + double target_vel = spindle_vel * tc->uu_per_rev; if(tc->currentvel >= target_vel) { tc_debug_print("Hit accel target in pos sync\n"); // move target so as to drive pos_error to 0 next cycle - tp->spindle.offset = tp->spindle.revs - tc->progress / tc->uu_per_rev; + tp->spindle.offset = spindle_pos; + tc->progress_at_sync = tc->progress; + tc_debug_print("Spindle offset %f\n", tp->spindle.offset); tc->sync_accel = 0; tc->target_vel = target_vel; } else { @@ -2750,17 +2806,78 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, tc->target_vel = tc->maxvel; } } else { + // Multiply by user feed rate to get equivalent desired position + double pos_desired = spindle_displacement * tc->uu_per_rev; + double pos_error = pos_desired - (tc->progress - tc->progress_at_sync); + tc_debug_print(" pos_desired %f, progress %f", pos_desired, tc->progress); + + if(nexttc) { + // If we're in a parabolic blend, the next segment will be active too, + // so make sure to account for its progress + tc_debug_print(" nexttc_progress %f", nexttc->progress); + pos_error -= nexttc->progress; + } + tc_debug_print(", pos_error %f\n", pos_error); + // we have synced the beginning of the move as best we can - // track position (minimize pos_error). - tc_debug_print("tracking in pos_sync\n"); - double errorvel; - spindle_vel = (tp->spindle.revs - oldrevs) / tp->cycleTime; - target_vel = spindle_vel * tc->uu_per_rev; - errorvel = pmSqrt(fabs(pos_error) * tpGetScaledAccel(tp,tc)); - if(pos_error<0) { - errorvel *= -1.0; + // This is the velocity we should be at when the position error is c0 + double v_final = spindle_vel * tc->uu_per_rev; + + /* + * Correct for position errors when tracking spindle motion. + * This approach assumes that if position error is 0, the correct + * velocity is just the nominal target velocity. If the position error + * is non-zero, however, then we need to correct it, but then return to + * the nominal velocity. + * + * velocity + * | v_p + * | /\ + * | /..\ v_0 + * |--------....----------- + * | .... + * | .... + * |_________________________ + * |----| t time + * + * To correct a position error x_err (shaded area above), we need to + * momentarily increase the velocity, then decrease back to the nonimal + * velocity. + * + * In effect, this is the trapezoidal velocity planning problem, if: + * 1) remaining distance dx = x_err + * 2) "final" velocity = v_0 + * 3) max velocity / acceleration from motion segment + */ + double a_max = tpGetScaledAccel(tp, tc) * emcmotStatus->spindle_tracking_gain; + double v_max = tc->maxvel; + + switch(emcmotStatus->pos_tracking_mode) { + case 2: + { + double v_sq_alt = pmSq(v_final) + pos_error * a_max; + double v_target_alt = pmSqrt(fmax(v_sq_alt, 0.0)); + tc->target_vel = v_target_alt; + break; } - tc->target_vel = target_vel + errorvel; + case 1: + { + double v_sq = a_max * pos_error; + double v_target_stock = signum(v_sq) * pmSqrt(fabs(v_sq)) + v_final; + tc->target_vel = v_target_stock; + break; + } + case 0: + default: + { + double v_target_trapz = fmin(findTrapezoidalDesiredVel(a_max, pos_error, v_final, tc->currentvel, tc->cycle_time), v_max); + tc->target_vel = v_target_trapz; + break; + } + } + + tc_debug_print("in position sync, target_vel = %f, ideal_vel = %f, vel_err = %f\n", tc->target_vel, v_final, v_final - tc->target_vel); } //Finally, clip requested velocity at zero @@ -3179,18 +3296,18 @@ int tpRunCycle(TP_STRUCT * const tp, long period) * spindle motion.*/ switch (tc->synchronized) { case TC_SYNC_NONE: - emcmotStatus->spindleSync = 0; + emcmotStatus->spindle_fb.synced = 0; break; case TC_SYNC_VELOCITY: - tp_debug_print("sync velocity\n"); + tc_debug_print("sync velocity\n"); tpSyncVelocityMode(tp, tc, nexttc); break; case TC_SYNC_POSITION: - tp_debug_print("sync position\n"); + tc_debug_print("sync position\n"); tpSyncPositionMode(tp, tc, nexttc); break; default: - tp_debug_print("unrecognized spindle sync state!\n"); + tc_debug_print("unrecognized spindle sync state!\n"); break; } diff --git a/src/emc/tp/tp_types.h b/src/emc/tp/tp_types.h index cb272e251e3..56607dc0730 100644 --- a/src/emc/tp/tp_types.h +++ b/src/emc/tp/tp_types.h @@ -77,7 +77,6 @@ typedef enum { */ typedef struct { double offset; - double revs; int waiting_for_index; int waiting_for_atspeed; } tp_spindle_t; diff --git a/src/emc/usr_intf/axis/scripts/axis.py b/src/emc/usr_intf/axis/scripts/axis.py index bca064350d5..22fe89aacfe 100755 --- a/src/emc/usr_intf/axis/scripts/axis.py +++ b/src/emc/usr_intf/axis/scripts/axis.py @@ -1684,6 +1684,19 @@ def run_warn(): if o.canon.max_extents_notool[i] > machine_limit_max[i]: warnings.append(_("Program exceeds machine maximum on axis %s") % "XYZABCUVW"[i]) + # Warn user if spindle-synched feeds violate axis limits + axis_max_vel = tuple([ + float(inifile.find("AXIS_%d" % i,"MAX_VELOCITY") or 0.0) * 60 + for i in range(9)]) + for line_no, delta, rpm, fpr in o.canon.feed_synched: + fpm = rpm * fpr # feed per minute + max_fpm = o.canon.calc_velocity(delta, axis_max_vel) * 0.95 + if fpm > max_fpm: + warnings.append(_( + "Spindle speed %(rpm_set).1f RPM exceeds maximum " + "%(rpm_max).1f RPM for spindle-synched motion " + "on line %(line_no)d" % + dict(rpm_set=rpm, rpm_max=max_fpm/fpr, line_no=line_no))) if warnings: text = "\n".join(warnings) return int(root_window.tk.call("nf_dialog", ".error", diff --git a/src/hal/components/quant.comp b/src/hal/components/quant.comp new file mode 100644 index 00000000000..0030f9cff53 --- /dev/null +++ b/src/hal/components/quant.comp @@ -0,0 +1,29 @@ +// This is a component for EMC2 HAL +// Copyright 2016 Robert W. Ellenberg +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of version 2 of the GNU General +// Public License as published by the Free Software Foundation. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +// +component quant "Quantize an input signal to a specified number of levels"; + +pin in float in "Analog input value" ; +pin in float resolution "Resolution of output signal (levels per unit)" ; +pin out float out "Quantized output value (analog)"; + +function _; +license "GPL"; +;; +#include "rtapi_math.h" +FUNCTION(_) { + out = floor(in * resolution) / resolution; +} diff --git a/tests/motion/spindlesync-exceeds-maxvel/.gitignore b/tests/motion/spindlesync-exceeds-maxvel/.gitignore new file mode 100644 index 00000000000..b568a3e9e69 --- /dev/null +++ b/tests/motion/spindlesync-exceeds-maxvel/.gitignore @@ -0,0 +1 @@ +/sim.var* diff --git a/tests/motion/spindlesync-exceeds-maxvel/README b/tests/motion/spindlesync-exceeds-maxvel/README new file mode 100644 index 00000000000..1920ef6394d --- /dev/null +++ b/tests/motion/spindlesync-exceeds-maxvel/README @@ -0,0 +1,7 @@ +In a spindle-synched G33 move, it is possible to specify a feed per +revolution and spindle speed that requires linear motion exceeding +axis velocity limits. This test sets up this kind of condition and +measures actual pitch during the motion, failing if pitch is not +within an epsilon value. + +[1]: https://github.com/LinuxCNC/linuxcnc/issues/167 \ No newline at end of file diff --git a/tests/motion/spindlesync-exceeds-maxvel/checkresult b/tests/motion/spindlesync-exceeds-maxvel/checkresult new file mode 100755 index 00000000000..bffcc8f9bda --- /dev/null +++ b/tests/motion/spindlesync-exceeds-maxvel/checkresult @@ -0,0 +1,4 @@ +#!/bin/sh +# If test-ui.py succeeds, then test succeeds +exit 0 + diff --git a/tests/motion/spindlesync-exceeds-maxvel/postgui.hal b/tests/motion/spindlesync-exceeds-maxvel/postgui.hal new file mode 100644 index 00000000000..d15f9fda470 --- /dev/null +++ b/tests/motion/spindlesync-exceeds-maxvel/postgui.hal @@ -0,0 +1,3 @@ +net Zvel => python-ui.Zvel +net spindle-rpm-est => python-ui.spindle-speed +net running halui.program.is-running => python-ui.running diff --git a/tests/motion/spindlesync-exceeds-maxvel/simpockets.tbl b/tests/motion/spindlesync-exceeds-maxvel/simpockets.tbl new file mode 100644 index 00000000000..4b427daaf99 --- /dev/null +++ b/tests/motion/spindlesync-exceeds-maxvel/simpockets.tbl @@ -0,0 +1,3 @@ +T1 P1 D0.125000 Z+1.000000 ; +T10 P3 D0.500000 Z+3.000000 ; +T99999 P50 Z+2.000000 ; diff --git a/tests/motion/spindlesync-exceeds-maxvel/test-ui.py b/tests/motion/spindlesync-exceeds-maxvel/test-ui.py new file mode 100755 index 00000000000..001f400aba2 --- /dev/null +++ b/tests/motion/spindlesync-exceeds-maxvel/test-ui.py @@ -0,0 +1,74 @@ +#!/usr/bin/env python + +import linuxcnc, hal +import sys, os, time + + +# Params +spindle_speed = 2000 # RPM +z_axis_maxvel = 4 * 60 # IPM +# Pitch at which Z axis must travel at double maxvel if spindle speed +# is constant +max_pitch = float(z_axis_maxvel) / float(spindle_speed) * 2 + + +# Set up component +h = hal.component("python-ui") +# - Monitor spindle and Z axis speed for debugging +h.newpin("spindle-speed", hal.HAL_FLOAT, hal.HAL_IN) +h.newpin("Zvel", hal.HAL_FLOAT, hal.HAL_IN) +# - Estimated pitch +h.newpin("pitch", hal.HAL_FLOAT, hal.HAL_IN) +# - Signal program start/stop +h.newpin("running", hal.HAL_BIT, hal.HAL_IN) +h.ready() # mark the component as 'ready' +os.system("halcmd source ./postgui.hal") + + +# Initialization +c = linuxcnc.command() +s = linuxcnc.stat() +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.mode(linuxcnc.MODE_MDI) + +# Spindle: set speed and start +c.mdi('S%d M3' % spindle_speed) + +# Spindle synchronized motion at 2x Z axis maxvel +c.mdi('G33 Z-5 K%.3f' % max_pitch) + +# While above MDI command runs, take an average of pitch while the Z +# axis is in motion +running = True +timeout = 5 +avg_sum = 0; avg_count = 0 +while timeout > 0: + if abs(h['pitch']) > 0.001: + rps = h['spindle-speed']/60 + ips = - h['Zvel'] + pitch = ips/rps + print "spindle velocity, revs/second: %.3f" % rps + print "Z velocity: %.3f" % ips + print "pitch: %.3f" % pitch + print + avg_sum += pitch + avg_count += 1 + timeout = 5 if h['running'] else timeout - 1 + time.sleep(0.1) +avg_pitch = avg_sum/avg_count +print "Average pitch = %.3f" % avg_pitch + +# Pitch should either be as specified (by slowing down spindle) or +# else program should have aborted; if it didn't abort and pitch is +# incorrect, then return the special failure result 166 +if (abs(avg_pitch - max_pitch) < 0.01): + res = 0 + print "OK: average pitch = %.3f" % avg_pitch +else: + res = 166 + print "Error: expected pitch = %.3f; got %.3f" % (max_pitch, avg_pitch) + +# Shutdown +c.wait_complete() +sys.exit(res) diff --git a/tests/motion/spindlesync-exceeds-maxvel/test.ini b/tests/motion/spindlesync-exceeds-maxvel/test.ini new file mode 100644 index 00000000000..43817adadc0 --- /dev/null +++ b/tests/motion/spindlesync-exceeds-maxvel/test.ini @@ -0,0 +1,87 @@ +[EMC] +DEBUG = 0x0 +#DEBUG = 0x7FFFFF +LOG_LEVEL = 5 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[RS274NGC] +PARAMETER_FILE = sim.var +SUBROUTINE_PATH = . +LOG_LEVEL = 5 + +[EMCMOT] +EMCMOT = motmod +COMM_TIMEOUT = 4.0 +COMM_WAIT = 0.010 +BASE_PERIOD = 40000 +SERVO_PERIOD = 1000000 + +[HAL] +HALUI = halui +HALFILE = ../../../lib/hallib/core_sim.hal +HALFILE = ../../../lib/hallib/sim_spindle_encoder.hal +POSTGUI_HALFILE = postgui.hal + +[TRAJ] +NO_FORCE_HOMING=1 +AXES = 3 +COORDINATES = X Y Z +HOME = 0 0 0 +LINEAR_UNITS = inch +ANGULAR_UNITS = degree +CYCLE_TIME = 0.010 +DEFAULT_VELOCITY = 1.2 +MAX_LINEAR_VELOCITY = 4 + +[AXIS_0] +TYPE = LINEAR +HOME = 0.000 +MAX_VELOCITY = 4 +MAX_ACCELERATION = 1000.0 +BACKLASH = 0.000 +INPUT_SCALE = 4000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 + +[AXIS_1] +TYPE = LINEAR +HOME = 0.000 +MAX_VELOCITY = 4 +MAX_ACCELERATION = 1000.0 +BACKLASH = 0.000 +INPUT_SCALE = 4000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 + +[AXIS_2] +TYPE = LINEAR +HOME = 0.0 +MAX_VELOCITY = 4 +MAX_ACCELERATION = 1000.0 +BACKLASH = 0.000 +INPUT_SCALE = 4000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 + +[EMCIO] +EMCIO = io +CYCLE_TIME = 0.100 +TOOL_TABLE = simpockets.tbl +TOOL_CHANGE_QUILL_UP = 1 +RANDOM_TOOLCHANGER = 0 + diff --git a/tests/motion/spindlesync-exceeds-maxvel/test.sh b/tests/motion/spindlesync-exceeds-maxvel/test.sh new file mode 100755 index 00000000000..925a64afab5 --- /dev/null +++ b/tests/motion/spindlesync-exceeds-maxvel/test.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +linuxcnc -r test.ini +res=$? +echo "Exit status $res" >&2 +if test $res = 166; then + echo "Test failed: unexpected pitch" >&2 + exit 1 +fi +exit 0 diff --git a/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini b/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini index 1185e66118a..877eb86e91c 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini +++ b/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini @@ -183,8 +183,8 @@ HOME_SEQUENCE = 0 TYPE = LINEAR HOME = 0.0 # Make Z accel and max vel really slow to highlight issues with helices -MAX_VELOCITY = .01 -MAX_ACCELERATION = .1 +MAX_VELOCITY = .5 +MAX_ACCELERATION = 10 BACKLASH = 0.0 INPUT_SCALE = 2000 OUTPUT_SCALE = 1.000 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_blend.ngc b/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_blend.ngc new file mode 100644 index 00000000000..40847f81382 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_blend.ngc @@ -0,0 +1,24 @@ +(From TP Issue #68) +G20 G8 G18 +G64 P0.01 Q0.0 +M3 S400 +G0 X2.0 Z0 +G0 X1.1 Z0.1 +(Lead-in move in "normal" feed mode) +G1 X1.0 Z0.0 F20 +(No blend here - switch to position sync mode) +G33 K0.1 X1.0 Z-0.5 +(Tangent blend during position sync) +G33 K0.1 X1.0 Z-0.75 +G33 K0.1 X1.0 Z-1.0 +(Arc blend here during position sync) +G33 K0.1 X0.800 Z-1.5 +(Arc blend here during position sync) +G33 K0.1 X0.800 Z-2.0 +(No blend here - switch to normal mode) +G1 X1.1 Z-2.1 F20 +(Clear work) +G0 X2.0 +G0 Z0 +M5 +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_simple.ngc b/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_simple.ngc index fb9336ab30f..a7aa197e668 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_simple.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_simple.ngc @@ -1,7 +1,7 @@ G90 G20 G64 (absolute distance mode) G0 X1 Z0.1 (rapid to position) -S100 M3 (start spindle turning) -G33 Z-2 K0.025 +S321 M3 (start spindle turning) +G33 Z-2 K0.05 G0 X1.25 (rapid move tool away from work) Z0.1 (rapid move to starting Z position) M9 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/spindle/g95_blend.ngc b/tests/trajectory-planner/circular-arcs/nc_files/spindle/g95_blend.ngc new file mode 100644 index 00000000000..b29e907260f --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/nc_files/spindle/g95_blend.ngc @@ -0,0 +1,30 @@ +(Test blend performance during G95 feed mode) +G20 G90 +G94 +#2=1000 +M3 S[#2] +G0 X0.0 Z0.0 +F0 +G95 +#1=0.01 +G1 X0.1 F[#1] +G1 X0.2 F[#1] +G1 X0.3 F[#1] +G1 X0.4 F[#1] +G1 X0.5 F[#1] +G1 X1.0 F[#1] +(Test transition between G94 / G95) +G94 +G1 Z1.0 F[60.0*#2*#1] +G95 +G1 X0.6 F[#1] +G1 X0.5 F[#1] +G1 X0.4 F[#1] +G1 X0.0 F[#1] +G1 Z0.5 F[#1] +G1 Z0.0 F[#1] +G94 +G0 X0.0 Z0.0 +M5 +M2 +