diff --git a/.gitignore b/.gitignore index b92acacf62f..9d8fbbb7fc9 100644 --- a/.gitignore +++ b/.gitignore @@ -40,3 +40,8 @@ oprofile* position.txt *.9 *.glade.h + +*.glade.h +# expanded INI files +*.ini.expanded +build diff --git a/README b/README index 673760e5258..abe1706d425 100644 --- a/README +++ b/README @@ -46,3 +46,14 @@ to run the software go back to the top level directory, and issue: . scripts/rip-environment linuxcnc + + +Unit Tests +--------- + +Building unit tests requires Meson (see https://mesonbuild.com/Getting-meson.html for details). + +To build and run the unit tests (from the repositor root directory): + + meson build && cd build + ninja test diff --git a/lib/hallib/core_sim9.hal b/lib/hallib/core_sim9.hal index 8d83381ce64..b4bcde1241f 100644 --- a/lib/hallib/core_sim9.hal +++ b/lib/hallib/core_sim9.hal @@ -6,9 +6,6 @@ loadrt trivkins # motion controller, get name and thread periods from ini file loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[TRAJ]AXES # load 6 differentiators (for velocity and accel signals -loadrt ddt names=ddt_x,ddt_xv,ddt_y,ddt_yv,ddt_z,ddt_zv -# load additional blocks -loadrt hypot names=vel_xy,vel_xyz # add motion controller functions to servo thread addf motion-command-handler servo-thread diff --git a/lib/hallib/sim_spindle_encoder.hal b/lib/hallib/sim_spindle_encoder.hal index 20ae6b95f0b..5000cbf7a1c 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 +setp spindle_mass.gain .005 -# 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 8b2e4a075f7..9fb54bcb26a 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,32 @@ 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 +258,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, lineno, 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((lineno, delta, s, fpr)) + def user_defined_function(self, i, p, q): if self.suppress > 0: return color = self.colors['m1xx'] diff --git a/meson.build b/meson.build index 7da4b87470d..1424bdaccfd 100644 --- a/meson.build +++ b/meson.build @@ -5,12 +5,15 @@ tp_src_dir = join_paths(src_root, 'emc/tp') inc_dir = include_directories([ 'unit_tests', + 'unit_tests/mock', src_root, + join_paths(src_root, 'emc/kinematics'), tp_src_dir, 'src/libnml/posemath', 'src/emc/nml_intf', 'src/emc/motion', 'src/rtapi', + 'src/hal', ]) cc = meson.get_compiler('c') @@ -18,23 +21,44 @@ m_dep = cc.find_library('m', required : true) add_global_arguments(['-DULAPI','-DUNIT_TEST'], language : 'c') -pm = static_library('posemath', +# Define static libraries for easier linking with unit tests + +posemath_obj = static_library('posemath', 'src/libnml/posemath/_posemath.c', - include_directories : inc_dir) + include_directories : inc_dir, + dependencies : [m_dep] + ) -# TODO implement subdir builds for tp, motion, etc +emcpose_obj = static_library('emcpose', + join_paths(src_root, 'emc/nml_intf/emcpose.c'), + include_directories : inc_dir + ) -# KLUDGE just shove all the source files into the test build -test_blendmath=executable('test_blendmath', - ['unit_tests/tp/test_blendmath.c', +utlib_tp_deps = static_library('tp_deps', + [ + join_paths(tp_src_dir, 'joint_util.c'), join_paths(tp_src_dir, 'blendmath.c'), join_paths(tp_src_dir, 'spherical_arc.c'), - join_paths(src_root, 'emc/nml_intf/emcpose.c'), join_paths(tp_src_dir, 'tc.c'), - ], + join_paths(tp_src_dir, 'tcq.c'), + ], + include_directories : inc_dir, + link_with: [emcpose_obj, posemath_obj]) + +tp_test_files = [ + 'test_blendmath', + 'test_joint_util', + 'test_spherical_arc', + 'test_posemath' + ] + +foreach n : tp_test_files + +test(n, executable(n, + join_paths('unit_tests/tp', n+'.c'), dependencies : [m_dep], include_directories : inc_dir, - link_with : pm - ) + link_with : [ utlib_tp_deps] + )) +endforeach -test('blendmath test',test_blendmath) diff --git a/src/Makefile b/src/Makefile index a99e5268680..fc4bb299931 100644 --- a/src/Makefile +++ b/src/Makefile @@ -942,6 +942,8 @@ motmod-objs += emc/tp/tcq.o motmod-objs += emc/tp/tp.o motmod-objs += emc/tp/spherical_arc.o motmod-objs += emc/tp/blendmath.o +motmod-objs += emc/tp/math_util.o +motmod-objs += emc/tp/joint_util.o motmod-objs += emc/motion/motion.o motmod-objs += emc/motion/command.o motmod-objs += emc/motion/control.o diff --git a/src/emc/ini/initraj.cc b/src/emc/ini/initraj.cc index fd293d12ddd..258833d5d74 100644 --- a/src/emc/ini/initraj.cc +++ b/src/emc/ini/initraj.cc @@ -193,6 +193,14 @@ static int loadTraj(EmcIniFile *trajInifile) old_inihal_data.traj_arc_blend_tangent_kink_ratio = arcBlendTangentKinkRatio; //TODO update inihal + int extraConsistencyChecks = 0; + double maxPositionDriftError = 0.0001; + + trajInifile->Find(&extraConsistencyChecks, "EXTRA_CONSISTENCY_CHECKS", "TRAJ"); + trajInifile->Find(&maxPositionDriftError, "MAX_POSITION_DRIFT_ERROR", "TRAJ"); + + emcSetupConsistencyChecks(extraConsistencyChecks, maxPositionDriftError); + double maxFeedScale = 1.0; trajInifile->Find(&maxFeedScale, "MAX_FEED_OVERRIDE", "DISPLAY"); diff --git a/src/emc/motion-logger/motion-logger.c b/src/emc/motion-logger/motion-logger.c index 81a3e8db7fb..e68c4621b49 100644 --- a/src/emc/motion-logger/motion-logger.c +++ b/src/emc/motion-logger/motion-logger.c @@ -571,13 +571,14 @@ int main(int argc, char* argv[]) { break; 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; + log_print("SPINDLE_ON speed=%f, css_factor=%f, xoffset=%f\n", + c->spindle_speed, c->css_factor, c->css_xoffset); + emcmotStatus->spindle_cmd.velocity_rpm_out = c->spindle_speed; 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: @@ -625,6 +626,12 @@ int main(int argc, char* argv[]) { log_print("SETUP_ARC_BLENDS\n"); break; + case EMCMOT_SETUP_CONSISTENCY_CHECKS: + log_print("SETUP_CONSISTENCY_CHECKS enabled=%d, position_drift=%f\n", + c->consistencyCheckConfig.extraConsistencyChecks, + c->consistencyCheckConfig.maxPositionDriftError); + break; + default: log_print("ERROR: unknown command %d\n", c->command); break; diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 305469c5bf0..e48b9d7bc9f 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 */ @@ -899,7 +899,8 @@ check_stuff ( "before command_handler()" ); int res_addline = tpAddLine(&emcmotDebug->tp, emcmotCommand->pos, emcmotCommand->motion_type, emcmotCommand->vel, emcmotCommand->ini_maxvel, emcmotCommand->acc, emcmotStatus->enables_new, issue_atspeed, - emcmotCommand->turn); + emcmotCommand->turn, + emcmotCommand->pure_angular); //KLUDGE ignore zero length line if (res_addline < 0) { reportError(_("can't add linear move at line %d, error code %d"), @@ -953,10 +954,11 @@ check_stuff ( "before command_handler()" ); /* append it to the emcmotDebug->tp */ tpSetId(&emcmotDebug->tp, emcmotCommand->id); int res_addcircle = tpAddCircle(&emcmotDebug->tp, emcmotCommand->pos, - emcmotCommand->center, emcmotCommand->normal, - emcmotCommand->turn, emcmotCommand->motion_type, - emcmotCommand->vel, emcmotCommand->ini_maxvel, - emcmotCommand->acc, emcmotStatus->enables_new, issue_atspeed); + emcmotCommand->center, emcmotCommand->normal, + emcmotCommand->turn, emcmotCommand->motion_type, + emcmotCommand->vel, emcmotCommand->ini_maxvel, + emcmotCommand->acc, emcmotCommand->acc_normal, + emcmotStatus->enables_new, issue_atspeed); if (res_addcircle < 0) { reportError(_("can't add circular move at line %d, error code %d"), emcmotCommand->id, res_addcircle); @@ -1026,9 +1028,9 @@ check_stuff ( "before command_handler()" ); /* set the max acceleration */ /* can do it at any time */ rtapi_print_msg(RTAPI_MSG_DBG, "SET_ACCEL"); - emcmotStatus->acc = emcmotCommand->acc; - tpSetAmax(&emcmotDebug->tp, emcmotStatus->acc); - break; + emcmotStatus->acc = emcmotCommand->acc; + // WARNING has no effect on coordinated motion TP, only single-axis planner + break; case EMCMOT_PAUSE: /* pause the motion */ @@ -1394,7 +1396,11 @@ check_stuff ( "before command_handler()" ); /* append it to the emcmotDebug->tp */ tpSetId(&emcmotDebug->tp, emcmotCommand->id); - if (-1 == tpAddLine(&emcmotDebug->tp, emcmotCommand->pos, emcmotCommand->motion_type, emcmotCommand->vel, emcmotCommand->ini_maxvel, emcmotCommand->acc, emcmotStatus->enables_new, 0, -1)) { + if (-1 == tpAddLine(&emcmotDebug->tp, emcmotCommand->pos, + emcmotCommand->motion_type, emcmotCommand->vel, + emcmotCommand->ini_maxvel, emcmotCommand->acc, + emcmotStatus->enables_new, 0, -1, + emcmotCommand->pure_angular)) { reportError(_("can't add probe move")); emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC; tpAbort(&emcmotDebug->tp); @@ -1522,7 +1528,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 +1536,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->spindle_speed; + emcmotStatus->spindle_cmd.css_factor = emcmotCommand->css_factor; + emcmotStatus->spindle_cmd.xoffset = emcmotCommand->css_xoffset; + 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 +1567,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 +1578,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: @@ -1648,26 +1646,32 @@ check_stuff ( "before command_handler()" ); joint->comp.entries++; break; - case EMCMOT_SET_OFFSET: - emcmotStatus->tool_offset = emcmotCommand->tool_offset; - break; + case EMCMOT_SET_OFFSET: + emcmotStatus->tool_offset = emcmotCommand->tool_offset; + break; - default: - rtapi_print_msg(RTAPI_MSG_DBG, "UNKNOWN"); - reportError(_("unrecognized command %d"), emcmotCommand->command); - emcmotStatus->commandStatus = EMCMOT_COMMAND_UNKNOWN_COMMAND; - break; - case EMCMOT_SET_MAX_FEED_OVERRIDE: - emcmotConfig->maxFeedScale = emcmotCommand->maxFeedScale; - break; - case EMCMOT_SETUP_ARC_BLENDS: - emcmotConfig->arcBlendEnable = emcmotCommand->arcBlendEnable; - emcmotConfig->arcBlendFallbackEnable = emcmotCommand->arcBlendFallbackEnable; - emcmotConfig->arcBlendOptDepth = emcmotCommand->arcBlendOptDepth; - emcmotConfig->arcBlendGapCycles = emcmotCommand->arcBlendGapCycles; - emcmotConfig->arcBlendRampFreq = emcmotCommand->arcBlendRampFreq; - emcmotConfig->arcBlendTangentKinkRatio = emcmotCommand->arcBlendTangentKinkRatio; - break; + case EMCMOT_SET_MAX_FEED_OVERRIDE: + emcmotConfig->maxFeedScale = emcmotCommand->maxFeedScale; + break; + + case EMCMOT_SETUP_ARC_BLENDS: + emcmotConfig->arcBlendEnable = emcmotCommand->arcBlendEnable; + emcmotConfig->arcBlendFallbackEnable = emcmotCommand->arcBlendFallbackEnable; + emcmotConfig->arcBlendOptDepth = emcmotCommand->arcBlendOptDepth; + emcmotConfig->arcBlendGapCycles = emcmotCommand->arcBlendGapCycles; + emcmotConfig->arcBlendRampFreq = emcmotCommand->arcBlendRampFreq; + emcmotConfig->arcBlendTangentKinkRatio = emcmotCommand->arcBlendTangentKinkRatio; + break; + + case EMCMOT_SETUP_CONSISTENCY_CHECKS: + emcmotConfig->consistencyCheckConfig = emcmotCommand->consistencyCheckConfig; + break; + + default: + rtapi_print_msg(RTAPI_MSG_DBG, "UNKNOWN"); + reportError(_("unrecognized command %d"), emcmotCommand->command); + emcmotStatus->commandStatus = EMCMOT_COMMAND_UNKNOWN_COMMAND; + break; } /* end of: command switch */ if (emcmotStatus->commandStatus != EMCMOT_COMMAND_OK) { diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index b0ef750d301..8d491dbd740 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,17 +1748,19 @@ 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->pos_tracking_error) = emcmotStatus->pos_tracking_error; + *(emcmot_hal_data->program_line) = emcmotStatus->id; *(emcmot_hal_data->motion_type) = emcmotStatus->motionType; *(emcmot_hal_data->distance_to_go) = emcmotStatus->distance_to_go; @@ -1774,22 +1792,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 9000f75d676..f62857533b4 100644 --- a/src/emc/motion/mot_priv.h +++ b/src/emc/motion/mot_priv.h @@ -90,6 +90,9 @@ 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_float_t *pos_tracking_error; + 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 +150,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 4583308064e..586f5d946b8 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -321,11 +321,19 @@ 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_OUT, &(emcmot_hal_data->pos_tracking_error), mot_comp_id, "motion.pos-tracking-error")) < 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; @@ -912,7 +920,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; @@ -1034,21 +1042,20 @@ static int init_comm_buffers(void) /* init motion emcmotDebug->tp */ if (-1 == tpCreate(&emcmotDebug->tp, DEFAULT_TC_QUEUE_SIZE, - emcmotDebug->queueTcSpace)) { - rtapi_print_msg(RTAPI_MSG_ERR, - "MOTION: failed to create motion emcmotDebug->tp\n"); - return -1; + emcmotDebug->queueTcSpace)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "MOTION: failed to create motion emcmotDebug->tp\n"); + return -1; } -// tpInit(&emcmotDebug->tp); // tpInit called from tpCreate + tpSetCycleTime(&emcmotDebug->tp, emcmotConfig->trajCycleTime); - tpSetPos(&emcmotDebug->tp, &emcmotStatus->carte_pos_cmd); - tpSetVmax(&emcmotDebug->tp, emcmotStatus->vel, emcmotStatus->vel); - tpSetAmax(&emcmotDebug->tp, emcmotStatus->acc); + int res_pos = tpSetPos(&emcmotDebug->tp, &emcmotStatus->carte_pos_cmd); + int res_vel = tpSetVmax(&emcmotDebug->tp, emcmotStatus->vel, emcmotStatus->vel); emcmotStatus->tail = 0; rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_comm_buffers() complete\n"); - return 0; + return res_pos | res_vel; } /* init_threads() creates realtime threads, exports functions to diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 50ef08ac2a0..1efaab035f6 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -177,6 +177,7 @@ extern "C" { EMCMOT_SET_OFFSET, /* set tool offsets */ EMCMOT_SET_MAX_FEED_OVERRIDE, EMCMOT_SETUP_ARC_BLENDS, + EMCMOT_SETUP_CONSISTENCY_CHECKS, } cmd_code_t; /* this enum lists the possible results of a command */ @@ -194,6 +195,11 @@ extern "C" { #define EMCMOT_TERM_COND_BLEND 2 #define EMCMOT_TERM_COND_TANGENT 3 +typedef struct { + // Consistency checking within TP + int extraConsistencyChecks; + double maxPositionDriftError; +} consistency_check_config_t; /********************************* COMMAND STRUCTURE *********************************/ @@ -215,10 +221,15 @@ extern "C" { double vel; /* max velocity */ double ini_maxvel; /* max velocity allowed by machine constraints (the ini file) */ + int pure_angular; // Canon decided this move should be treated as an angular move (may have small XYZUVW motion anyway) int motion_type; /* this move is because of traverse, feed, arc, or toolchange */ double spindlesync; /* user units per spindle revolution, 0 = no sync */ + double spindle_speed; /* commanded spindle speed */ + double css_factor; // Only used during CSS mode + double css_xoffset; // Only used during CSS mode double acc; /* max acceleration */ - double backlash; /* amount of backlash */ + double acc_normal; /* max normal acceleration (circular moves only)*/ + double backlash; /* amount of backlash */ int id; /* id for motion */ int termCond; /* termination condition */ double tolerance; /* tolerance for path deviation in CONTINUOUS mode */ @@ -256,6 +267,9 @@ extern "C" { double arcBlendRampFreq; double arcBlendTangentKinkRatio; double maxFeedScale; + + consistency_check_config_t consistencyCheckConfig; + } emcmot_command_t; /*! \todo FIXME - these packed bits might be replaced with chars @@ -544,17 +558,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 +598,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 +670,12 @@ 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; + double pos_tracking_error; 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 */ @@ -750,6 +773,7 @@ Suggestion: Split this in to an Error and a Status flag register.. double arcBlendRampFreq; double arcBlendTangentKinkRatio; double maxFeedScale; + consistency_check_config_t consistencyCheckConfig; } emcmot_config_t; /********************************* diff --git a/src/emc/motion/usrmotintf.cc b/src/emc/motion/usrmotintf.cc index ec4ad552cbc..2362b49b001 100644 --- a/src/emc/motion/usrmotintf.cc +++ b/src/emc/motion/usrmotintf.cc @@ -249,7 +249,6 @@ void printTPstruct(TP_STRUCT * tp) printf("queueSize=%d\n", tp->queueSize); printf("cycleTime=%f\n", tp->cycleTime); printf("vMax=%f\n", tp->vMax); - printf("aMax=%f\n", tp->aMax); printf("vLimit=%f\n", tp->vLimit); printf("wMax=%f\n", tp->wMax); printf("wDotMax=%f\n", tp->wDotMax); diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index d6b9f22dca5..d0c841507ff 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -1515,6 +1515,7 @@ void EMC_TRAJ_LINEAR_MOVE::update(CMS * cms) cms->update(acc); cms->update(feed_mode); cms->update(indexrotary); + cms->update(pure_angular); } /* @@ -1536,6 +1537,7 @@ void EMC_TRAJ_CIRCULAR_MOVE::update(CMS * cms) cms->update(vel); cms->update(ini_maxvel); cms->update(acc); + cms->update(acc_normal); cms->update(feed_mode); } @@ -2818,6 +2820,7 @@ void EMC_TRAJ_PROBE::update(CMS * cms) cms->update(vel); cms->update(ini_maxvel); cms->update(acc); + cms->update(pure_angular); cms->update(probe_type); } diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index ab18a04e9cd..bdbd9b43b02 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -441,9 +441,9 @@ extern int emcTrajStep(); extern int emcTrajResume(); extern int emcTrajDelay(double delay); extern int emcTrajLinearMove(EmcPose end, int type, double vel, - double ini_maxvel, double acc, int indexrotary); + double ini_maxvel, double acc, int indexrotary, int pure_angular); extern int emcTrajCircularMove(EmcPose end, PM_CARTESIAN center, PM_CARTESIAN - normal, int turn, int type, double vel, double ini_maxvel, double acc); + normal, int turn, int type, double vel, double ini_maxvel, double acc, double acc_normal); extern int emcTrajSetTermCond(int cond, double tolerance); extern int emcTrajSetSpindleSync(double feed_per_revolution, bool wait_for_index); extern int emcTrajSetOffset(EmcPose tool_offset); @@ -452,7 +452,7 @@ extern int emcTrajSetRotation(double rotation); extern int emcTrajSetHome(EmcPose home); extern int emcTrajClearProbeTrippedFlag(); extern int emcTrajProbe(EmcPose pos, int type, double vel, - double ini_maxvel, double acc, unsigned char probe_type); + double ini_maxvel, double acc, int pure_angular, unsigned char probe_type); extern int emcAuxInputWait(int index, int input_type, int wait_type, int timeout); extern int emcTrajRigidTap(EmcPose pos, double vel, double ini_maxvel, double acc); @@ -556,6 +556,8 @@ int emcSetupArcBlends(int arcBlendEnable, double arcBlendRampFreq, double arcBlendTangentKinkRatio); +int emcSetupConsistencyChecks(int consistency_checks, double max_position_drift_error); + extern int emcUpdate(EMC_STAT * stat); // full EMC status extern EMC_STAT *emcStatus; diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index 095568b2853..72c88eca816 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -489,7 +489,7 @@ class EMC_AXIS_STAT:public EMC_AXIS_STAT_MSG { class EMC_TRAJ_CMD_MSG:public RCS_CMD_MSG { public: EMC_TRAJ_CMD_MSG(NMLTYPE t, size_t s):RCS_CMD_MSG(t, s) { - }; + } // For internal NML/CMS use only. void update(CMS * cms); @@ -788,6 +788,7 @@ class EMC_TRAJ_LINEAR_MOVE:public EMC_TRAJ_CMD_MSG { double vel, ini_maxvel, acc; int feed_mode; int indexrotary; + int pure_angular; }; class EMC_TRAJ_CIRCULAR_MOVE:public EMC_TRAJ_CMD_MSG { @@ -805,7 +806,7 @@ class EMC_TRAJ_CIRCULAR_MOVE:public EMC_TRAJ_CMD_MSG { PM_CARTESIAN normal; int turn; int type; - double vel, ini_maxvel, acc; + double vel, ini_maxvel, acc, acc_normal; int feed_mode; }; @@ -944,6 +945,7 @@ class EMC_TRAJ_PROBE:public EMC_TRAJ_CMD_MSG { EmcPose pos; int type; double vel, ini_maxvel, acc; + int pure_angular; unsigned char probe_type; }; diff --git a/src/emc/nml_intf/emcpose.c b/src/emc/nml_intf/emcpose.c index 7e389729640..5daa750806b 100644 --- a/src/emc/nml_intf/emcpose.c +++ b/src/emc/nml_intf/emcpose.c @@ -78,6 +78,25 @@ int emcPoseSub(EmcPose const * const p1, EmcPose const * const p2, EmcPose * con } +int emcPoseMultScalar(EmcPose * const p1, double m) +{ +#ifdef EMCPOSE_PEDANTIC + if (!p1 || !p2) { + return EMCPOSE_ERR_INPUT_MISSING; + } +#endif + + pmCartScalMultEq(&p1->tran, m); + p1->a *= m; + p1->b *= m; + p1->c *= m; + p1->u *= m; + p1->v *= m; + p1->w *= m; + return EMCPOSE_ERR_OK; +} + + int emcPoseSelfAdd(EmcPose * const self, EmcPose const * const p2) { return emcPoseAdd(self, p2, self); @@ -306,3 +325,26 @@ int emcPoseValid(EmcPose const * const pose) return 1; } } + +/** + * Checks all axis values in an EmcPose to see if they exceed a magnitude threshold. + * @return a bitmask that is 0 if all axes are within the threshold. Any + * out-of-limit axes set their corresponding bit to 1 in the returned value (X + * is 0th bit, Y is 1st, etc.). + */ +unsigned int findAbsThresholdViolations(EmcPose vec, double threshold) +{ + threshold = fabs(threshold); + // Bit-mask each failure so we can report all failed axes + unsigned fail_bits = (unsigned)(0x0 + | (fabs(vec.tran.x) > threshold) << 0 + | (fabs(vec.tran.y) > threshold) << 1 + | (fabs(vec.tran.z) > threshold) << 2 + | (fabs(vec.a) > threshold) << 3 + | (fabs(vec.b) > threshold) << 4 + | (fabs(vec.c) > threshold) << 5 + | (fabs(vec.u) > threshold) << 6 + | (fabs(vec.v) > threshold) << 7 + | (fabs(vec.w) > threshold) << 8); + return fail_bits; +} diff --git a/src/emc/nml_intf/emcpose.h b/src/emc/nml_intf/emcpose.h index dc6b7e80e38..5aa378de170 100644 --- a/src/emc/nml_intf/emcpose.h +++ b/src/emc/nml_intf/emcpose.h @@ -29,6 +29,8 @@ void emcPoseZero(EmcPose * const pos); int emcPoseAdd(EmcPose const * const p1, EmcPose const * const p2, EmcPose * const out); int emcPoseSub(EmcPose const * const p1, EmcPose const * const p2, EmcPose * const out); +int emcPoseMultScalar(EmcPose * const p1, double m); + int emcPoseToPmCartesian(EmcPose const * const pose, PmCartesian * const xyz, PmCartesian * const abc, PmCartesian * const uvw); int pmCartesianToEmcPose(PmCartesian const * const xyz, @@ -47,6 +49,8 @@ int emcPoseGetUVW(EmcPose const * const pose, PmCartesian * const uvw); int emcPoseMagnitude(EmcPose const * const pose, double * const out); +unsigned int findAbsThresholdViolations(EmcPose vec, double threshold); + int emcPoseValid(EmcPose const * const pose); #endif 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/nml_intf/motion_types.h b/src/emc/nml_intf/motion_types.h index 8d9a430a09e..f0818dc6055 100644 --- a/src/emc/nml_intf/motion_types.h +++ b/src/emc/nml_intf/motion_types.h @@ -13,9 +13,16 @@ // 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 -#define EMC_MOTION_TYPE_TRAVERSE 1 -#define EMC_MOTION_TYPE_FEED 2 -#define EMC_MOTION_TYPE_ARC 3 -#define EMC_MOTION_TYPE_TOOLCHANGE 4 -#define EMC_MOTION_TYPE_PROBING 5 -#define EMC_MOTION_TYPE_INDEXROTARY 6 +#ifndef MOTION_TYPES_H +#define MOTION_TYPES_H + +typedef enum { + EMC_MOTION_TYPE_TRAVERSE = 1, + EMC_MOTION_TYPE_FEED = 2, + EMC_MOTION_TYPE_ARC = 3, + EMC_MOTION_TYPE_TOOLCHANGE = 4, + EMC_MOTION_TYPE_PROBING = 5, + EMC_MOTION_TYPE_INDEXROTARY = 6, +} EMCMotionTypes; + +#endif diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index d67f7a925fc..a411a58d71f 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,11 +230,15 @@ 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 = + PyObject *result = spindle_synched_motion ? + callmethod(callback, "straight_feed_synched", "ifffffffffff", + line_number, 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); + if(result == NULL) interp_error ++; + Py_XDECREF(result); } void STRAIGHT_TRAVERSE(int line_number, @@ -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() {} @@ -708,6 +726,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/rs274ngc/interp_base.hh b/src/emc/rs274ngc/interp_base.hh index e8d3e8ce08a..158e7800fb2 100644 --- a/src/emc/rs274ngc/interp_base.hh +++ b/src/emc/rs274ngc/interp_base.hh @@ -15,6 +15,8 @@ * along with this program; if not, write to the Free Software * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. */ +#ifndef INTERP_BASE_HH +#define INTERP_BASE_HH #include /* Size of certain arrays */ @@ -56,3 +58,5 @@ public: InterpBase *interp_from_shlib(const char *shlib); extern "C" InterpBase *makeInterp(); + +#endif diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 356fa1690a1..d91f3f5d1bb 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -26,6 +26,7 @@ #include "emcpos.h" #include "libintl.h" #include "python_plugin.hh" +#include "interp_base.hh" #define _(s) gettext(s) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 945f3717845..c5c90d62cca 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -52,8 +52,12 @@ #define canon_debug(...) #endif +static bool limitSpindleSpeed(double rpm); +static bool limitSpindleSpeedFromVel(double nominal_vel, double max_vel); +static double limitSpindleSpeedByActiveFeedRate(double ini_maxvel); + 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 double xy_rotation = 0.; @@ -96,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) @@ -136,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 @@ -158,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; @@ -438,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; @@ -453,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; @@ -463,6 +483,36 @@ 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; + // KLUDGE effective spindle speed depends on current state + // If CSS mode is active, then we need to assume the worst-case (maximum) spindle speed specified by the D word + // Otherwise, the nominal spindle speed is used + + double planned_spindle_rpm = css_maximum > 0.0 ? css_maximum : spindleSpeed_rpm; + + double uu_per_sec = uuPerRev * planned_spindle_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); @@ -489,7 +539,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); } @@ -504,7 +554,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); } @@ -560,29 +610,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; } } @@ -649,9 +700,11 @@ static void applyMinDisplacement(double &dx, * Get the limiting acceleration for a displacement from the current position to the given position. * returns a single acceleration that is the minimum of all axis accelerations. */ -static AccelData getStraightAcceleration(double x, double y, double z, - double a, double b, double c, - double u, double v, double w) +static AccelData getStraightAcceleration( + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, + int known_xyz_motion) { double dx, dy, dz, du, dv, dw, da, db, dc; double tx, ty, tz, tu, tv, tw, ta, tb, tc; @@ -675,12 +728,15 @@ static AccelData getStraightAcceleration(double x, double y, double z, applyMinDisplacement(dx, dy, dz, da, db, dc, du, dv, dw); if(debug_velacc) - printf("getStraightAcceleration dx %g dy %g dz %g da %g db %g dc %g du %g dv %g dw %g ", + printf("getStraightAcceleration dx %g dy %g dz %g da %g db %g" + " dc %g du %g dv %g dw %g\n", dx, dy, dz, da, db, dc, du, dv, dw); // Figure out what kind of move we're making. This is used to determine // the units of vel/acc. - if (dx <= 0.0 && dy <= 0.0 && dz <= 0.0 && + if (known_xyz_motion) + cartesian_move = 1; + else if (dx <= 0.0 && dy <= 0.0 && dz <= 0.0 && du <= 0.0 && dv <= 0.0 && dw <= 0.0) { cartesian_move = 0; } else { @@ -762,7 +818,17 @@ static AccelData getStraightAcceleration(double x, double y, double z, return out; } -static AccelData getStraightAcceleration(CANON_POSITION pos) +static AccelData getStraightAcceleration( + double x, double y, double z, + double a, double b, double c, + double u, double v, double w) +{ + return getStraightAcceleration( + x, y, z, a, b, c, u, v, w, 0); +} + +static AccelData getStraightAcceleration(CANON_POSITION pos, + int known_xyz_motion) { return getStraightAcceleration(pos.x, @@ -773,12 +839,13 @@ static AccelData getStraightAcceleration(CANON_POSITION pos) pos.c, pos.u, pos.v, - pos.w); + pos.w, + known_xyz_motion); } 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; @@ -787,7 +854,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; @@ -838,7 +905,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; } @@ -852,7 +919,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; } @@ -888,7 +955,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; } @@ -938,22 +1005,10 @@ static void flush_segments(void) { #endif VelData linedata = getStraightVelocity(x, y, z, a, b, c, u, v, w); - double vel = 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 ini_maxvel = linedata.vel; + double vel = limitSpindleSpeedByActiveFeedRate(ini_maxvel); + canon_debug("Line segment maximum velocity %f, requested vel %f\n", ini_maxvel, vel); EMC_TRAJ_LINEAR_MOVE linearMoveMsg; linearMoveMsg.feed_mode = feed_mode; @@ -972,8 +1027,9 @@ static void flush_segments(void) { linearMoveMsg.end.b = TO_EXT_ANG(b); linearMoveMsg.end.c = TO_EXT_ANG(c); + linearMoveMsg.pure_angular = angular_move && !cartesian_move; 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); @@ -1097,13 +1153,14 @@ void STRAIGHT_TRAVERSE(int line_number, acc = accdata.acc; linearMoveMsg.end = to_ext_pose(x,y,z,a,b,c,u,v,w); + linearMoveMsg.pure_angular = angular_move && !cartesian_move; linearMoveMsg.vel = linearMoveMsg.ini_maxvel = toExtVel(vel); linearMoveMsg.acc = toExtAcc(acc); linearMoveMsg.indexrotary = rotary_unlock_for_traverse; 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); @@ -1111,7 +1168,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); } @@ -1121,9 +1178,6 @@ void STRAIGHT_FEED(int line_number, double a, double b, double c, double u, double v, double w) { - EMC_TRAJ_LINEAR_MOVE linearMoveMsg; - linearMoveMsg.feed_mode = feed_mode; - from_prog(x,y,z,a,b,c,u,v,w); rotate_and_offset_pos(x,y,z,a,b,c,u,v,w); see_segment(line_number, x, y, z, a, b, c, u, v, w); @@ -1144,7 +1198,9 @@ void RIGID_TAP(int line_number, double x, double y, double z) canonEndPoint.u, canonEndPoint.v, canonEndPoint.w); ini_maxvel = veldata.vel; - + + limitSpindleSpeedByActiveFeedRate(ini_maxvel); + AccelData accdata = getStraightAcceleration(x, y, z, canonEndPoint.a, canonEndPoint.b, canonEndPoint.c, canonEndPoint.u, canonEndPoint.v, canonEndPoint.w); @@ -1180,7 +1236,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); @@ -1189,28 +1244,17 @@ 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 vel = limitSpindleSpeedByActiveFeedRate(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); probeMsg.acc = toExtAcc(acc); + probeMsg.pure_angular = angular_move & ! cartesian_move; probeMsg.type = EMC_MOTION_TYPE_PROBING; probeMsg.probe_type = probe_type; @@ -1279,11 +1323,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; @@ -1524,7 +1573,6 @@ void ARC_FEED(int line_number, { EMC_TRAJ_CIRCULAR_MOVE circularMoveMsg; - EMC_TRAJ_LINEAR_MOVE linearMoveMsg; canon_debug("line = %d\n", line_number); canon_debug("first_end = %f, second_end = %f\n", first_end,second_end); @@ -1562,6 +1610,8 @@ void ARC_FEED(int line_number, } } + + EMC_TRAJ_LINEAR_MOVE linearMoveMsg; linearMoveMsg.feed_mode = feed_mode; circularMoveMsg.feed_mode = feed_mode; flush_segments(); @@ -1767,11 +1817,11 @@ void ARC_FEED(int line_number, } //FIXME allow tangential acceleration like in TP - double a_max_normal = a_max_axes * sqrt(3.0)/2.0; + double a_normal_budget = a_max_axes * sqrt(3.0)/2.0; canon_debug("a_max_axes = %f\n", a_max_axes); // Compute the centripetal acceleration - double v_max_radial = sqrt(a_max_normal * effective_radius); + double v_max_radial = sqrt(a_normal_budget * effective_radius); canon_debug("v_max_radial = %f\n", v_max_radial); // Restrict our maximum velocity in-plane if need be @@ -1811,7 +1861,8 @@ void ARC_FEED(int line_number, // Use "straight" acceleration measure to compute acceleration bounds due // to non-circular components (helical axis, other axes) - AccelData accdata = getStraightAcceleration(endpt); + AccelData accdata = getStraightAcceleration( + endpt, fabs(total_xyz_length) > 0.000001); double tt_max_motion = accdata.tmax; double tt_max_spiral = spiral_length / a_max_axes; @@ -1822,8 +1873,14 @@ 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); +#ifdef EMCCANON_DEBUG + double nominal_vel = getActiveFeedRate(FEED_LINEAR); +#endif + + // Make sure spindle speed is within range (for spindle_sync motion only) + double vel = limitSpindleSpeedByActiveFeedRate(v_max); + + canon_debug("current F = %f\n", nominal_vel); canon_debug("vel = %f\n",vel); canon_debug("v_max = %f\n",v_max); @@ -1837,6 +1894,8 @@ void ARC_FEED(int line_number, // or we wouldn't be calling ARC_FEED linearMoveMsg.end = to_ext_pose(endpt); linearMoveMsg.type = EMC_MOTION_TYPE_ARC; + + linearMoveMsg.pure_angular = angular_move && !cartesian_move; linearMoveMsg.vel = toExtVel(vel); linearMoveMsg.ini_maxvel = toExtVel(v_max); linearMoveMsg.acc = toExtAcc(a_max); @@ -1850,7 +1909,7 @@ void ARC_FEED(int line_number, // Convert internal center and normal to external units circularMoveMsg.center = to_ext_len(center_cart); - circularMoveMsg.normal = to_ext_len(normal_cart); + circularMoveMsg.normal = normal_cart; if (rotation > 0) circularMoveMsg.turn = rotation - 1; @@ -1863,6 +1922,7 @@ void ARC_FEED(int line_number, circularMoveMsg.vel = toExtVel(vel); circularMoveMsg.ini_maxvel = toExtVel(v_max); circularMoveMsg.acc = toExtAcc(a_max); + circularMoveMsg.acc_normal = toExtAcc(a_max_axes); //FIXME what happens if accel or vel is zero? // The end point is still updated, but nothing is added to the interp list @@ -1898,6 +1958,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; @@ -1905,18 +1989,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); } @@ -1926,47 +1999,75 @@ 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 + canon_debug("Spindle speed %f is within max of %f\n", spindleSpeed_rpm, limit_rpm); + 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) { + canon_debug("No spindle speed limit required\n"); + return false; + } else if (feed_mode) { + canon_debug("Skipping spindle speed limit due to velocity sync\n"); + 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); +} + +static double limitSpindleSpeedByActiveFeedRate(double ini_maxvel) +{ + const double nominal_vel = getActiveFeedRate(static_cast(!cartesian_move && angular_move)); + // Limit spindle speed during rigid tapping + limitSpindleSpeedFromVel(nominal_vel, ini_maxvel); + + double real_max_vel = std::min(ini_maxvel, nominal_vel); + canon_debug("%s: nominal_vel %f, real_max_vel %f\n", __PRETTY_FUNCTION__, nominal_vel, real_max_vel); + return real_max_vel; +} + + + void STOP_SPINDLE_TURNING() { EMC_SPINDLE_OFF emc_spindle_off_msg; @@ -2066,7 +2167,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); } @@ -2085,6 +2186,7 @@ void START_CHANGE() void CHANGE_TOOL(int slot) { EMC_TRAJ_LINEAR_MOVE linearMoveMsg; + linearMoveMsg.feed_mode = feed_mode; EMC_TOOL_LOAD load_tool_msg; @@ -2130,6 +2232,7 @@ void CHANGE_TOOL(int slot) linearMoveMsg.end = to_ext_pose(x, y, z, a, b, c, u, v, w); + linearMoveMsg.pure_angular = angular_move && !cartesian_move; linearMoveMsg.vel = linearMoveMsg.ini_maxvel = toExtVel(vel); linearMoveMsg.acc = toExtAcc(acc); linearMoveMsg.type = EMC_MOTION_TYPE_TOOLCHANGE; @@ -2137,14 +2240,16 @@ void CHANGE_TOOL(int slot) linearMoveMsg.indexrotary = -1; int old_feed_mode = feed_mode; - if(feed_mode) + if(feed_mode) { STOP_SPEED_FEED_SYNCH(); + } - if(vel && acc) + if(vel && acc) { 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); } @@ -2580,12 +2685,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 @@ -2611,20 +2718,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) @@ -2733,7 +2859,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 @@ -2802,7 +2928,7 @@ int GET_EXTERNAL_FLOOD() double GET_EXTERNAL_SPEED() { // speed is in RPMs everywhere - return spindleSpeed; + return spindleSpeed_rpm; } CANON_DIRECTION GET_EXTERNAL_SPINDLE() @@ -3296,12 +3422,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/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 0f0e70f6793..9062aada703 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -1817,7 +1817,7 @@ static int emcTaskIssueCommand(NMLmsg * cmd) retval = emcTrajLinearMove(emcTrajLinearMoveMsg->end, emcTrajLinearMoveMsg->type, emcTrajLinearMoveMsg->vel, emcTrajLinearMoveMsg->ini_maxvel, emcTrajLinearMoveMsg->acc, - emcTrajLinearMoveMsg->indexrotary); + emcTrajLinearMoveMsg->indexrotary, emcTrajLinearMoveMsg->pure_angular); break; case EMC_TRAJ_CIRCULAR_MOVE_TYPE: @@ -1827,7 +1827,8 @@ static int emcTaskIssueCommand(NMLmsg * cmd) emcTrajCircularMoveMsg->turn, emcTrajCircularMoveMsg->type, emcTrajCircularMoveMsg->vel, emcTrajCircularMoveMsg->ini_maxvel, - emcTrajCircularMoveMsg->acc); + emcTrajCircularMoveMsg->acc, + emcTrajCircularMoveMsg->acc_normal); break; case EMC_TRAJ_PAUSE_TYPE: @@ -1894,6 +1895,7 @@ static int emcTaskIssueCommand(NMLmsg * cmd) ((EMC_TRAJ_PROBE *) cmd)->vel, ((EMC_TRAJ_PROBE *) cmd)->ini_maxvel, ((EMC_TRAJ_PROBE *) cmd)->acc, + ((EMC_TRAJ_PROBE *) cmd)->pure_angular, ((EMC_TRAJ_PROBE *) cmd)->probe_type); break; diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 0e9a8cf46d5..55ed1494982 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -987,7 +987,7 @@ int emcTrajSetTermCond(int cond, double tolerance) } int emcTrajLinearMove(EmcPose end, int type, double vel, double ini_maxvel, double acc, - int indexrotary) + int indexrotary, int pure_angular) { #ifdef ISNAN_TRAP if (std::isnan(end.tran.x) || std::isnan(end.tran.y) || std::isnan(end.tran.z) || @@ -1008,20 +1008,22 @@ int emcTrajLinearMove(EmcPose end, int type, double vel, double ini_maxvel, doub emcmotCommand.ini_maxvel = ini_maxvel; emcmotCommand.acc = acc; emcmotCommand.turn = indexrotary; + emcmotCommand.pure_angular = pure_angular; return usrmotWriteEmcmotCommand(&emcmotCommand); } int emcTrajCircularMove(EmcPose end, PM_CARTESIAN center, - PM_CARTESIAN normal, int turn, int type, double vel, double ini_maxvel, double acc) + PM_CARTESIAN normal, int turn, int type, double vel, double ini_maxvel, double acc, double acc_normal) { #ifdef ISNAN_TRAP if (std::isnan(end.tran.x) || std::isnan(end.tran.y) || std::isnan(end.tran.z) || std::isnan(end.a) || std::isnan(end.b) || std::isnan(end.c) || std::isnan(end.u) || std::isnan(end.v) || std::isnan(end.w) || std::isnan(center.x) || std::isnan(center.y) || std::isnan(center.z) || - std::isnan(normal.x) || std::isnan(normal.y) || std::isnan(normal.z)) { - printf("std::isnan error in emcTrajCircularMove()\n"); + std::isnan(normal.x) || std::isnan(normal.y) || std::isnan(normal.z) || + std::isnan(acc_normal)) { + printf("std::isnan error in emcTrajCircularMove()\n"); return 0; // ignore it for now, just don't send it } #endif @@ -1045,6 +1047,7 @@ int emcTrajCircularMove(EmcPose end, PM_CARTESIAN center, emcmotCommand.vel = vel; emcmotCommand.ini_maxvel = ini_maxvel; emcmotCommand.acc = acc; + emcmotCommand.acc_normal = acc_normal; return usrmotWriteEmcmotCommand(&emcmotCommand); } @@ -1056,7 +1059,7 @@ int emcTrajClearProbeTrippedFlag() return usrmotWriteEmcmotCommand(&emcmotCommand); } -int emcTrajProbe(EmcPose pos, int type, double vel, double ini_maxvel, double acc, unsigned char probe_type) +int emcTrajProbe(EmcPose pos, int type, double vel, double ini_maxvel, double acc, int pure_angular, unsigned char probe_type) { #ifdef ISNAN_TRAP if (std::isnan(pos.tran.x) || std::isnan(pos.tran.y) || std::isnan(pos.tran.z) || @@ -1074,6 +1077,7 @@ int emcTrajProbe(EmcPose pos, int type, double vel, double ini_maxvel, double ac emcmotCommand.vel = vel; emcmotCommand.ini_maxvel = ini_maxvel; emcmotCommand.acc = acc; + emcmotCommand.pure_angular = pure_angular; emcmotCommand.probe_type = probe_type; return usrmotWriteEmcmotCommand(&emcmotCommand); @@ -1395,7 +1399,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); @@ -1414,9 +1418,9 @@ int emcSpindleOn(double speed, double css_factor, double offset) { emcmotCommand.command = EMCMOT_SPINDLE_ON; - emcmotCommand.vel = speed; - emcmotCommand.ini_maxvel = css_factor; - emcmotCommand.acc = offset; + emcmotCommand.spindle_speed = speed; + emcmotCommand.css_factor = css_factor; + emcmotCommand.css_xoffset = offset; return usrmotWriteEmcmotCommand(&emcmotCommand); } @@ -1505,12 +1509,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]; @@ -1569,6 +1582,15 @@ int emcSetupArcBlends(int arcBlendEnable, return usrmotWriteEmcmotCommand(&emcmotCommand); } +int emcSetupConsistencyChecks(int consistency_checks, + double max_position_drift_error) +{ + emcmotCommand.command = EMCMOT_SETUP_CONSISTENCY_CHECKS; + emcmotCommand.consistencyCheckConfig.extraConsistencyChecks = consistency_checks; + emcmotCommand.consistencyCheckConfig.maxPositionDriftError = max_position_drift_error; + return usrmotWriteEmcmotCommand(&emcmotCommand); +} + int emcSetMaxFeedOverride(double maxFeedScale) { emcmotCommand.command = EMCMOT_SET_MAX_FEED_OVERRIDE; emcmotCommand.maxFeedScale = maxFeedScale; diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index 21c3d21c971..6e1ed0a4f94 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -38,13 +38,7 @@ double findMaxTangentAngle(double v_plan, double acc_limit, double cycle_time) //TODO somewhat redundant with findKinkAccel, should refactor double acc_margin = BLEND_ACC_RATIO_NORMAL * BLEND_KINK_FACTOR * acc_limit; double dx = v_plan / cycle_time; - if (dx > 0.0) { - return (acc_margin / dx); - } else { - tp_debug_print(" Velocity or period is negative!\n"); - //Should not happen... - return TP_ANGLE_EPSILON; - } + return fabs(acc_margin / fmax(dx, TP_POS_EPSILON)); } @@ -57,12 +51,7 @@ double findMaxTangentAngle(double v_plan, double acc_limit, double cycle_time) double findKinkAccel(double kink_angle, double v_plan, double cycle_time) { double dx = v_plan / cycle_time; - if (dx > 0.0) { - return (dx * kink_angle); - } else { - rtapi_print_msg(RTAPI_MSG_ERR, "dx < 0 in KinkAccel\n"); - return 0; - } + return fabs(dx * kink_angle); } @@ -318,9 +307,11 @@ int pmCartCartParallel(PmCartesian const * const u1, pmCartMagSq(&u_diff, &d_diff); } - tp_debug_json_start(pmCartCartParallel); - tp_debug_json_double(d_diff); - tp_debug_json_end(); +#if defined(TP_DEBUG) && defined(TP_PEDANTIC) + print_json5_log_start(pmCartCartParallel, Check); + print_json5_double(d_diff); + print_json5_end_(); +#endif return d_diff < tol; } @@ -342,9 +333,11 @@ int pmCartCartAntiParallel(PmCartesian const * const u1, pmCartMagSq(&u_sum, &d_sum); } - tp_debug_json_start(pmCartCartAntiParallel); - tp_debug_json_double(d_sum); - tp_debug_json_end(); +#if defined(TP_DEBUG) && defined(TP_PEDANTIC) + print_json5_log_start(pmCartCartAntiParallel, Check); + print_json5_double(d_sum); + print_json5_end_(); +#endif return d_sum < tol; } @@ -408,13 +401,13 @@ double pmCartMin(PmCartesian const * const in) * Calculate the diameter of a circle incscribed on a central cross section of a 3D * rectangular prism. * - * @param normal normal direction of plane slicing prism. + * @param normal normal direction of plane slicing prism (must be unit length!). * @param extents distance from center to one corner of the prism. * @param diameter diameter of inscribed circle on cross section. * */ -int calculateInscribedDiameter(PmCartesian const * const normal, - PmCartesian const * const bounds, double * const diameter) +int calculateInscribedRadius(PmCartesian const * const normal, + PmCartesian const * const bounds, double * const radius) { if (!normal ) { return TP_ERR_MISSING_INPUT; @@ -465,28 +458,28 @@ int calculateInscribedDiameter(PmCartesian const * const normal, pmCartMag(&planar_z, &z_scale); double x_extent=0, y_extent=0, z_extent=0; - if (bounds->x != 0) { + if (bounds->x > 0) { x_extent = bounds->x / x_scale; } - if (bounds->y != 0) { + if (bounds->y > 0) { y_extent = bounds->y / y_scale; } - if (bounds->z != 0) { + if (bounds->z > 0) { z_extent = bounds->z / z_scale; } // Find the highest value to start from - *diameter = fmax(fmax(x_extent, y_extent),z_extent); + *radius = fmax(fmax(x_extent, y_extent),z_extent); // Only for active axes, find the minimum extent - if (bounds->x != 0) { - *diameter = fmin(*diameter, x_extent); + if (bounds->x > 0) { + *radius = fmin(*radius, x_extent); } - if (bounds->y != 0) { - *diameter = fmin(*diameter, y_extent); + if (bounds->y > 0) { + *radius = fmin(*radius, y_extent); } - if (bounds->z != 0) { - *diameter = fmin(*diameter, z_extent); + if (bounds->z > 0) { + *radius = fmin(*radius, z_extent); } return TP_ERR_OK; @@ -635,7 +628,7 @@ int blendParamKinematics(BlendGeom3 * const geom, tcFindBlendTolerance(prev_tc, tc, ¶m->tolerance, &nominal_tolerance); // Calculate max acceleration based on plane containing lines - int res_dia = calculateInscribedDiameter(&geom->binormal, acc_bound, ¶m->a_max); + int res_dia = calculateInscribedRadius(&geom->binormal, acc_bound, ¶m->a_max); // Store max normal acceleration param->a_n_max = param->a_max * BLEND_ACC_RATIO_NORMAL; @@ -655,7 +648,7 @@ int blendParamKinematics(BlendGeom3 * const geom, // Calculate the maximum planar velocity double v_planar_max; //FIXME sloppy handling of return value - res_dia |= calculateInscribedDiameter(&geom->binormal, vel_bound, &v_planar_max); + res_dia |= calculateInscribedRadius(&geom->binormal, vel_bound, &v_planar_max); tp_debug_print("v_planar_max = %f\n", v_planar_max); // Clip the angle at a reasonable value (less than 90 deg), to prevent div by zero @@ -737,7 +730,7 @@ int blendInit3FromLineArc(BlendGeom3 * const geom, BlendParameters * const param &geom->center2, &geom->radius2); // Handle convexity - param->convex2 = arcConvexTest(&geom->center2, &geom->P, &geom->u_tan1, true); + param->convex2 = checkRayIntersectsArc(&geom->center2, &geom->P, &geom->u_tan1, true); tp_debug_print("circ2 convex: %d\n", param->convex2); @@ -823,7 +816,7 @@ int blendInit3FromArcLine(BlendGeom3 * const geom, BlendParameters * const param &geom->center1, &geom->radius1); - param->convex1 = arcConvexTest(&geom->center1, &geom->P, &geom->u_tan2, false); + param->convex1 = checkRayIntersectsArc(&geom->center1, &geom->P, &geom->u_tan2, false); tp_debug_print("circ1 convex: %d\n", param->convex1); @@ -939,8 +932,8 @@ int blendInit3FromArcArc(BlendGeom3 * const geom, BlendParameters * const param, geom->P.y, geom->P.z); - param->convex1 = arcConvexTest(&geom->center1, &geom->P, &geom->u_tan2, false); - param->convex2 = arcConvexTest(&geom->center2, &geom->P, &geom->u_tan1, true); + param->convex1 = checkRayIntersectsArc(&geom->center1, &geom->P, &geom->u_tan2, false); + param->convex2 = checkRayIntersectsArc(&geom->center2, &geom->P, &geom->u_tan1, true); tp_debug_print("circ1 convex: %d, circ2 convex: %d\n", param->convex1, param->convex2); @@ -1628,7 +1621,9 @@ PmCircleLimits pmCircleActualMaxVel(PmCircle const * circle, { double a_n_max_cutoff = BLEND_ACC_RATIO_NORMAL * a_max; + // For debugging only double eff_radius = pmCircleEffectiveMinRadius(circle); + // Find the acceleration necessary to reach the maximum velocity double a_n_vmax = pmSq(v_max) / fmax(eff_radius, DOUBLE_FUZZ); // Find the maximum velocity that still obeys our desired tangential / total acceleration ratio @@ -1643,14 +1638,21 @@ PmCircleLimits pmCircleActualMaxVel(PmCircle const * circle, acc_ratio_tan = pmSqrt(1.0 - pmSq(a_n_vmax / a_max)); } - tp_debug_json_start(pmCircleActualMaxVel); - tp_debug_json_double(eff_radius); - tp_debug_json_double(v_max); - tp_debug_json_double(v_max_cutoff); - tp_debug_json_double(a_n_max_cutoff); - tp_debug_json_double(a_n_vmax); - tp_debug_json_double(acc_ratio_tan); - tp_debug_json_end(); +#ifdef TP_DEBUG + print_json5_log_start(pmCircleActualMaxVel, Check); + print_json5_double_("angle", circle->angle); + print_json5_double_("nominal_radius", circle->radius); + double r_spiral = spiralEffectiveRadius(circle); + print_json5_double_("spiral_radius", r_spiral); + print_json5_double(eff_radius); + print_json5_double(v_max); + print_json5_double(v_max_cutoff); + print_json5_double(a_max); + print_json5_double(a_n_max_cutoff); + print_json5_double(a_n_vmax); + print_json5_double(acc_ratio_tan); + print_json5_log_end(); +#endif PmCircleLimits limits = { v_max_actual, @@ -1717,14 +1719,22 @@ static int pmCircleAngleFromParam(PmCircle const * const circle, return TP_ERR_OK; } - -static void printSpiralArcLengthFit(SpiralArcLengthFit const * const fit) +static void printSpiralArcLengthFit(SpiralArcLengthFit const * const fit, + double min_radius, + double total_angle, + double spiral_coef) { - tp_debug_print("Spiral fit: b0 = %.12f, b1 = %.12f, length = %.12f, spiral_in = %d\n", - fit->b0, - fit->b1, - fit->total_planar_length, - fit->spiral_in); +#ifdef TP_DEBUG + print_json5_log_start(SpiralFit, Command); + print_json5_double(min_radius); + print_json5_double(total_angle); + print_json5_double(spiral_coef); + print_json5_double_("b0", fit->b0); + print_json5_double_("b1", fit->b1); + print_json5_double_("total_planar_length", fit->total_planar_length); + print_json5_bool_("spiral_in", fit->spiral_in); + print_json5_log_end(); +#endif } /** @@ -1764,9 +1774,6 @@ int findSpiralArcLengthFit(PmCircle const * const circle, } else { fit->spiral_in = false; } - tp_debug_print("radius = %.12f, angle = %.12f\n", min_radius, circle->angle); - tp_debug_print("spiral_coef = %.12f\n", spiral_coef); - //Compute the slope of the arc length vs. angle curve at the start and end of the segment double slope_start = pmSqrt(pmSq(min_radius) + pmSq(spiral_coef)); @@ -1776,7 +1783,7 @@ int findSpiralArcLengthFit(PmCircle const * const circle, fit->b1 = slope_start; fit->total_planar_length = fit->b0 * pmSq(circle->angle) + fit->b1 * circle->angle; - printSpiralArcLengthFit(fit); + printSpiralArcLengthFit(fit, min_radius, circle->angle, spiral_coef); // Check against start and end angle double angle_end_chk = 0.0; @@ -1819,6 +1826,19 @@ int pmCircleAngleFromProgress(PmCircle const * const circle, return pmCircleAngleFromParam(circle, fit, t, angle); } +double spiralEffectiveRadius(PmCircle const * circle) +{ + double dr = circle->spiral / circle->angle; + + // Exact representation of spiral arc length flattened into + double n_inner = pmSq(dr) + pmSq(circle->radius); + double den = n_inner+pmSq(dr); + double num = pmSqrt(pmCb(n_inner)); + double r_spiral = num / den; + + return r_spiral; +} + /** * Find the effective minimum radius for acceleration calculations. @@ -1827,19 +1847,116 @@ int pmCircleAngleFromProgress(PmCircle const * const circle, */ double pmCircleEffectiveMinRadius(PmCircle const * circle) { - double dr = circle->spiral / circle->angle; double h2; pmCartMagSq(&circle->rHelix, &h2); + double dh2 = h2 / pmSq(circle->angle); - // Exact representation of spiral arc length flattened into - double n_inner = pmSq(dr) + pmSq(circle->radius); - double den = n_inner+pmSq(dr); - double num = pmSqrt(n_inner * n_inner * n_inner); - double r_spiral = num / den; + double r_spiral = spiralEffectiveRadius(circle); // Curvature of helix, assuming that helical motion is independent of plane motion - double effective_radius = h2 / r_spiral + r_spiral; + double effective_radius = dh2 / r_spiral + r_spiral; 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; +} + +EndCondition checkEndCondition(double cycleTime, + double progress, + double target, + double currentvel, + double v_f, + double a_max) +{ + double dx = target - progress; + // Start with safe defaults (segment will not end next cycle + EndCondition out = { + v_f, + TP_BIG_NUM * cycleTime, + }; + + double v_avg = (currentvel + v_f) / 2.0; + + //Check that we have a non-zero "average" velocity between now and the + //finish. If not, it means that we have to accelerate from a stop, which + //will take longer than the minimum 2 timesteps that each segment takes, so + //we're safely far form the end. + + //Get dt assuming that we can magically reach the final velocity at + //the end of the move. + // + //KLUDGE: start with a value below the cutoff + double dt = TP_TIME_EPSILON / 2.0; + if (v_avg > TP_VEL_EPSILON) { + //Get dt from distance and velocity (avoid div by zero) + dt = fmax(dt, dx / v_avg); + } else { + if ( dx > (v_avg * cycleTime) && dx > TP_POS_EPSILON) { + tc_pdebug_print(" below velocity threshold, assuming far from end\n"); + return out; + } + } + + + // Assuming a full timestep: + double dv = v_f - currentvel; + double a_f = dv / fmax(dt, TP_TIME_EPSILON); + + //If this is a valid acceleration, then we're done. If not, then we solve + //for v_f and dt given the max acceleration allowed. + //If we exceed the maximum acceleration, then the dt estimate is too small. + double a = a_f; + int recalc = sat_inplace(&a, a_max); + + //Need to recalculate vf and above + if (recalc) { + double disc = pmSq(currentvel / a) + 2.0 / a * dx; + if (disc < 0) { + //Should mean that dx is too big, i.e. we're not close enough + tc_pdebug_print(" dx = %f, too large, not at end yet\n",dx); + return out; + } + + if (disc < TP_TIME_EPSILON * TP_TIME_EPSILON) { + tc_pdebug_print("disc too small, skipping sqrt\n"); + dt = -currentvel / a; + } else if (a > 0) { + tc_pdebug_print("using positive sqrt\n"); + dt = -currentvel / a + pmSqrt(disc); + } else { + tc_pdebug_print("using negative sqrt\n"); + dt = -currentvel / a - pmSqrt(disc); + } + + tc_pdebug_print(" revised dt = %f\n", dt); + //Update final velocity with actual result + out.v_f = currentvel + dt * a; + } + + out.dt = dt; + return out; +} diff --git a/src/emc/tp/blendmath.h b/src/emc/tp/blendmath.h index 6766e3e8d64..b41f79b0724 100644 --- a/src/emc/tp/blendmath.h +++ b/src/emc/tp/blendmath.h @@ -141,13 +141,19 @@ int findIntersectionAngle(PmCartesian const * const u1, double pmCartMin(PmCartesian const * const in); -int calculateInscribedDiameter(PmCartesian const * const normal, - PmCartesian const * const bounds, double * const diameter); +int calculateInscribedRadius(PmCartesian const * const normal, + PmCartesian const * const bounds, double * const radius); 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 pmUnitCartsColinear(PmCartesian const * const u1, PmCartesian const * const u2); @@ -256,9 +262,24 @@ int pmCircleAngleFromProgress(PmCircle const * const circle, double progress, double * const angle); double pmCircleEffectiveMinRadius(const PmCircle *circle); +double spiralEffectiveRadius(PmCircle const * circle); static inline double findVPeak(double a_t_max, double distance) { return pmSqrt(a_t_max * distance); } + +// Start by mimicing the current structure for minimal changes +typedef struct { + double v_f; + double dt; +} EndCondition; + +EndCondition checkEndCondition(double cycleTime, + double progress, + double target, + double currentvel, + double v_f, + double a_max); + #endif diff --git a/src/emc/tp/joint_util.c b/src/emc/tp/joint_util.c new file mode 100644 index 00000000000..34910932755 --- /dev/null +++ b/src/emc/tp/joint_util.c @@ -0,0 +1,125 @@ +#include "joint_util.h" +#include "motion_debug.h" +#include "rtapi_math.h" + +extern emcmot_debug_t *emcmotDebug; + +PmCartesian getXYZAccelBounds() { + PmCartesian acc_bound = { + emcmotDebug->joints[0].acc_limit, + emcmotDebug->joints[1].acc_limit, + emcmotDebug->joints[2].acc_limit, + }; + return acc_bound; +} + +PmCartesian getXYZVelBounds() { + PmCartesian vel_bound = { + emcmotDebug->joints[0].vel_limit, + emcmotDebug->joints[1].vel_limit, + emcmotDebug->joints[2].vel_limit, + }; + return vel_bound; +} + +unsigned jointAccelViolation(int joint_idx, double acc) +{ + // Allow some numerical tolerance here since max acceleration is a bit + // fuzzy anyway. Torque is the true limiting factor, so there has to be + // some slack here anyway due to variations in table load, friction, etc. + static const double ABS_TOL = 1e-4; + const double REL_TOL = 1.0 + ABS_TOL; + const double a_max_nominal = emcmotDebug->joints[joint_idx].acc_limit; + const double a_limit = fmax(a_max_nominal * REL_TOL, a_max_nominal + ABS_TOL); + return (unsigned)(fabs(acc) > a_limit) << joint_idx; +} + +unsigned jointVelocityViolation(int joint_idx, double v_actual) +{ + // Allow some numerical tolerance here since max acceleration is a bit + // fuzzy anyway. Torque is the true limiting factor, so there has to be + // some slack here anyway due to variations in table load, friction, etc. + static const double ABS_TOL = 1e-2; + const double REL_ACC_TOL = 1.0 + ABS_TOL; + const double v_max_nominal = emcmotDebug->joints[joint_idx].vel_limit; + const double v_limit = fmax(v_max_nominal * REL_ACC_TOL, v_max_nominal + ABS_TOL); + return (unsigned)(fabs(v_actual) > v_limit) << joint_idx; +} + +unsigned jointPositionViolation(int joint_idx, double position) +{ + static const double ABS_TOL = 1e-6; + return (unsigned)(position < emcmotDebug->joints[joint_idx].min_pos_limit - ABS_TOL && + position > emcmotDebug->joints[joint_idx].max_pos_limit + ABS_TOL) << joint_idx; +} + +/** + * Checks for any acceleration violations based on axis limits. + * + * @return 0 if all acceleration bounds are respected, or a bit-set of failed axes (in XYZABCUVW bit order). + */ +unsigned findAccelViolations(EmcPose axis_accel) +{ + // Bit-mask each failure so we can report all failed axes + unsigned fail_bits = (unsigned)(0x0 + | jointAccelViolation(0, axis_accel.tran.x) + | jointAccelViolation(1, axis_accel.tran.y) + | jointAccelViolation(2, axis_accel.tran.z) + | jointAccelViolation(3, axis_accel.a) + | jointAccelViolation(4, axis_accel.b) + | jointAccelViolation(5, axis_accel.c) + | jointAccelViolation(6, axis_accel.u) + | jointAccelViolation(7, axis_accel.v) + | jointAccelViolation(8, axis_accel.w)); + return fail_bits; +} + +unsigned findVelocityViolations(EmcPose axis_vel) +{ + // Bit-mask each failure so we can report all failed axes + unsigned fail_bits = (unsigned)(0x0 + | jointVelocityViolation(0, axis_vel.tran.x) + | jointVelocityViolation(1, axis_vel.tran.y) + | jointVelocityViolation(2, axis_vel.tran.z) + | jointVelocityViolation(3, axis_vel.a) + | jointVelocityViolation(4, axis_vel.b) + | jointVelocityViolation(5, axis_vel.c) + | jointVelocityViolation(6, axis_vel.u) + | jointVelocityViolation(7, axis_vel.v) + | jointVelocityViolation(8, axis_vel.w)); + return fail_bits; +} + +unsigned findPositionLimitViolations(EmcPose axis_pos) +{ + // Bit-mask each failure so we can report all failed axes + unsigned fail_bits = (unsigned)(0x0 + | jointPositionViolation(0, axis_pos.tran.x) + | jointPositionViolation(1, axis_pos.tran.y) + | jointPositionViolation(2, axis_pos.tran.z) + | jointPositionViolation(3, axis_pos.a) + | jointPositionViolation(4, axis_pos.b) + | jointPositionViolation(5, axis_pos.c) + | jointPositionViolation(6, axis_pos.u) + | jointPositionViolation(7, axis_pos.v) + | jointPositionViolation(8, axis_pos.w)); + return fail_bits; +} + + +double findMinNonZero(const PmCartesian * const bounds) { + //Start with max accel value + double act_limit = fmax(fmax(bounds->x, bounds->y), bounds->z); + + // Compare only with active axes + if (bounds->x > 0) { + act_limit = fmin(act_limit, bounds->x); + } + if (bounds->y > 0) { + act_limit = fmin(act_limit, bounds->y); + } + if (bounds->z > 0) { + act_limit = fmin(act_limit, bounds->z); + } + return act_limit; +} diff --git a/src/emc/tp/joint_util.h b/src/emc/tp/joint_util.h new file mode 100644 index 00000000000..76b470b0e3d --- /dev/null +++ b/src/emc/tp/joint_util.h @@ -0,0 +1,21 @@ +#ifndef JOINT_UTIL_H +#define JOINT_UTIL_H + +#include +#include + +PmCartesian getXYZAccelBounds(); + +PmCartesian getXYZVelBounds(); + +unsigned findAccelViolations(EmcPose axis_accel); +unsigned findVelocityViolations(EmcPose axis_vel); +unsigned findPositionLimitViolations(EmcPose axis_pos); + +/** + * Finds the smallest non-zero component in a non-negative "bounds" vector. + * Used to identify the "slowest" axis, but ignore axes + */ +double findMinNonZero(PmCartesian const * const bounds); + +#endif // JOINT_UTIL_H diff --git a/src/emc/tp/math_util.c b/src/emc/tp/math_util.c new file mode 100644 index 00000000000..f253ea392eb --- /dev/null +++ b/src/emc/tp/math_util.c @@ -0,0 +1,25 @@ +/******************************************************************** +* Description: math_util.c +* +* Simpler math helper functions that seem to be missing from rtapi_math. +* +* Author: Robert W. Ellenberg +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +********************************************************************/ + +#include "math_util.h" + +long max(long y, long x) { + return y > x ? y : x; +} + +long min(long y, long x) { + return y < x ? y : x; +} + +double signum(double x) { + return (x > 0.0) ? 1.0 : (x < 0.0) ? -1.0 : 0.0; +} diff --git a/src/emc/tp/math_util.h b/src/emc/tp/math_util.h new file mode 100644 index 00000000000..f4adedbdfb6 --- /dev/null +++ b/src/emc/tp/math_util.h @@ -0,0 +1,20 @@ +/******************************************************************** +* Description: math_util.h +* +* Simpler math helper functions that seem to be missing from rtapi_math. +* +* Author: Robert W. Ellenberg +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +********************************************************************/ + +#ifndef MATH_UTIL_H +#define MATH_UTIL_H + +long max(long y, long x); +long min(long y, long x); +double signum(double x); + +#endif // MATH_UTIL_H diff --git a/src/emc/tp/rtapi_json5.h b/src/emc/tp/rtapi_json5.h new file mode 100644 index 00000000000..74533cb114f --- /dev/null +++ b/src/emc/tp/rtapi_json5.h @@ -0,0 +1,143 @@ +/** + * @file rtapi_json5.h + * + * @author Robert W. Ellenberg + * @copyright Copyright (c) 2019 All rights reserved. This project is released under the GPL v2 license. + */ + +#ifndef RTAPI_JSON5_H +#define RTAPI_JSON5_H + +#include "rtapi.h" /* printing functions */ +#include "posemath.h" +#include "emcpos.h" +#include "blendmath.h" + +// Macro wrappers to stringify the variable name +// NOTE: callers can directly call the underlying print function if a custom field name is needed +#define print_json5_double(varname_) print_json5_double_(#varname_, (double)varname_) +#define print_json5_string(varname_) print_json5_string_(#varname_, varname_) +#define print_json5_long(varname_) print_json5_long_(#varname_, (long)varname_) +#define print_json5_unsigned(varname_) print_json5_unsigned_(#varname_, (unsigned)varname_) +#define print_json5_PmCartesian(varname_) print_json5_PmCartesian_(#varname_, varname_) +#define print_json5_EmcPose(varname_) print_json5_EmcPose_(#varname_, varname_) +#define print_json5_PmCartLine(varname_) print_json5_PmCartLine_( #varname_, varname_) +#define print_json5_PmCircle(varname_) print_json5_PmCircle_(#varname_, varname_) + +// Hacky way to do realtime-logging of +// KLUDGE shove these in the header to avoid linking issues + +static inline void print_json5_start_() +{ + rtapi_print("{"); +} + +static inline void print_json5_end_() +{ + rtapi_print("}\n"); +} + +static inline void print_json5_object_start_(const char *fname) +{ + rtapi_print("%s: {", fname ?: "NULL"); +} + +static inline void print_json5_object_end_() +{ + rtapi_print("}, "); +} + +static inline void print_json5_double_(const char *varname, double value) +{ + rtapi_print("%s: %0.17g, ", varname ?: "NO_NAME", value); +} + +static inline void print_json5_bool_(const char *varname, double value) +{ + rtapi_print("%s: %s, ", varname ?: "NO_NAME", value ? "true" : "false"); +} + +static inline void print_json5_int_(const char *varname, int value) +{ + rtapi_print("%s: %d, ", varname ?: "NO_NAME", value); +} + +static inline void print_json5_long_(const char *varname, long value) +{ + rtapi_print("%s: %ld, ", varname ?: "NO_NAME", value); +} + +static inline void print_json5_ll_(const char *varname, long long value) +{ + rtapi_print("%s: %lld, ", varname ?: "NO_NAME", value); +} + +static inline void print_json5_unsigned_(const char *varname, unsigned value) +{ + rtapi_print("%s: %u, ", varname ?: "NO_NAME", value); +} + +static inline void print_json5_string_(const char *field_name, const char *value) +{ + // TODO handle nulls gracefully with proper JSON null + rtapi_print("%s: \"%s\", ", field_name ?: "NULL", value ?: "NULL"); +} + +static inline void print_json5_PmCartesian_(const char *varname, PmCartesian value) +{ + + rtapi_print("%s: [%0.17g, %0.17g, %0.17g], ", varname ?: "NO_NAME", value.x, value.y, value.z); +} + +static inline void print_json5_EmcPose_(const char *varname, EmcPose value) +{ + rtapi_print("%s: [%0.17g, %0.17g, %0.17g, %0.17g, %0.17g, %0.17g, %0.17g, %0.17g, %0.17g], ", + varname, + value.tran.x, + value.tran.y, + value.tran.z, + value.a, + value.b, + value.c, + value.u, + value.v, + value.w); +} + +static inline void print_json5_PmCartLine_(const char *name, PmCartLine value) +{ + print_json5_object_start_(name); + // Need manual field names here + print_json5_PmCartesian_("start", value.start); + print_json5_PmCartesian_("end", value.start); + print_json5_PmCartesian_("uVec", value.uVec); + print_json5_double_("tmag", value.tmag); + print_json5_double_("tmag_zero", value.tmag_zero); + print_json5_object_end_(); +} + +static inline void print_json5_PmCircle_(const char *name, PmCircle circle) +{ + print_json5_object_start_(name); + print_json5_PmCartesian_("center", circle.center); + print_json5_PmCartesian_("normal", circle.normal); + print_json5_PmCartesian_("rTan", circle.rTan); + print_json5_PmCartesian_("rPerp", circle.rPerp); + print_json5_PmCartesian_("rHelix", circle.rHelix); + print_json5_double_("radius", circle.radius); + print_json5_double_("angle", circle.angle); + print_json5_double_("spiral", circle.spiral); + print_json5_object_end_(); +} + +static inline void print_json5_SpiralArcLengthFit_(const char *name, SpiralArcLengthFit fit) +{ + print_json5_object_start_(name); + print_json5_double_("b0", fit.b0); + print_json5_double_("b1", fit.b1); + print_json5_double_("total_planar_length", fit.total_planar_length); + print_json5_long_("spiral_in", fit.spiral_in); + print_json5_object_end_(); +} + +#endif // RTAPI_JSON5_H diff --git a/src/emc/tp/spherical_arc.c b/src/emc/tp/spherical_arc.c index 18a2c099abe..532632baa2b 100644 --- a/src/emc/tp/spherical_arc.c +++ b/src/emc/tp/spherical_arc.c @@ -23,9 +23,9 @@ int arcInitFromPoints(SphericalArc * const arc, PmCartesian const * const start, PmCartesian const * const center) { #ifdef ARC_PEDANTIC - if (!P0 || !P1 || !center) { + if (!start || !end || !center) { return TP_ERR_MISSING_INPUT; - + } if (!arc) { return TP_ERR_MISSING_OUTPUT; } @@ -86,18 +86,19 @@ int arcInitFromPoints(SphericalArc * const arc, PmCartesian const * const start, int arcPoint(SphericalArc const * const arc, double progress, PmCartesian * const out) { - //TODO pedantic +#ifdef TP_PEDANTIC + if (!arc) {return TP_ERR_MISSING_INPUT;} + if (!out) {return TP_ERR_MISSING_OUTPUT;} +#endif //Convert progress to actual progress around the arc double net_progress = progress - arc->line_length; if (net_progress <= 0.0 && arc->line_length > 0) { - tc_debug_print("net_progress = %f, line_length = %f\n", net_progress, arc->line_length); //Get position on line (not actually an angle in this case) pmCartScalMult(&arc->uTan, net_progress, out); pmCartCartAdd(out, &arc->start, out); } else { double angle_in = net_progress / arc->radius; - tc_debug_print("angle_in = %f, angle_total = %f\n", angle_in, arc->angle); double scale0 = sin(arc->angle - angle_in) / arc->Sangle; double scale1 = sin(angle_in) / arc->Sangle; @@ -161,16 +162,32 @@ int arcFromLines(SphericalArc * const arc, PmCartLine const * const line1, return arcInitFromPoints(arc, start, end, ¢er); } -int arcConvexTest(PmCartesian const * const center, - PmCartesian const * const P, PmCartesian const * const uVec, int reverse_dir) + +/** + * Tests if a ray from the specified point / direction intersects the circle with the given center. + * + * @note that the circle is assumed to be coindicent with P (so the radius is || P - center ||). + * + * For blending, this is used to determine if a given segment forms a convex or + * concave intersection with another segment. "Convex" means that the tangent + * vector of the other segment (at P) is in the interior of the circle + * containing this segment. For an arc-arc intersection, there are actually 4 possibilities: + * + * convex-convex (each arc's tangent vector is in the other's interior) + * concave-convex + * convex-concave + * concave-concave + */ +int checkRayIntersectsArc(PmCartesian const * const center, + PmCartesian const * const P, + PmCartesian const * const u_ray, + int reverse_dir) { - //Check if an arc-line intersection is concave or convex double dot; - PmCartesian diff; - pmCartCartSub(P, center, &diff); - pmCartCartDot(&diff, uVec, &dot); + PmCartesian r_CP; + pmCartCartSub(P, center, &r_CP); + pmCartCartDot(&r_CP, u_ray, &dot); - tp_debug_print("convex test: dot = %f, reverse_dir = %d\n", dot, reverse_dir); int convex = (reverse_dir != 0) ^ (dot < 0); return convex; } diff --git a/src/emc/tp/spherical_arc.h b/src/emc/tp/spherical_arc.h index cea336909b9..a670538b8eb 100644 --- a/src/emc/tp/spherical_arc.h +++ b/src/emc/tp/spherical_arc.h @@ -60,8 +60,8 @@ int arcFromLines(SphericalArc * const arc, PmCartLine const * const line1, PmCartLine const * const line2, double radius, double blend_dist, double center_dist, PmCartesian * const start, PmCartesian * const end, int consume); -int arcConvexTest(PmCartesian const * const center, - PmCartesian const * const P, PmCartesian const * const uVec, int reverse_dir); +int checkRayIntersectsArc(PmCartesian const * const center, + PmCartesian const * const P, PmCartesian const * const u_ray, int reverse_dir); int arcTangent(SphericalArc const * const arc, PmCartesian * const tan, int at_end); #endif diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index 049ff676dbf..f3f6a9928d5 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -31,7 +31,7 @@ double tcGetMaxTargetVel(TC_STRUCT const * const tc, double max_scale) { - double v_max_target; + double v_max_target = tc->maxvel; switch (tc->synchronized) { case TC_SYNC_NONE: @@ -43,8 +43,6 @@ double tcGetMaxTargetVel(TC_STRUCT const * const tc, max_scale = 1.0; case TC_SYNC_POSITION: // Assume no spindle override during blend target - default: - v_max_target = tc->maxvel; break; } @@ -52,7 +50,7 @@ double tcGetMaxTargetVel(TC_STRUCT const * const tc, return fmin(v_max_target, tc->maxvel); } -double tcGetOverallMaxAccel(const TC_STRUCT *tc) +double tcGetAccelScale(const TC_STRUCT *tc) { // Handle any acceleration reduction due to an approximate-tangent "blend" with the previous or next segment double a_scale = (1.0 - fmax(tc->kink_accel_reduce, tc->kink_accel_reduce_prev)); @@ -64,7 +62,11 @@ double tcGetOverallMaxAccel(const TC_STRUCT *tc) a_scale *= 0.5; } - return tc->maxaccel * a_scale; + return a_scale; +} +double tcGetOverallMaxAccel(const TC_STRUCT *tc) +{ + return tc->maxaccel * tcGetAccelScale(tc); } /** @@ -72,7 +74,7 @@ double tcGetOverallMaxAccel(const TC_STRUCT *tc) */ double tcGetTangentialMaxAccel(TC_STRUCT const * const tc) { - double a_scale = tcGetOverallMaxAccel(tc); + double a_scale = tcGetAccelScale(tc); // Reduce allowed tangential acceleration in circular motions to stay // within overall limits (accounts for centripetal acceleration while @@ -81,7 +83,7 @@ double tcGetTangentialMaxAccel(TC_STRUCT const * const tc) //Limit acceleration for cirular arcs to allow for normal acceleration a_scale *= tc->acc_ratio_tan; } - return a_scale; + return tc->maxaccel * a_scale; } @@ -425,26 +427,18 @@ int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos) /** - * Set the terminal condition of a segment. - * This function will eventually handle state changes associated with altering a terminal condition. + * Set the terminal condition (i.e. blend or stop) for the given motion segment. + * Also sets flags on the next segment relevant to blending (e.g. parabolic blend sets the blend_prev flag). */ -int tcSetTermCond(TC_STRUCT *prev_tc, TC_STRUCT *tc, int term_cond) { - switch (term_cond) { - case TC_TERM_COND_STOP: - case TC_TERM_COND_EXACT: - case TC_TERM_COND_TANGENT: - if (tc) {tc->blend_prev = 0;} - break; - case TC_TERM_COND_PARABOLIC: - if (tc) {tc->blend_prev = 1;} - break; - default: - break; - +int tcSetTermCond(TC_STRUCT *tc, TC_STRUCT *nexttc, tc_term_cond_t term_cond) +{ + if (tc) { + tp_debug_print("setting term condition %d on tc id %d, type %d\n", term_cond, tc->id, tc->motion_type); + tc->term_cond = term_cond; } - if (prev_tc) { - tp_debug_print("setting term condition %d on tc id %d, type %d\n", term_cond, prev_tc->id, prev_tc->motion_type); - prev_tc->term_cond = term_cond; + if (nexttc) { + nexttc->blend_prev = (tc && term_cond == TC_TERM_COND_PARABOLIC); + tp_debug_print("set blend_prev flag %d on tc id %d, type %d\n", nexttc->blend_prev, nexttc->id, nexttc->motion_type); } return 0; } @@ -476,6 +470,7 @@ int tcConnectBlendArc(TC_STRUCT * const prev_tc, TC_STRUCT * const tc, tp_debug_print(" L1 end : %f %f %f\n",prev_tc->coords.line.xyz.end.x, prev_tc->coords.line.xyz.end.y, prev_tc->coords.line.xyz.end.z); + } else { tp_debug_print("connect: consume prev_tc\n"); } @@ -526,10 +521,10 @@ int tcFindBlendTolerance(TC_STRUCT const * const prev_tc, double T1 = prev_tc->tolerance; double T2 = tc->tolerance; //Detect zero tolerance = no tolerance and force to reasonable maximum - if (T1 == 0) { + if (T1 <= 0) { T1 = prev_tc->nominal_length * tolerance_ratio; } - if (T2 == 0) { + if (T2 <= 0) { T2 = tc->nominal_length * tolerance_ratio; } *nominal_tolerance = fmin(T1,T2); @@ -576,15 +571,24 @@ int tcFlagEarlyStop(TC_STRUCT * const tc, return TP_ERR_OK; } -double pmLine9Target(PmLine9 * const line9) +double pmLine9Target(PmLine9 * const line9, int pure_angular) { - if (!line9->xyz.tmag_zero) { + if (pure_angular && !line9->abc.tmag_zero) { + return line9->abc.tmag; + } else if (!line9->xyz.tmag_zero) { return line9->xyz.tmag; } else if (!line9->uvw.tmag_zero) { return line9->uvw.tmag; } else if (!line9->abc.tmag_zero) { return line9->abc.tmag; } else { + rtapi_print_msg(RTAPI_MSG_DBG,"line can't have zero length! xyz start = %.12e,%.12e,%.12e, end = %.12e,%.12e,%.12e\n", + line9->xyz.start.x, + line9->xyz.start.y, + line9->xyz.start.z, + line9->xyz.end.x, + line9->xyz.end.y, + line9->xyz.end.z); return 0.0; } } @@ -597,7 +601,7 @@ double pmLine9Target(PmLine9 * const line9) * the struct is properly initialized BEFORE calling this function. */ int tcInit(TC_STRUCT * const tc, - int motion_type, + tc_motion_type_t motion_type, int canon_motion_type, double cycle_time, unsigned char enables, @@ -652,7 +656,6 @@ int tcSetupMotion(TC_STRUCT * const tc, int tcSetupState(TC_STRUCT * const tc, TP_STRUCT const * const tp) { - tcSetTermCond(tc, NULL, tp->termCond); tc->tolerance = tp->tolerance; tc->synchronized = tp->synchronized; tc->uu_per_rev = tp->uu_per_rev; @@ -677,6 +680,14 @@ int pmLine9Init(PmLine9 * const line9, int abc_fail = pmCartLineInit(&line9->abc, &start_abc, &end_abc); int uvw_fail = pmCartLineInit(&line9->uvw, &start_uvw, &end_uvw); +#ifdef TP_DEBUG + print_json5_log_start(pmLine9Init, Command); + print_json5_PmCartLine_("xyz", line9->xyz); + print_json5_PmCartLine_("abc", line9->abc); + print_json5_PmCartLine_("uvw", line9->abc); + print_json5_end_(); +#endif + if (xyz_fail || abc_fail || uvw_fail) { rtapi_print_msg(RTAPI_MSG_ERR,"Failed to initialize Line9, err codes %d, %d, %d\n", xyz_fail,abc_fail,uvw_fail); @@ -729,7 +740,7 @@ int tcUpdateCircleAccRatio(TC_STRUCT * tc) if (tc->motion_type == TC_CIRCULAR) { PmCircleLimits limits = pmCircleActualMaxVel(&tc->coords.circle.xyz, tc->maxvel, - tcGetOverallMaxAccel(tc)); + tc->acc_normal_max * tcGetAccelScale(tc)); tc->maxvel = limits.v_max; tc->acc_ratio_tan = limits.acc_ratio; return 0; @@ -756,13 +767,24 @@ int tcFinalizeLength(TC_STRUCT * const tc) tp_debug_print("tc %d already finalized\n", tc->id); return TP_ERR_NO_ACTION; } - - tp_debug_print("Finalizing motion id %d, type %d\n", tc->id, tc->motion_type); +#ifdef TP_DEBUG + double maxvel_old = tc->maxvel; +#endif tcClampVelocityByLength(tc); tcUpdateCircleAccRatio(tc); +#ifdef TP_DEBUG + print_json5_log_start(tpFinalizeLength, Lookahead); + print_json5_int_("id", tc->id); + print_json5_string_("motion_type", tcMotionTypeAsString((tc_motion_type_t)tc->motion_type)); + print_json5_double(maxvel_old); + print_json5_double_("maxvel", tc->maxvel); + print_json5_double_("acc_ratio_tan", tc->acc_ratio_tan); + print_json5_log_end(); +#endif + tc->finalized = 1; return TP_ERR_OK; } @@ -778,7 +800,6 @@ int tcClampVelocityByLength(TC_STRUCT * const tc) //Reduce max velocity to match sample rate //Assume that cycle time is valid here double sample_maxvel = tc->target / tc->cycle_time; - tp_debug_print("sample_maxvel = %f\n",sample_maxvel); tc->maxvel = fmin(tc->maxvel, sample_maxvel); return TP_ERR_OK; } @@ -892,4 +913,50 @@ int tcClearFlags(TC_STRUCT * const tc) return TP_ERR_OK; } +// Helper functions to convert enums to pretty-print for debug output +const char *tcTermCondAsString(tc_term_cond_t c) +{ + switch (c) + { + case TC_TERM_COND_STOP: + return "EXACT_STOP"; + case TC_TERM_COND_EXACT: + return "EXACT_PATH"; + case TC_TERM_COND_PARABOLIC: + return "PARABOLIC"; + case TC_TERM_COND_TANGENT: + return "TANGENT"; + } + return "NONE"; +} + +const char *tcMotionTypeAsString(tc_motion_type_t c) +{ + switch (c) + { + case TC_LINEAR: + return "Linear"; + case TC_CIRCULAR: + return "Circular"; + case TC_RIGIDTAP: + return "RigidTap"; + case TC_SPHERICAL: + return "SphericalArc"; + } + return "NONE"; +} + +const char *tcSyncModeAsString(tc_spindle_sync_t c) +{ + switch (c) + { + case TC_SYNC_NONE: + return "sync_none"; + case TC_SYNC_VELOCITY: + return "sync_velocity"; + case TC_SYNC_POSITION: + return "sync_position"; + } + return "NONE"; +} diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index 2f108060016..fde8e983611 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -25,6 +25,7 @@ double tcGetMaxTargetVel(TC_STRUCT const * const tc, double max_scale); +double tcGetAccelScale(TC_STRUCT const * tc); double tcGetOverallMaxAccel(TC_STRUCT const * tc); double tcGetTangentialMaxAccel(TC_STRUCT const * const tc); @@ -45,7 +46,7 @@ int tcGetIntersectionPoint(TC_STRUCT const * const prev_tc, int tcCanConsume(TC_STRUCT const * const tc); -int tcSetTermCond(TC_STRUCT * prev_tc, TC_STRUCT * tc, int term_cond); +int tcSetTermCond(TC_STRUCT * tc, TC_STRUCT * nexttc, tc_term_cond_t term_cond); int tcConnectBlendArc(TC_STRUCT * const prev_tc, TC_STRUCT * const tc, PmCartesian const * const circ_start, @@ -63,7 +64,7 @@ int pmCircleTangentVector(PmCircle const * const circle, int tcFlagEarlyStop(TC_STRUCT * const tc, TC_STRUCT * const nexttc); -double pmLine9Target(PmLine9 * const line9); +double pmLine9Target(PmLine9 * const line9, int pure_angular); int pmLine9Init(PmLine9 * const line9, EmcPose const * const start, @@ -85,7 +86,7 @@ int pmRigidTapInit(PmRigidTap * const tap, double pmRigidTapTarget(PmRigidTap * const tap, double uu_per_rev); int tcInit(TC_STRUCT * const tc, - int motion_type, + tc_motion_type_t motion_type, int canon_motion_type, double cycle_time, unsigned char enables, @@ -111,4 +112,9 @@ int tcPureRotaryCheck(TC_STRUCT const * const tc); int tcSetCircleXYZ(TC_STRUCT * const tc, PmCircle const * const circ); int tcClearFlags(TC_STRUCT * const tc); + +const char *tcTermCondAsString(tc_term_cond_t c); +const char *tcMotionTypeAsString(tc_motion_type_t c); +const char *tcSyncModeAsString(tc_spindle_sync_t c); + #endif /* TC_H */ diff --git a/src/emc/tp/tc_types.h b/src/emc/tp/tc_types.h index 0d31fef9d26..d22881060bc 100644 --- a/src/emc/tp/tc_types.h +++ b/src/emc/tp/tc_types.h @@ -49,8 +49,10 @@ typedef enum { #define TC_OPTIM_UNTOUCHED 0 #define TC_OPTIM_AT_MAX 1 -#define TC_ACCEL_TRAPZ 0 -#define TC_ACCEL_RAMP 1 +typedef enum { + TC_ACCEL_TRAPZ, + TC_ACCEL_RAMP, +} TCAccelMode; /** * Spiral arc length approximation by quadratic fit. @@ -100,6 +102,12 @@ typedef struct { double aios[EMCMOT_MAX_AIO]; } syncdio_t; + +typedef struct { + double position; //!< Reference position for displacement calculations + double direction; //!< Direction of "positive" spindle motion (as -1, 1, or 0) +} spindle_origin_t; + typedef struct { PmCartLine xyz; // original, but elongated, move down PmCartLine aux_xyz; // this will be generated on the fly, for the other @@ -107,7 +115,6 @@ typedef struct { PmCartesian abc; PmCartesian uvw; double reversal_target; - double spindlerevs_at_reversal; RIGIDTAP_STATE state; } PmRigidTap; @@ -117,6 +124,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 @@ -132,6 +140,7 @@ typedef struct { //Acceleration double maxaccel; // accel calc'd by task double acc_ratio_tan;// ratio between normal and tangential accel + double acc_normal_max; // Max acceleration allowed in normal direction (worst-case for the whole curve) int id; // segment's serial number @@ -142,19 +151,19 @@ typedef struct { Arc9 arc; } coords; - int motion_type; // TC_LINEAR (coords.line) or + tc_motion_type_t motion_type; // TC_LINEAR (coords.line) or // TC_CIRCULAR (coords.circle) or // TC_RIGIDTAP (coords.rigidtap) int active; // this motion is being executed int canon_motion_type; // this motion is due to which canon function? - int term_cond; // gcode requests continuous feed at the end of + tc_term_cond_t term_cond; // gcode requests continuous feed at the end of // this segment (g64 mode) int blending_next; // segment is being blended into following segment double blend_vel; // velocity below which we should start blending double tolerance; // during the blend at the end of this move, // stay within this distance from the path. - int synchronized; // spindle sync state + tc_spindle_sync_t synchronized; // spindle sync state double uu_per_rev; // for sync, user units per rev (e.g. 0.0625 for 16tpi) double vel_at_blend_start; int sync_accel; // we're accelerating up to sync with the spindle diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index d7f4f76df7f..c31a1f88584 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -21,6 +21,10 @@ #include "motion_types.h" #include "spherical_arc.h" #include "blendmath.h" +#include "math_util.h" +#include "joint_util.h" +#include "string.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" @@ -66,8 +70,16 @@ STATIC double estimateParabolicBlendPerformance( STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc); +typedef enum { + UPDATE_NORMAL, + UPDATE_PARABOLIC_BLEND, + UPDATE_SPLIT +} UpdateCycleMode; + STATIC int tpUpdateCycle(TP_STRUCT * const tp, - TC_STRUCT * const tc, TC_STRUCT const * const nexttc); + TC_STRUCT * const tc, + TC_STRUCT const * const nexttc, + UpdateCycleMode cycle_mode); STATIC int tpRunOptimization(TP_STRUCT * const tp); @@ -75,6 +87,40 @@ STATIC inline int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT const * const tc); + +STATIC int tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc); + +typedef struct { + char axes[9]; +} AxisMaskString; + +static inline AxisMaskString axisBitMaskToString(unsigned failed_axes) +{ + AxisMaskString axis_str = {{"XYZABCUVW"}}; + int i; + for (i = 0; i < 9;++i) { + if (failed_axes & (1 << i)) { + continue; + } + axis_str.axes[i] =' '; + } + return axis_str; +} + + +static inline void reportTPAxisError(TP_STRUCT const *tp, unsigned failed_axes, const char *msg_prefix) +{ + if (failed_axes) + { + AxisMaskString failed_axes_str = axisBitMaskToString(failed_axes); + rtapi_print_msg(RTAPI_MSG_ERR, "%s, axes [%s] at %g sec\n", + msg_prefix ?: "unknown error", + failed_axes_str.axes, + tp->time_elapsed_sec); + } +} + + /** * @section tpcheck Internal state check functions. * These functions compartmentalize some of the messy state checks. @@ -103,10 +149,8 @@ STATIC int tcRotaryMotionCheck(TC_STRUCT const * const tc) { } case TC_SPHERICAL: return true; - default: - tp_debug_print("Unknown motion type!\n"); - return false; } + return false; } @@ -130,51 +174,6 @@ STATIC double tpGetTangentKinkRatio(void) { } -STATIC int tpGetMachineAccelBounds(PmCartesian * const acc_bound) { - if (!acc_bound) { - return TP_ERR_FAIL; - } - - acc_bound->x = emcmotDebug->joints[0].acc_limit; - acc_bound->y = emcmotDebug->joints[1].acc_limit; - acc_bound->z = emcmotDebug->joints[2].acc_limit; - return TP_ERR_OK; -} - - -STATIC int tpGetMachineVelBounds(PmCartesian * const vel_bound) { - if (!vel_bound) { - return TP_ERR_FAIL; - } - - vel_bound->x = emcmotDebug->joints[0].vel_limit; - vel_bound->y = emcmotDebug->joints[1].vel_limit; - vel_bound->z = emcmotDebug->joints[2].vel_limit; - return TP_ERR_OK; -} - -STATIC int tpGetMachineActiveLimit(double * const act_limit, PmCartesian const * const bounds) { - if (!act_limit) { - return TP_ERR_FAIL; - } - //Start with max accel value - *act_limit = fmax(fmax(bounds->x,bounds->y),bounds->z); - - // Compare only with active axes - if (bounds->x > 0) { - *act_limit = fmin(*act_limit, bounds->x); - } - if (bounds->y > 0) { - *act_limit = fmin(*act_limit, bounds->y); - } - if (bounds->z > 0) { - *act_limit = fmin(*act_limit, bounds->z); - } - tp_debug_print(" arc blending a_max=%f\n", *act_limit); - return TP_ERR_OK; -} - - /** * Get a segment's feed scale based on the current planner state and emcmotStatus. * @note depends on emcmotStatus for system information. @@ -221,7 +220,6 @@ 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 @@ -232,6 +230,17 @@ STATIC inline double getMaxFeedScale(TC_STRUCT const * tc) } } +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. @@ -239,7 +248,7 @@ STATIC inline double getMaxFeedScale(TC_STRUCT const * tc) */ 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); @@ -283,22 +292,43 @@ STATIC inline double tpGetRealFinalVel(TP_STRUCT const * const tp, v_target_next = tpGetRealTargetVel(tp, nexttc); } - tc_debug_print("v_target_next = %f\n",v_target_next); // Limit final velocity to minimum of this and next target velocities double v_target = fmin(v_target_this, v_target_next); double finalvel = fmin(tc->finalvel, v_target); return finalvel; } +/** + * Set up a spindle origin based on the current spindle COMMANDED direction and the given position. + * + * Convert the 2-part spindle position and sign to a signed double. + * Get acceleration for a tc based on the trajectory planner state. + * changes in displacement due to sign flips + */ +STATIC inline void resetSpindleOrigin(spindle_origin_t *origin) +{ + if (!origin) { + return; + } + origin->direction = 0.0; + origin->position = 0.0; +} + /** - * Convert the 2-part spindle position and sign to a signed double. + * Set up a spindle origin based on the current spindle COMMANDED direction and the given position. + * + * The origin is used to calculate displacements used in spindle position tracking. + * The direction is stored as part of the origin to prevent discontinuous + * changes in displacement due to sign flips */ -STATIC inline double tpGetSignedSpindlePosition(double spindle_pos, int spindle_dir) { - if (spindle_dir < 0.0) { - spindle_pos*=-1.0; +STATIC inline void setSpindleOrigin(spindle_origin_t *origin, double position) +{ + if (!origin) { + return; } - return spindle_pos; + origin->position = position; + origin->direction = signum(emcmotStatus->spindle_cmd.velocity_rpm_out); } /** @@ -364,6 +394,9 @@ int tpClear(TP_STRUCT * const tp) tcqInit(&tp->queue); tp->queueSize = 0; tp->goalPos = tp->currentPos; + ZERO_EMC_POSE(tp->currentVel); + tp->time_elapsed_sec = 0.0; + tp->time_elapsed_ticks = 0; tp->nextId = 0; tp->execId = 0; tp->motionType = 0; @@ -375,10 +408,11 @@ 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; + emcmotStatus->pos_tracking_error = 0; ZERO_EMC_POSE(emcmotStatus->dtg); SET_MOTION_INPOS_FLAG(1); @@ -396,26 +430,19 @@ int tpInit(TP_STRUCT * const tp) //Velocity limits tp->vLimit = 0.0; tp->ini_maxvel = 0.0; - //Accelerations - tp->aLimit = 0.0; - PmCartesian acc_bound; - //FIXME this acceleration bound isn't valid (nor is it used) - tpGetMachineAccelBounds(&acc_bound); - tpGetMachineActiveLimit(&tp->aMax, &acc_bound); //Angular limits tp->wMax = 0.0; tp->wDotMax = 0.0; - tp->spindle.offset = 0.0; - tp->spindle.revs = 0.0; + resetSpindleOrigin(&tp->spindle.origin); tp->spindle.waiting_for_index = MOTION_INVALID_ID; tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID; ZERO_EMC_POSE(tp->currentPos); + ZERO_EMC_POSE(tp->currentVel); - PmCartesian vel_bound; - tpGetMachineVelBounds(&vel_bound); - tpGetMachineActiveLimit(&tp->vMax, &vel_bound); + PmCartesian vel_bound = getXYZVelBounds(); + tp->vMax = findMinNonZero(&vel_bound); return tpClear(tp); } @@ -472,18 +499,6 @@ int tpSetVlimit(TP_STRUCT * const tp, double vLimit) return TP_ERR_OK; } -/** Sets the max acceleration for the trajectory planner. */ -int tpSetAmax(TP_STRUCT * const tp, double aMax) -{ - if (0 == tp || aMax <= 0.0) { - return TP_ERR_FAIL; - } - - tp->aMax = aMax; - - return TP_ERR_OK; -} - /** * Sets the id that will be used for the next appended motions. * nextId is incremented so that the next time a motion is appended its id will @@ -525,11 +540,9 @@ int tpGetExecId(TP_STRUCT * const tp) * begins. If cond is TC_TERM_COND_PARABOLIC, the following move is begun when the * current move slows below a calculated blend velocity. */ -int tpSetTermCond(TP_STRUCT * const tp, int cond, double tolerance) +int tpSetTermCond(TP_STRUCT * const tp, tc_term_cond_t cond, double tolerance) { - if (!tp) { - return TP_ERR_FAIL; - } + tp_err_t res = TP_ERR_FAIL; switch (cond) { //Purposeful waterfall for now @@ -537,15 +550,15 @@ int tpSetTermCond(TP_STRUCT * const tp, int cond, double tolerance) case TC_TERM_COND_TANGENT: case TC_TERM_COND_EXACT: case TC_TERM_COND_STOP: + if (tp) { tp->termCond = cond; tp->tolerance = tolerance; + res = TP_ERR_OK; + } break; - default: - //Invalid condition - return -1; } - return TP_ERR_OK; + return res; } /** @@ -663,10 +676,14 @@ STATIC double tpCalculateOptimizationInitialVel(TP_STRUCT const * const tp, TC_S double acc_scaled = tcGetTangentialMaxAccel(tc); double triangle_vel = findVPeak(acc_scaled, tc->target); double max_vel = tpGetMaxTargetVel(tp, tc); - tp_debug_json_start(tpCalculateOptimizationInitialVel); - tp_debug_json_double(triangle_vel); - tp_debug_json_end(); - return fmin(triangle_vel, max_vel); + double optim_init_vel = fmin(triangle_vel, max_vel); +#ifdef TP_DEBUG + print_json5_log_start(tpCalculateOptimizationInitialVel, Command); + print_json5_double(triangle_vel); + print_json5_double(optim_init_vel); + print_json5_log_end(); +#endif + return optim_init_vel; } @@ -698,7 +715,8 @@ STATIC int tpInitBlendArcFromPrev(TP_STRUCT const * const tp, // Copy over state data from TP tcSetupState(blend_tc, tp); - + blend_tc->term_cond = TC_TERM_COND_TANGENT; + // Set kinematics parameters from blend calculations tcSetupMotion(blend_tc, vel, @@ -715,9 +733,6 @@ STATIC int tpInitBlendArcFromPrev(TP_STRUCT const * const tp, blend_tc->target = length; blend_tc->nominal_length = length; - // Set the blend arc to be tangent to the next segment - tcSetTermCond(blend_tc, NULL, TC_TERM_COND_TANGENT); - //NOTE: blend arc radius and everything else is finalized, so set this to 1. //In the future, radius may be adjustable. tcFinalizeLength(blend_tc); @@ -742,7 +757,6 @@ STATIC int tcSetLineXYZ(TC_STRUCT * const tc, PmCartLine const * const line) return TP_ERR_OK; } - static inline int find_max_element(double arr[], int sz) { if (sz < 1) { @@ -759,6 +773,21 @@ static inline int find_max_element(double arr[], int sz) return max_idx; } +const char *blendTypeAsString(tc_blend_type_t c) +{ + switch(c) { + case NO_BLEND: + return "NO_BLEND"; + case PARABOLIC_BLEND: + return "PARABOLIC_BLEND"; + case TANGENT_SEGMENTS_BLEND: + return "TANGENT_SEGMENTS"; + case ARC_BLEND: + return "ARC_BLEND"; + } + return ""; +} + /** * Compare performance of blend arc and equivalent tangent speed. * If we can go faster by assuming the segments are already tangent (and @@ -775,12 +804,15 @@ STATIC tc_blend_type_t tpChooseBestBlend(TP_STRUCT const * const tp, return NO_BLEND; } - // Can't blend segments that are explicitly disallowed switch (prev_tc->term_cond) { case TC_TERM_COND_EXACT: case TC_TERM_COND_STOP: + // Can't blend segments that are explicitly disallowed return NO_BLEND; + case TC_TERM_COND_PARABOLIC: + case TC_TERM_COND_TANGENT: + break; } // Compute performance measures ("perf_xxx") for each method. This is @@ -792,34 +824,37 @@ STATIC tc_blend_type_t tpChooseBestBlend(TP_STRUCT const * const tp, double perf_tangent = prev_tc->kink_vel; double perf_arc_blend = blend_tc ? blend_tc->maxvel : 0.0; - tp_debug_print("Blend performance: parabolic %f, tangent %f, arc_blend %f, ", - perf_parabolic, - perf_tangent, - perf_arc_blend); - // KLUDGE Order the performance measurements so that they match the enum values double perf[3] = {perf_parabolic, perf_tangent, perf_arc_blend}; tc_blend_type_t best_blend = find_max_element(perf, 3); switch (best_blend) { case PARABOLIC_BLEND: // parabolic - tp_debug_print("using parabolic blend\n"); tcRemoveKinkProperties(prev_tc, tc); tcSetTermCond(prev_tc, tc, TC_TERM_COND_PARABOLIC); break; case TANGENT_SEGMENTS_BLEND: // tangent - tp_debug_print("using approximate tangent blend\n"); // NOTE: acceleration / velocity reduction is done dynamically in functions that access TC_STRUCT properties tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); break; case ARC_BLEND: // arc blend - tp_debug_print("using blend arc\n"); tcRemoveKinkProperties(prev_tc, tc); - break; case NO_BLEND: + tcRemoveKinkProperties(prev_tc, tc); + tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP); break; } + +#ifdef TP_DEBUG + print_json5_log_start(CheckBlendPerformance,Command); + print_json5_double(perf_parabolic); + print_json5_double(perf_tangent); + print_json5_double(perf_arc_blend); + print_json5_string_("best_blend", blendTypeAsString(best_blend)); + print_json5_log_end(); +#endif + return best_blend; } @@ -828,12 +863,9 @@ STATIC tp_err_t tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const pre { tp_debug_print("-- Starting LineArc blend arc --\n"); - PmCartesian acc_bound, vel_bound; + PmCartesian vel_bound = getXYZVelBounds(); + PmCartesian acc_bound = getXYZAccelBounds(); - //Get machine limits - tpGetMachineAccelBounds(&acc_bound); - tpGetMachineVelBounds(&vel_bound); - //Populate blend geometry struct BlendGeom3 geom; BlendParameters param; @@ -845,7 +877,7 @@ STATIC tp_err_t tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const pre 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", @@ -984,11 +1016,8 @@ STATIC tp_err_t tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const pre { tp_debug_print("-- Starting ArcLine blend arc --\n"); - PmCartesian acc_bound, vel_bound; - - //Get machine limits - tpGetMachineAccelBounds(&acc_bound); - tpGetMachineVelBounds(&vel_bound); + PmCartesian vel_bound = getXYZVelBounds(); + PmCartesian acc_bound = getXYZAccelBounds(); //Populate blend geometry struct BlendGeom3 geom; @@ -1002,7 +1031,7 @@ STATIC tp_err_t tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const pre 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); @@ -1132,11 +1161,8 @@ STATIC tp_err_t tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev return TP_ERR_FAIL; } - PmCartesian acc_bound, vel_bound; - - //Get machine limits - tpGetMachineAccelBounds(&acc_bound); - tpGetMachineVelBounds(&vel_bound); + PmCartesian vel_bound = getXYZVelBounds(); + PmCartesian acc_bound = getXYZAccelBounds(); //Populate blend geometry struct BlendGeom3 geom; @@ -1149,7 +1175,7 @@ STATIC tp_err_t tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev 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", @@ -1172,8 +1198,6 @@ STATIC tp_err_t tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev return TP_ERR_FAIL; } - - int res_param = blendComputeParameters(¶m); int res_points = blendFindPoints3(&points_approx, &geom, ¶m); @@ -1290,12 +1314,10 @@ STATIC tp_err_t tpCreateLineLineBlend(TP_STRUCT * const tp, TC_STRUCT * const pr { tp_debug_print("-- Starting LineLine blend arc --\n"); - PmCartesian acc_bound, vel_bound; - - //Get machine limits - tpGetMachineAccelBounds(&acc_bound); - tpGetMachineVelBounds(&vel_bound); - + + PmCartesian vel_bound = getXYZVelBounds(); + PmCartesian acc_bound = getXYZAccelBounds(); + // Setup blend data structures BlendGeom3 geom; BlendParameters param; @@ -1306,7 +1328,7 @@ STATIC tp_err_t tpCreateLineLineBlend(TP_STRUCT * const tp, TC_STRUCT * const pr 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", @@ -1391,16 +1413,47 @@ STATIC inline int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc tp->done = 0; tp->depth = tcqLen(&tp->queue); //Fixing issue with duplicate id's? - tp_debug_print("Adding TC id %d of type %d, total length %0.08f\n",tc->id,tc->motion_type,tc->target); +#ifdef TP_DEBUG + print_json5_log_start(Enqueue, Command); + print_json5_long_("id", tc->id); + print_json5_string_("motion_type", tcMotionTypeAsString(tc->motion_type)); + print_json5_double_("target", tc->target); + print_json5_string_("term_cond", tcTermCondAsString(tc->term_cond)); + print_json5_end_(); +#endif return TP_ERR_OK; } -STATIC int handleModeChange(TC_STRUCT * const prev_tc, TC_STRUCT * const tc) +int handlePrevTermCondition(TC_STRUCT *prev_tc, TC_STRUCT *tc) +{ + if (!tc) { + return TP_ERR_FAIL; + } + + switch (tc->motion_type) { + case TC_RIGIDTAP: + tcSetTermCond(tc, NULL, TC_TERM_COND_EXACT); + tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP); + break; + case TC_LINEAR: + case TC_CIRCULAR: + case TC_SPHERICAL: + { + tc_term_cond_t prev_term = prev_tc ? prev_tc->term_cond : TC_TERM_COND_STOP; + tcSetTermCond(prev_tc, tc, prev_term); + } + break; + } + return TP_ERR_OK; +} + +STATIC int handleModeChange(TC_STRUCT *prev_tc, TC_STRUCT *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("Blending disabled: can't blend between rapid and feed motions\n"); @@ -1408,9 +1461,7 @@ STATIC int handleModeChange(TC_STRUCT * const prev_tc, TC_STRUCT * const tc) } 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); + tp_debug_print("Blending disabled: entering position-sync mode\n"); tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP); } return TP_ERR_OK; @@ -1429,12 +1480,61 @@ 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: + // No update necessary + 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); + + // Make sure the blending flags are consistent w/ previous segment + handlePrevTermCondition(prev_tc, tc); + + // Prevent blends for specific mode changes (where blending isn't possible anyway) + 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 + 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. */ -int tpAddRigidTap(TP_STRUCT * const tp, EmcPose end, double vel, double ini_maxvel, - double acc, unsigned char enables) { +int tpAddRigidTap(TP_STRUCT * const tp, + EmcPose end, + double vel, + double ini_maxvel, + double acc, + unsigned char enables) { if (tpErrorCheck(tp)) { return TP_ERR_FAIL; } @@ -1477,17 +1577,7 @@ int tpAddRigidTap(TP_STRUCT * const tp, EmcPose end, double vel, double ini_maxv &end); tc.target = pmRigidTapTarget(&tc.coords.rigidtap, tp->uu_per_rev); - // Force exact stop mode after rigid tapping regardless of TP setting - tcSetTermCond(&tc, NULL, 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( @@ -1747,10 +1837,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 = tcGetMaxTargetVel(prev_tc, getMaxFeedScale(prev_tc)); - double v_max2 = tcGetMaxTargetVel(tc, getMaxFeedScale(tc)); - // Note that this is a minimum since the velocity at the intersection must - // be the slower of the two segments not to violate constraints. + 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); @@ -1766,9 +1854,7 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp, pmCartScalMult(&this_tan, a_inst, &acc2); pmCartCartSub(&acc2,&acc1,&acc_diff); - //TODO store this in TP struct instead? - PmCartesian acc_bound; - tpGetMachineAccelBounds(&acc_bound); + PmCartesian acc_bound = getXYZAccelBounds(); PmCartesian acc_scale; findAccelScale(&acc_diff,&acc_bound,&acc_scale); @@ -1831,9 +1917,6 @@ static bool tpCreateBlendIfPossible( res_create = tpCreateArcArcBlend(tp, prev_tc, tc, blend_tc); break; case BLEND_NONE: - default: - tp_debug_print("intersection type not recognized, aborting arc\n"); - res_create = TP_ERR_FAIL; break; } @@ -1907,13 +1990,41 @@ STATIC tc_blend_type_t tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const * end of the previous move to the new end specified here at the * currently-active accel and vel settings from the tp struct. */ -int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, double vel, double - ini_maxvel, double acc, unsigned char enables, char atspeed, int indexrotary) { +int tpAddLine(TP_STRUCT * const tp, + EmcPose end, + int canon_motion_type, + double vel, + double ini_maxvel, + double acc, + unsigned char enables, + char atspeed, + int indexrotary, + int pure_angular) +{ if (tpErrorCheck(tp) < 0) { return TP_ERR_FAIL; } - tp_info_print("== AddLine ==\n"); +#ifdef TP_DEBUG + { + // macros use the variable name, need a plain name to please the JSON5 parser + print_json5_log_start(tpAddLine, Command); + print_json5_long_("id", tp->nextId); + print_json5_EmcPose_("goal", tp->goalPos); + print_json5_EmcPose(end); + print_json5_double(vel); + print_json5_double(ini_maxvel); + print_json5_double(acc); + print_json5_unsigned(enables); + print_json5_long(indexrotary); + print_json5_long(atspeed); + print_json5_long(canon_motion_type); + EmcPose delta = tp->goalPos; + emcPoseSub(&end, &tp->goalPos, &delta); + print_json5_EmcPose(delta); + print_json5_log_end(); + } +#endif // Initialize new tc struct for the line segment TC_STRUCT tc = {0}; @@ -1929,6 +2040,7 @@ int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, double v // Copy over state data from the trajectory planner tcSetupState(&tc, tp); + tcSetTermCond(&tc, NULL, tp->termCond); // Copy in motion parameters tcSetupMotion(&tc, @@ -1939,7 +2051,7 @@ int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, double v pmLine9Init(&tc.coords.line, &tp->goalPos, &end); - tc.target = pmLine9Target(&tc.coords.line); + tc.target = pmLine9Target(&tc.coords.line, pure_angular); if (tc.target < TP_POS_EPSILON) { rtapi_print_msg(RTAPI_MSG_DBG,"failed to create line id %d, zero-length segment\n",tp->nextId); return TP_ERR_ZERO_LENGTH; @@ -1950,21 +2062,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); - handleModeChange(prev_tc, &tc); - if (emcmotConfig->arcBlendEnable){ - tpHandleBlendArc(tp, &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); } @@ -1987,6 +2085,7 @@ int tpAddCircle(TP_STRUCT * const tp, double vel, double ini_maxvel, double acc, + double acc_normal, unsigned char enables, char atspeed) { @@ -1994,8 +2093,30 @@ int tpAddCircle(TP_STRUCT * const tp, return TP_ERR_FAIL; } - tp_info_print("== AddCircle ==\n"); - tp_debug_print("ini_maxvel = %f\n",ini_maxvel); +#ifdef TP_DEBUG + { + // macros use the variable name, need a plain name to please the JSON5 parser + long nextId = tp->nextId; + EmcPose goal = tp->goalPos; + print_json5_log_start(tpAddCircle, Command); + print_json5_long(nextId); + print_json5_EmcPose(goal); + print_json5_PmCartesian(center); + print_json5_PmCartesian(normal); + print_json5_long(turn); + print_json5_double(vel); + print_json5_double(ini_maxvel); + print_json5_double(acc); + print_json5_double(acc_normal); + print_json5_unsigned(enables); + print_json5_long(atspeed); + print_json5_long(canon_motion_type); + EmcPose delta = tp->goalPos; + emcPoseSub(&end, &tp->goalPos, &delta); + print_json5_EmcPose(delta); + print_json5_log_end(); + } +#endif TC_STRUCT tc = {0}; @@ -2010,6 +2131,7 @@ int tpAddCircle(TP_STRUCT * const tp, // Copy over state data from the trajectory planner tcSetupState(&tc, tp); + tcSetTermCond(&tc, NULL, tp->termCond); // Setup circle geometry int res_init = pmCircle9Init(&tc.coords.circle, @@ -2021,12 +2143,13 @@ int tpAddCircle(TP_STRUCT * const tp, if (res_init) return res_init; + tc.acc_normal_max = acc_normal; + // Update tc target with existing circular segment tc.target = pmCircle9Target(&tc.coords.circle); if (tc.target < TP_POS_EPSILON) { return TP_ERR_ZERO_LENGTH; } - tp_debug_print("tc.target = %f\n",tc.target); tc.nominal_length = tc.target; // Copy in motion parameters @@ -2038,21 +2161,7 @@ int tpAddCircle(TP_STRUCT * const tp, //Reduce max velocity to match sample rate tcClampVelocityByLength(&tc); - TC_STRUCT *prev_tc; - prev_tc = tcqLast(&tp->queue); - - handleModeChange(prev_tc, &tc); - if (emcmotConfig->arcBlendEnable){ - tpHandleBlendArc(tp, &tc); - findSpiralArcLengthFit(&tc.coords.circle.xyz, &tc.coords.circle.fit); - } - tcFinalizeLength(prev_tc); - tcFlagEarlyStop(prev_tc, &tc); - - int retval = tpAddSegmentToQueue(tp, &tc, true); - - tpRunOptimization(tp); - return retval; + return tpFinalizeAndEnqueue(tp, &tc); } @@ -2108,10 +2217,15 @@ STATIC int tpComputeBlendVelocity( double theta; - PmCartesian v1, v2; + PmCartesian v1={0}, v2={0}; + + int res1 = tcGetEndAccelUnitVector(tc, &v1); + int res2 = tcGetStartAccelUnitVector(nexttc, &v2); - tcGetEndAccelUnitVector(tc, &v1); - tcGetStartAccelUnitVector(nexttc, &v2); + if (res1 || res2) { + rtapi_print_msg(RTAPI_MSG_WARN, "Got bad unit vectors in parabolic blend velocity calculation\n"); + return TP_ERR_FAIL; + } findIntersectionAngle(&v1, &v2, &theta); double cos_theta = cos(theta); @@ -2205,25 +2319,49 @@ STATIC int tcUpdateDistFromAccel(TC_STRUCT * const tc, double acc, double vel_de return TP_ERR_OK; } -STATIC void tpDebugCycleInfo(TP_STRUCT const * const tp, TC_STRUCT const * const tc, TC_STRUCT const * const nexttc, double acc) { +const char *cycleModeToString(UpdateCycleMode mode) +{ + switch (mode) { + case UPDATE_NORMAL: + return "normal"; + case UPDATE_PARABOLIC_BLEND: + return "parabolic_blend"; + case UPDATE_SPLIT: + return "split_cycle"; + } + return "unknown"; +} + +STATIC void tpDebugCycleInfo(TP_STRUCT const * const tp, TC_STRUCT const * const tc, TC_STRUCT const * const nexttc, double acc, int accel_mode, UpdateCycleMode cycle) { #ifdef TC_DEBUG // Find maximum allowed velocity from feed and machine limits - double tc_target_vel = tpGetRealTargetVel(tp, tc); + const double v_target = tpGetRealTargetVel(tp, tc); // Store a copy of final velocity - double tc_finalvel = tpGetRealFinalVel(tp, tc, nexttc); + const double v_final = tpGetRealFinalVel(tp, tc, nexttc); + const double v_max = tpGetMaxTargetVel(tp, tc); /* Debug Output */ - tc_debug_print("tc state: vr = %f, vf = %f, maxvel = %f\n", - tc_target_vel, tc_finalvel, tc->maxvel); - tc_debug_print(" currentvel = %f, fs = %f, tc = %f, term = %d\n", - tc->currentvel, tpGetFeedScale(tp,tc), tc->cycle_time, tc->term_cond); - tc_debug_print(" acc = %f,T = %f, P = %f\n", acc, - tc->target, tc->progress); - tc_debug_print(" motion type %d\n", tc->motion_type); - - if (tc->on_final_decel) { - rtapi_print(" on final decel\n"); - } + print_json5_log_start(tc_state, Run); + print_json5_ll_("time_ticks", tp->time_elapsed_ticks); + print_json5_double_("id", tc->id); + print_json5_double(v_target); + print_json5_double(v_final); + print_json5_double(v_max); + print_json5_double_("target", tc->target); + print_json5_double_("progress", tc->progress); + print_json5_double_("v_current", tc->currentvel); + print_json5_double_("a_current", acc); + print_json5_double_("feed_scale", tpGetFeedScale(tp, tc)); + print_json5_double_("dt", tc->cycle_time); + print_json5_string_("term_cond", tcTermCondAsString((tc_term_cond_t)tc->term_cond)); + print_json5_bool_("final_decel", tc->on_final_decel); + print_json5_bool_("need_split", tc->splitting); + print_json5_bool_("is_blending", tc->is_blending); + print_json5_long_("canon_type", tc->canon_motion_type); + print_json5_string_("motion_type", tcMotionTypeAsString(tc->motion_type)); + print_json5_string_("accel_mode",accel_mode ? "ramp" : "trapezoidal"); + print_json5_string_("cycle", cycleModeToString(cycle)); + print_json5_end_(); #endif } @@ -2239,8 +2377,6 @@ STATIC void tpDebugCycleInfo(TP_STRUCT const * const tp, TC_STRUCT const * const void tpCalculateTrapezoidalAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc, double * const acc, double * const vel_desired) { - tc_debug_print("using trapezoidal acceleration\n"); - // Find maximum allowed velocity from feed and machine limits double tc_target_vel = tpGetRealTargetVel(tp, tc); // Store a copy of final velocity @@ -2257,30 +2393,8 @@ void tpCalculateTrapezoidalAccel(TP_STRUCT const * const tp, TC_STRUCT * const t double dx = tc->target - tc->progress; double maxaccel = tcGetTangentialMaxAccel(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 @@ -2302,7 +2416,6 @@ STATIC int tpCalculateRampAccel(TP_STRUCT const * const tp, double * const acc, double * const vel_desired) { - tc_debug_print("using ramped acceleration\n"); // displacement remaining in this segment double dx = tc->target - tc->progress; @@ -2358,7 +2471,37 @@ void tpToggleDIOs(TC_STRUCT * const tc) { tc->syncdio.anychanged = 0; //we have turned them all on/off, nothing else to do for this TC the next time } } +/** + * Compute the spindle displacement used for spindle position tracking. + * + * The displacement is always computed with respect to a specie + */ +static inline double findSpindleDisplacement( + double new_pos, + spindle_origin_t origin + ) +{ + return origin.direction * (new_pos - origin.position); +} + +/** + * 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 spindleReversed(spindle_origin_t origin, double prev_pos, double current_pos) +{ + return origin.direction * (current_pos - prev_pos) < 0; +} +static inline bool cmdReverseSpindle(void) +{ + 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. @@ -2366,33 +2509,31 @@ void tpToggleDIOs(TC_STRUCT * const tc) { * during a rigid tap cycle. In particular, the target and spindle goal need to * be carefully handled since we're reversing direction. */ -STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, +STATIC void tpUpdateRigidTapState(TP_STRUCT * const tp, TC_STRUCT * const tc) { - static double old_spindlepos; - double new_spindlepos = emcmotStatus->spindleRevs; - if (emcmotStatus->spindle.direction < 0) new_spindlepos = -new_spindlepos; + static double old_spindle_pos = 0.0; + double spindle_pos = emcmotStatus->spindle_fb.position_rev; - switch (tc->coords.rigidtap.state) { + RIGIDTAP_STATE const initial_state = tc->coords.rigidtap.state; + switch (initial_state) { case RIGIDTAP_START: old_spindlepos = new_spindlepos; tc->coords.rigidtap.state = TAPPING; // Deliberate fallthrough 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 (spindleReversed(tp->spindle.origin, old_spindle_pos, spindle_pos) && tc->currentvel <= 0.0) { 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; + setSpindleOrigin(&tp->spindle.origin, spindle_pos); pmCartLinePoint(&tc->coords.rigidtap.xyz, tc->progress, &start); end = tc->coords.rigidtap.xyz.start; @@ -2400,26 +2541,28 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, rtapi_print_msg(RTAPI_MSG_DBG, "old target = %f", tc->target); tc->coords.rigidtap.reversal_target = aux->tmag; tc->target = aux->tmag + 10. * tc->uu_per_rev; + // NOTE: reset both progress and sync location: + // At the point of reversal, the spindle is already synchronized tc->progress = 0.0; + tc->progress_at_sync = 0.0; rtapi_print_msg(RTAPI_MSG_DBG, "new target = %f", tc->target); tc->coords.rigidtap.state = RETRACTION; } - old_spindlepos = new_spindlepos; - tc_debug_print("Spindlepos = %f\n", new_spindlepos); break; case RETRACTION: - tc_debug_print("RETRACTION\n"); if (tc->progress >= tc->coords.rigidtap.reversal_target) { - emcmotStatus->spindle.speed *= -1; + // Flip spindle direction again 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 (spindleReversed(tp->spindle.origin, old_spindle_pos, spindle_pos) && tc->currentvel <= 0.0) { PmCartesian start, end; PmCartLine *aux = &tc->coords.rigidtap.aux_xyz; + + setSpindleOrigin(&tp->spindle.origin, spindle_pos); pmCartLinePoint(aux, tc->progress, &start); end = tc->coords.rigidtap.xyz.start; pmCartLineInit(aux, &start, &end); @@ -2431,13 +2574,18 @@ 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"); // this is a regular move now, it'll stop at target above. break; } + old_spindle_pos = spindle_pos; +#ifdef TC_DEBUG + RIGIDTAP_STATE current_state = tc->coords.rigidtap.state; + print_json5_log_start(tpUpdateRigidTapState, Run); + print_json5_unsigned(current_state); + print_json5_log_end(); +#endif } @@ -2471,8 +2619,6 @@ STATIC int tpUpdateMovementStatus(TP_STRUCT * const tp, TC_STRUCT const * const EmcPose tc_pos; tcGetEndpoint(tc, &tc_pos); - tc_debug_print("tc id = %u canon_type = %u mot type = %u\n", - tc->id, tc->canon_motion_type, tc->motion_type); tp->motionType = tc->canon_motion_type; tp->activeDepth = tc->active_depth; emcmotStatus->distance_to_go = tc->target - tc->progress; @@ -2514,7 +2660,7 @@ STATIC void tpUpdateBlend(TP_STRUCT * const tp, TC_STRUCT * const tc, nexttc->target_vel = 0.0; } - tpUpdateCycle(tp, nexttc, NULL); + tpUpdateCycle(tp, nexttc, NULL, UPDATE_PARABOLIC_BLEND); //Restore the original target velocity nexttc->target_vel = save_vel; } @@ -2569,9 +2715,9 @@ 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.origin.position += (tc->target - tc->progress_at_sync) / tc->uu_per_rev; } else { - tp->spindle.offset = 0.0; + tp->spindle.origin.position = 0.0; } if(tc->indexrotary != -1) { @@ -2618,7 +2764,7 @@ STATIC tp_err_t 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 @@ -2663,35 +2809,80 @@ STATIC tp_err_t 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; } } return TP_ERR_OK; } + +void checkPositionMatch(TP_STRUCT const *tp, TC_STRUCT const *tc) +{ + if (!emcmotConfig->consistencyCheckConfig.extraConsistencyChecks || tc->is_blending){ + return; + } + + EmcPose tp_position_error; + ZERO_EMC_POSE(tp_position_error); + tcGetPos(tc, &tp_position_error); + emcPoseSelfSub(&tp_position_error, &tp->currentPos); + + unsigned position_mismatch_axes = findAbsThresholdViolations(tp_position_error, emcmotConfig->consistencyCheckConfig.maxPositionDriftError); + reportTPAxisError(tp, position_mismatch_axes, "Motion start position difference exceeds threshold"); + +#ifdef TP_DEBUG + int need_print = true; +#else + int need_print = position_mismatch_axes != 0; +#endif + + // Log a bunch of TP internal state if required by debug level or position error + if (need_print) { + print_json5_log_start(ActivateSegment,Run); + print_json5_ll_("time_ticks", tp->time_elapsed_ticks); + print_json5_int_("id", tc->id); + // Position settings + print_json5_double_("target", tc->target); + print_json5_double_("progress", tc->progress); + // Velocity settings + print_json5_double_("reqvel", tc->reqvel); + print_json5_double_("target_vel", tc->target_vel); + print_json5_double_("finalvel", tc->finalvel); + // Acceleration settings + print_json5_double_("accel_scale", tcGetAccelScale(tc)); + print_json5_double_("acc_overall", tcGetOverallMaxAccel(tc)); + print_json5_double_("acc_tangential", tcGetTangentialMaxAccel(tc)); + print_json5_bool_("accel_ramp", tc->accel_mode); + print_json5_bool_("blend_prev", tc->blend_prev); + print_json5_string_("sync_mode", tcSyncModeAsString(tc->synchronized)); + print_json5_log_end(); + } +} + + /** * "Activate" a segment being read for the first time. * This function handles initial setup of a new segment read off of the queue * for the first time. */ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { - //Check if already active if (!tc || tc->active) { return TP_ERR_OK; } +#ifdef TP_PEDANTIC if (!tp) { return TP_ERR_MISSING_INPUT; } +#endif /* Based on the INI setting for "cutoff frequency", this calculation finds * short segments that can have their acceleration be simple ramps, instead @@ -2704,20 +2895,17 @@ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { // Given what velocities we can actually reach, estimate the total time for the segment under ramp conditions double segment_time = 2.0 * length / (tc->currentvel + fmin(tc->finalvel,tpGetRealTargetVel(tp,tc))); - if (segment_time < cutoff_time && tc->canon_motion_type != EMC_MOTION_TYPE_TRAVERSE && tc->term_cond == TC_TERM_COND_TANGENT && tc->motion_type != TC_RIGIDTAP) { - tp_debug_print("segment_time = %f, cutoff_time = %f, ramping\n", - segment_time, cutoff_time); tc->accel_mode = TC_ACCEL_RAMP; } // 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; @@ -2733,32 +2921,29 @@ STATIC tp_err_t 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", - tc->id, - tc->target_vel, - tc->reqvel, - tc->finalvel, - tc->target); - tc->active = 1; //Do not change initial velocity here, since tangent blending already sets this up tp->motionType = tc->canon_motion_type; tc->blending_next = 0; tc->on_final_decel = 0; - if (TC_SYNC_POSITION == tc->synchronized && !(emcmotStatus->spindleSync)) { + tp_err_t res = TP_ERR_OK; + + 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; - tp->spindle.offset = 0.0; + emcmotStatus->spindle_fb.index_enable = 1; + + tp->spindle.origin.position = 0.0; + tp->spindle.origin.direction = signum(emcmotStatus->spindle_cmd.velocity_rpm_out); rtapi_print_msg(RTAPI_MSG_DBG, "Waiting on sync...\n"); - return TP_ERR_WAITING; + res = TP_ERR_WAITING; } - return TP_ERR_OK; + checkPositionMatch(tp, tc); + return res; } @@ -2767,7 +2952,7 @@ STATIC tp_err_t 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(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) { @@ -2790,38 +2975,30 @@ STATIC void tpSyncVelocityMode(TC_STRUCT * const tc, TC_STRUCT * const nexttc) { 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 ((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; - } + spindle_origin_t spindle_origin = tp->spindle.origin; - 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, + spindle_origin); - 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.origin.position = spindle_pos; + tc->progress_at_sync = tc->progress; + tc_debug_print("Spindle offset %f\n", tp->spindle.origin.position); tc->sync_accel = 0; tc->target_vel = target_vel; } else { @@ -2830,17 +3007,80 @@ 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 + const double pos_desired = spindle_displacement * tc->uu_per_rev; + double net_progress = tc->progress - tc->progress_at_sync; + + if(nexttc) { + // If we're in a parabolic blend, the next segment will be active too, + // so make sure to account for its progress + net_progress += nexttc->progress; + } + const double pos_error = pos_desired - net_progress; + tc_debug_print("pos_desired %f, net_progress %f, pos_error %f\n", + pos_desired, + net_progress, + 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) * tcGetTangentialMaxAccel(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 = tcGetTangentialMaxAccel(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; + } + 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; } - tc->target_vel = target_vel + errorvel; + 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; + } + } + emcmotStatus->pos_tracking_error = pos_error; + + 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 @@ -2864,7 +3104,6 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, STATIC int tpDoParabolicBlending(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT * const nexttc) { - tc_debug_print("in DoParabolicBlend\n"); tpUpdateBlend(tp,tc,nexttc); /* Status updates */ @@ -2892,8 +3131,10 @@ STATIC int tpDoParabolicBlending(TP_STRUCT * const tp, TC_STRUCT * const tc, * Handles the majority of updates on a single segment for the current cycle. */ STATIC int tpUpdateCycle(TP_STRUCT * const tp, - TC_STRUCT * const tc, TC_STRUCT const * const nexttc) { - + TC_STRUCT * const tc, + TC_STRUCT const * const nexttc, + UpdateCycleMode cycle_mode) +{ //placeholders for position for this update EmcPose before; @@ -2920,8 +3161,9 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp, tpCalculateTrapezoidalAccel(tp, tc, nexttc, &acc, &vel_desired); } + int accel_mode_ramp = (res_accel == TP_ERR_OK); tcUpdateDistFromAccel(tc, acc, vel_desired); - tpDebugCycleInfo(tp, tc, nexttc, acc); + tpDebugCycleInfo(tp, tc, nexttc, acc, accel_mode_ramp, cycle_mode); //Check if we're near the end of the cycle and set appropriate changes tpCheckEndCondition(tp, tc, nexttc); @@ -2935,12 +3177,6 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp, //Store displacement (checking for valid pose) int res_set = tpAddCurrentPos(tp, &displacement); -#ifdef TC_DEBUG - double mag; - emcPoseMagnitude(&displacement, &mag); - tc_debug_print("cycle movement = %f\n", mag); -#endif - return res_set; } @@ -2966,7 +3202,7 @@ STATIC int tpUpdateInitialStatus(TP_STRUCT const * const tp) { STATIC inline int tcSetSplitCycle(TC_STRUCT * const tc, double split_time, double v_f) { - tp_debug_print("split time for id %d is %.16g\n", tc->id, split_time); + tc_pdebug_print("split time for id %d is %.16g\n", tc->id, split_time); if (tc->splitting != 0 && split_time > 0.0) { rtapi_print_msg(RTAPI_MSG_ERR,"already splitting on id %d with cycle time %.16g, dx = %.16g, split time %.12g\n", tc->id, @@ -2989,104 +3225,38 @@ STATIC inline int tcSetSplitCycle(TC_STRUCT * const tc, double split_time, * then we flag the segment as "splitting", so that during the next cycle, * it handles the transition to the next segment. */ -STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc) { - - //Assume no split time unless we find otherwise - tc->cycle_time = tp->cycleTime; - - //Initial guess at dt for next round - double dx = tc->target - tc->progress; - tc_debug_print("tpCheckEndCondition: dx = %e\n",dx); - - if (dx <= TP_POS_EPSILON) { - //If the segment is close to the target position, then we assume that it's done. - tp_debug_print("close to target, dx = %.12f\n",dx); - //Force progress to land exactly on the target to prevent numerical errors. - tc->progress = tc->target; - tcSetSplitCycle(tc, 0.0, tc->currentvel); - if (tc->term_cond == TC_TERM_COND_STOP || tc->term_cond == TC_TERM_COND_EXACT) { - tc->remove = 1; +STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc) +{ + EndCondition ec = checkEndCondition(tp->cycleTime, + tc->progress, + tc->target, + tc->currentvel, + tpGetRealFinalVel(tp, tc, nexttc), + tcGetTangentialMaxAccel(tc) + ); + + double dt = ec.dt; + int splitting = dt < tp->cycleTime; + if (splitting) { + if (dt < TP_TIME_EPSILON) { + dt = 0.0; } - return TP_ERR_OK; - } else if (tc->term_cond == TC_TERM_COND_STOP || tc->term_cond == TC_TERM_COND_EXACT) { - return TP_ERR_NO_ACTION; + tcSetSplitCycle(tc, dt, ec.v_f); } - - - double v_f = tpGetRealFinalVel(tp, tc, nexttc); - double v_avg = (tc->currentvel + v_f) / 2.0; - - //Check that we have a non-zero "average" velocity between now and the - //finish. If not, it means that we have to accelerate from a stop, which - //will take longer than the minimum 2 timesteps that each segment takes, so - //we're safely far form the end. - - //Get dt assuming that we can magically reach the final velocity at - //the end of the move. - // - //KLUDGE: start with a value below the cutoff - double dt = TP_TIME_EPSILON / 2.0; - if (v_avg > TP_VEL_EPSILON) { - //Get dt from distance and velocity (avoid div by zero) - dt = fmax(dt, dx / v_avg); - } else { - if ( dx > (v_avg * tp->cycleTime) && dx > TP_POS_EPSILON) { - tc_debug_print(" below velocity threshold, assuming far from end\n"); - return TP_ERR_NO_ACTION; - } +#ifdef TC_DEBUG + { + print_json5_log_start(checkEndCondition, Run); + print_json5_long_("id", tc->id); + print_json5_double_("v_final", ec.v_f); + print_json5_double_("t_remaining", ec.dt); + print_json5_double_("dt_used", dt); + print_json5_bool_("remove", tc->remove); + print_json5_bool_("need_split", splitting); + print_json5_end_(); } +#endif - //Calculate the acceleration this would take: - - double dv = v_f - tc->currentvel; - double a_f = dv / dt; - - //If this is a valid acceleration, then we're done. If not, then we solve - //for v_f and dt given the max acceleration allowed. - double a_max = tcGetTangentialMaxAccel(tc); - - //If we exceed the maximum acceleration, then the dt estimate is too small. - double a = a_f; - int recalc = sat_inplace(&a, a_max); - - //Need to recalculate vf and above - if (recalc) { - tc_debug_print(" recalculating with a_f = %f, a = %f\n", a_f, a); - double disc = pmSq(tc->currentvel / a) + 2.0 / a * dx; - if (disc < 0) { - //Should mean that dx is too big, i.e. we're not close enough - tc_debug_print(" dx = %f, too large, not at end yet\n",dx); - return TP_ERR_NO_ACTION; - } - - if (disc < TP_TIME_EPSILON * TP_TIME_EPSILON) { - tc_debug_print("disc too small, skipping sqrt\n"); - dt = -tc->currentvel / a; - } else if (a > 0) { - tc_debug_print("using positive sqrt\n"); - dt = -tc->currentvel / a + pmSqrt(disc); - } else { - tc_debug_print("using negative sqrt\n"); - dt = -tc->currentvel / a - pmSqrt(disc); - } - - tc_debug_print(" revised dt = %f\n", dt); - //Update final velocity with actual result - v_f = tc->currentvel + dt * a; - } - if (dt < TP_TIME_EPSILON) { - //Close enough, call it done - tc_debug_print("revised dt small, finishing tc\n"); - tc->progress = tc->target; - tcSetSplitCycle(tc, 0.0, v_f); - } else if (dt < tp->cycleTime ) { - tc_debug_print(" corrected v_f = %f, a = %f\n", v_f, a); - tcSetSplitCycle(tc, dt, v_f); - } else { - tc_debug_print(" dt = %f, not at end yet\n",dt); - return TP_ERR_NO_ACTION; - } return TP_ERR_OK; } @@ -3103,7 +3273,18 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, EmcPose before; tcGetPos(tc, &before); - tp_debug_print("tc id %d splitting\n",tc->id); +#ifdef TC_DEBUG + /* Debug Output */ + print_json5_log_start(tc_splitting, Run); + print_json5_double_("id", tc->id); + print_json5_double_("target", tc->target); + print_json5_double_("progress", tc->progress); + print_json5_double_("v_terminal", tc->term_vel); + print_json5_double_("dt", tc->cycle_time); + print_json5_end_(); +#endif + + //Shortcut tc update by assuming we arrive at end tc->progress = tc->target; //Get displacement from prev. position @@ -3114,12 +3295,6 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, // Update tp's position (checking for valid pose) tpAddCurrentPos(tp, &displacement); -#ifdef TC_DEBUG - double mag; - emcPoseMagnitude(&displacement, &mag); - tc_debug_print("cycle movement = %f\n",mag); -#endif - // Trigger removal of current segment at the end of the cycle tc->remove = 1; @@ -3128,33 +3303,31 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, return TP_ERR_OK; } + // Handle various cases of updates for split cycles + // Tangent: next segment gets a partial update for the remaining cycle time + // Parabolic: next segment updates with full cycle time (since both current / next are active at the same time) + // Exact: NO motion in next segment here since current segment must stop completely switch (tc->term_cond) { case TC_TERM_COND_TANGENT: nexttc->cycle_time = tp->cycleTime - tc->cycle_time; nexttc->currentvel = tc->term_vel; - tp_debug_print("Doing tangent split\n"); - break; + // Intentional fallthrough case TC_TERM_COND_PARABOLIC: + { + TC_STRUCT *next2tc = tcqItem(&tp->queue, 2); + tpUpdateCycle(tp, nexttc, next2tc, UPDATE_SPLIT); + } break; case TC_TERM_COND_STOP: - break; case TC_TERM_COND_EXACT: break; - default: - rtapi_print_msg(RTAPI_MSG_ERR,"unknown term cond %d in segment %d\n", - tc->term_cond, - tc->id); } - // Run split cycle update with remaining time in nexttc - // KLUDGE: use next cycle after nextc to prevent velocity dip (functions fail gracefully w/ NULL) - TC_STRUCT *next2tc = tcqItem(&tp->queue, 2); - - tpUpdateCycle(tp, nexttc, next2tc); - // Update status for the split portion // FIXME redundant tangent check, refactor to switch - if (tc->cycle_time > nexttc->cycle_time && tc->term_cond == TC_TERM_COND_TANGENT) { + if (tc->term_cond == TC_TERM_COND_STOP + || tc->term_cond == TC_TERM_COND_EXACT + || (tc->cycle_time > nexttc->cycle_time && tc->term_cond == TC_TERM_COND_TANGENT)) { //Majority of time spent in current segment tpToggleDIOs(tc); tpUpdateMovementStatus(tp, tc); @@ -3170,14 +3343,16 @@ STATIC int tpHandleRegularCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT * const nexttc) { + if (!tc) { + return TP_ERR_FAIL; + } if (tc->remove) { //Don't need to update since this segment is flagged for removal return TP_ERR_NO_ACTION; } //Run with full cycle time - tc_debug_print("Normal cycle\n"); tc->cycle_time = tp->cycleTime; - tpUpdateCycle(tp, tc, nexttc); + tpUpdateCycle(tp, tc, nexttc, UPDATE_NORMAL); /* Parabolic blending */ @@ -3187,10 +3362,12 @@ STATIC int tpHandleRegularCycle(TP_STRUCT * const tp, double target_vel_this = tpGetRealTargetVel(tp, tc); double target_vel_next = tpGetRealTargetVel(tp, nexttc); - tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, NULL); - tc->blend_vel = v_this; - if (nexttc) { - nexttc->blend_vel = v_next; + if (tc->term_cond == TC_TERM_COND_PARABOLIC) { + tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, NULL); + tc->blend_vel = v_this; + if (nexttc) { + nexttc->blend_vel = v_next; + } } if (nexttc && tcIsBlending(tc)) { @@ -3211,7 +3388,25 @@ STATIC int tpHandleRegularCycle(TP_STRUCT * const tp, * status; I think those are spelled out here correctly and I can't clean it up * without breaking the API that the TP presents to motion. */ -int tpRunCycle(TP_STRUCT * const tp, long period) +tp_err_t updateSyncTargets(TP_STRUCT *tp, TC_STRUCT *tc, TC_STRUCT *nexttc) +{ + switch (tc->synchronized) { + case TC_SYNC_NONE: + emcmotStatus->spindle_fb.synced = 0; + return TP_ERR_OK; + case TC_SYNC_VELOCITY: + tc_debug_print("sync velocity\n"); + tpSyncVelocityMode(tc, nexttc); + return TP_ERR_OK; + case TC_SYNC_POSITION: + tc_debug_print("sync position\n"); + tpSyncPositionMode(tp, tc, nexttc); + return TP_ERR_OK; + } + return TP_ERR_INVALID; +} + +int tpRunCycleInternal(TP_STRUCT * const tp) { //Pointers to current and next trajectory component TC_STRUCT *tc; @@ -3226,22 +3421,12 @@ int tpRunCycle(TP_STRUCT * const tp, long period) //Set GUI status to "zero" state tpUpdateInitialStatus(tp); -#ifdef TC_DEBUG - //Hack debug output for timesteps - // NOTE: need to track every timestep, even those where the trajectory planner is idle - static double time_elapsed = 0; - time_elapsed+=tp->cycleTime; -#endif - //If we have a NULL pointer, then the queue must be empty, so we're done. if(!tc) { tpHandleEmptyQueue(tp); return TP_ERR_WAITING; } - tc_debug_print("-------------------\n"); - - /* If the queue empties enough, assume that the program is near the end. * This forces the last segment to be "finalized" to let the optimizer run.*/ /*tpHandleLowQueue(tp);*/ @@ -3261,6 +3446,7 @@ int tpRunCycle(TP_STRUCT * const tp, long period) int res = tpActivateSegment(tp, tc); // Need to wait to continue motion, end planning here if (res == TP_ERR_WAITING) { + tp->time_at_wait = tp->time_elapsed_ticks; return TP_ERR_WAITING; } } @@ -3270,29 +3456,12 @@ int tpRunCycle(TP_STRUCT * const tp, long period) tpUpdateRigidTapState(tp, tc); } - /** If synchronized with spindle, calculate requested velocity to track - * spindle motion.*/ - switch (tc->synchronized) { - case TC_SYNC_NONE: - emcmotStatus->spindleSync = 0; - break; - case TC_SYNC_VELOCITY: - tp_debug_print("sync velocity\n"); - tpSyncVelocityMode(tc, nexttc); - break; - case TC_SYNC_POSITION: - tp_debug_print("sync position\n"); - tpSyncPositionMode(tp, tc, nexttc); - break; - default: - tp_debug_print("unrecognized spindle sync state!\n"); - break; - } - -#ifdef TC_DEBUG - EmcPose pos_before = tp->currentPos; -#endif + // Assume zero tracking error unless position sync is active + emcmotStatus->pos_tracking_error = 0.0; + // If synchronized with spindle, calculate requested velocity to track spindle motion + updateSyncTargets(tp, tc, nexttc); + tcClearFlags(tc); tcClearFlags(nexttc); @@ -3303,22 +3472,6 @@ int tpRunCycle(TP_STRUCT * const tp, long period) tpHandleRegularCycle(tp, tc, nexttc); } -#ifdef TC_DEBUG - double mag; - EmcPose disp; - emcPoseSub(&tp->currentPos, &pos_before, &disp); - emcPoseMagnitude(&disp, &mag); - tc_debug_print("time: %.12e total movement = %.12e vel = %.12e\n", - time_elapsed, - mag, emcmotStatus->current_vel); - - tc_debug_print("tp_displacement = %.12e %.12e %.12e time = %.12e\n", - disp.tran.x, - disp.tran.y, - disp.tran.z, - time_elapsed); -#endif - // If TC is complete, remove it from the queue. if (tc->remove) { tpCompleteSegment(tp, tc); @@ -3327,6 +3480,62 @@ int tpRunCycle(TP_STRUCT * const tp, long period) return TP_ERR_OK; } + +int tpRunCycle(TP_STRUCT *tp, long period) +{ + // Before every TP update, ensure that elapsed time and + // TP measurements are stored for error checks + tp->time_elapsed_sec+=tp->cycleTime; + ++tp->time_elapsed_ticks; + EmcPose const axis_pos_old = tp->currentPos; + + int res = tpRunCycleInternal(tp); + + if (emcmotConfig->consistencyCheckConfig.extraConsistencyChecks) { + // After update (even a no-op), update pos / vel / accel + EmcPose const axis_vel_old = tp->currentVel; + EmcPose const axis_pos = tp->currentPos; + + EmcPose axis_vel; + emcPoseSub(&axis_pos, &axis_pos_old, &axis_vel); + emcPoseMultScalar(&axis_vel, 1.0 / tp->cycleTime); + tp->currentVel = axis_vel; + + EmcPose axis_accel; + emcPoseSub(&axis_vel, &axis_vel_old, &axis_accel); + emcPoseMultScalar(&axis_accel, 1.0 / tp->cycleTime); + + unsigned accel_error_mask = findAccelViolations(axis_accel); + unsigned vel_error_mask = findVelocityViolations(axis_vel); + unsigned pos_limit_error_mask = findPositionLimitViolations(axis_pos); + + reportTPAxisError(tp, accel_error_mask, "Acceleration limit exceeded"); + reportTPAxisError(tp, vel_error_mask, "Velocity limit exceeded"); + reportTPAxisError(tp, pos_limit_error_mask, "Position limits exceeded"); + +#ifdef TC_DEBUG + unsigned debug_mask = (unsigned)(-1); +#else + unsigned debug_mask = 0; +#endif + + if (debug_mask || ( + accel_error_mask | vel_error_mask | pos_limit_error_mask) + ) { + print_json5_log_start(tpRunCycle, Run); + print_json5_ll_("time_ticks", tp->time_elapsed_ticks); + print_json5_EmcPose(axis_pos); + print_json5_EmcPose(axis_vel); + print_json5_EmcPose(axis_accel); + double current_vel = emcmotStatus->current_vel; + print_json5_double(current_vel); + print_json5_double_("time", tp->time_elapsed_sec); + print_json5_end_(); + } + } + return res; +} + int tpSetSpindleSync(TP_STRUCT * const tp, double sync, int mode) { // WARNING assumes positive sync if(sync > 0) { diff --git a/src/emc/tp/tp.h b/src/emc/tp/tp.h index 301e4ac5c18..f6b5571188f 100644 --- a/src/emc/tp/tp.h +++ b/src/emc/tp/tp.h @@ -26,20 +26,44 @@ int tpClearDIOs(TP_STRUCT * const tp); int tpSetCycleTime(TP_STRUCT * const tp, double secs); int tpSetVmax(TP_STRUCT * const tp, double vmax, double ini_maxvel); int tpSetVlimit(TP_STRUCT * const tp, double vLimit); -int tpSetAmax(TP_STRUCT * const tp, double aMax); int tpSetId(TP_STRUCT * const tp, int id); int tpGetExecId(TP_STRUCT * const tp); -int tpSetTermCond(TP_STRUCT * const tp, int cond, double tolerance); +int tpSetTermCond(TP_STRUCT * const tp, tc_term_cond_t cond, double tolerance); int tpSetPos(TP_STRUCT * const tp, EmcPose const * const pos); int tpAddCurrentPos(TP_STRUCT * const tp, EmcPose const * const disp); int tpSetCurrentPos(TP_STRUCT * const tp, EmcPose const * const pos); -int tpAddRigidTap(TP_STRUCT * const tp, EmcPose end, double vel, double - ini_maxvel, double acc, unsigned char enables); -int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, double vel, double - ini_maxvel, double acc, unsigned char enables, char atspeed, int indexrotary); -int tpAddCircle(TP_STRUCT * const tp, EmcPose end, PmCartesian center, - PmCartesian normal, int turn, int canon_motion_type, double vel, double ini_maxvel, - double acc, unsigned char enables, char atspeed); + +int tpAddRigidTap(TP_STRUCT * const tp, + EmcPose end, + double vel, + double ini_maxvel, + double acc, + unsigned char enables); + +int tpAddLine(TP_STRUCT * const tp, + EmcPose end, + int canon_motion_type, + double vel, + double ini_maxvel, + double acc, + unsigned char enables, + char atspeed, + int indexrotary, + int pure_angular); + +int tpAddCircle(TP_STRUCT * const tp, + EmcPose end, + PmCartesian center, + PmCartesian normal, + int turn, + int canon_motion_type, + double vel, + double ini_maxvel, + double acc, + double acc_normal, + unsigned char enables, + char atspeed); + int tpRunCycle(TP_STRUCT * const tp, long period); int tpPause(TP_STRUCT * const tp); int tpResume(TP_STRUCT * const tp); diff --git a/src/emc/tp/tp_debug.h b/src/emc/tp/tp_debug.h index d14c5e5e511..9f63a19ffad 100644 --- a/src/emc/tp/tp_debug.h +++ b/src/emc/tp/tp_debug.h @@ -6,7 +6,7 @@ * License: GPL Version 2 * System: Linux * -* Copyright (c) 2013 All rights reserved. +* Copyright (c) 2013-2019 All rights reserved. * * Last change: ********************************************************************/ @@ -14,23 +14,44 @@ #define TP_DEBUG_H #include "rtapi.h" /* printing functions */ +#include "posemath.h" +#include "emcpos.h" +#include "rtapi_json5.h" /** TP debug stuff */ #ifdef TP_DEBUG //Kludge because I didn't know any better at the time //FIXME replace these with better names? #define tp_debug_print(...) rtapi_print(__VA_ARGS__) +#define tp_debug_only(funcname,...) funcname(__VA_ARGS__) #elif defined(UNIT_TEST) #include #define tp_debug_print(...) printf(__VA_ARGS__) +#define tp_debug_only(funcname,...) funcname(__VA_ARGS__) #else #define tp_debug_print(...) +#define tp_debug_only(funcname,...) #endif -// Verbose but effective wrappers for building faux-JSON debug output for a function -#define tp_debug_json_double(varname_) tp_debug_print("%s: %g, ", #varname_, varname_) -#define tp_debug_json_start(fname_) tp_debug_print("%s: {", #fname_) -#define tp_debug_json_end() tp_debug_print("}\n") +#define print_json5_log_start(fname_, level_) do { \ + print_json5_start_(); \ + print_json5_string_("log_entry", #fname_); \ + print_json5_string_("log_level", #level_); \ +} while (0) + +#define print_json5_log_end() print_json5_end_() + +// Another layer of macro wrappers to conditionally compile debug-only statements +#define tp_debug_json_double(varname_) tp_debug_only(print_json5_double, varname_) +#define tp_debug_json_string(varname_) tp_debug_only(print_json5_string, varname_) +#define tp_debug_json_long(varname_) tp_debug_only(print_json5_long, varname_) +#define tp_debug_json_unsigned(varname_) tp_debug_only(print_json5_unsigned, varname_) +#define tp_debug_json_PmCartesian(varname_) tp_debug_only(print_json5_PmCartesian, varname_) +#define tp_debug_json_EmcPose(varname_) tp_debug_only(print_json5_EmcPose, varname_) +#define tp_debug_json_PmCartLine(name_, value_) tp_debug_only(print_json5_PmCartLine_, name_, value_) +#define tp_debug_json_PmCircle(name_, value_) tp_debug_only(print_json5_PmCircle_, name_, value_) +#define tp_debug_json_log_start(fname_, level) tp_debug_only(print_json5_log_start, fname_, level) +#define tp_debug_json_log_end() tp_debug_only(print_json5_end_) /** Use for profiling to make static function names visible */ #ifdef TP_PROFILE @@ -46,6 +67,13 @@ #define tc_debug_print(...) #endif +/** "TC" debug info for inspecting trajectory planner output at each timestep */ +#ifdef TP_PEDANTIC_DEBUG +#define tc_pdebug_print(...) rtapi_print(__VA_ARGS__) +#else +#define tc_pdebug_print(...) +#endif + /** TP position data output to debug acceleration spikes */ #ifdef TP_POSEMATH_DEBUG #define tp_posemath_debug(...) rtapi_print(__VA_ARGS__) diff --git a/src/emc/tp/tp_types.h b/src/emc/tp/tp_types.h index ac4cc38138a..32b24b15c11 100644 --- a/src/emc/tp/tp_types.h +++ b/src/emc/tp/tp_types.h @@ -37,7 +37,7 @@ /* "neighborhood" size (if two values differ by less than the epsilon, * then they are effectively equal.)*/ #define TP_ACCEL_EPSILON 1e-4 -#define TP_VEL_EPSILON 1e-8 +#define TP_VEL_EPSILON DOUBLE_FUZZ #define TP_POS_EPSILON 1e-12 #define TP_TIME_EPSILON 1e-12 #define TP_ANGLE_EPSILON 1e-6 @@ -77,8 +77,8 @@ typedef enum { * synchronized motion code. */ typedef struct { - double offset; - double revs; + spindle_origin_t origin; //!< initial position of spindle during synchronization (direction-aware) + int waiting_for_index; int waiting_for_atspeed; } tp_spindle_t; @@ -94,6 +94,7 @@ typedef struct { EmcPose currentPos; EmcPose goalPos; + EmcPose currentVel; int queueSize; double cycleTime; @@ -104,16 +105,11 @@ typedef struct { subsequent moves */ double vLimit; /* absolute upper limit on all vels */ - double aMax; /* max accel (unused) */ - //FIXME this shouldn't be a separate limit, - double aMaxCartesian; /* max cartesian acceleration by machine bounds */ - double aLimit; /* max accel (unused) */ - double wMax; /* rotational velocity max */ double wDotMax; /* rotational accelleration max */ int nextId; int execId; - int termCond; + tc_term_cond_t termCond; int done; int depth; /* number of total queued motions */ int activeDepth; /* number of motions blending */ @@ -123,7 +119,7 @@ typedef struct { double tolerance; /* for subsequent motions, stay within this distance of the programmed path during blends */ - int synchronized; // spindle sync required for this move + tc_spindle_sync_t synchronized; // spindle sync required for this move int velocity_mode; /* TRUE if spindle sync is in velocity mode, FALSE if in position mode */ double uu_per_rev; /* user units per spindle revolution */ @@ -131,6 +127,10 @@ typedef struct { syncdio_t syncdio; //record tpSetDout's here + double time_elapsed_sec; // Total elapsed TP run time in seconds + long long time_elapsed_ticks; // Total elapsed TP run time in cycles (ticks) + long long time_at_wait; // Time when TP started to wait for spindle + } TP_STRUCT; diff --git a/src/emc/usr_intf/axis/scripts/axis.py b/src/emc/usr_intf/axis/scripts/axis.py index 7ed3c786ccc..75bd87a7b44 100755 --- a/src/emc/usr_intf/axis/scripts/axis.py +++ b/src/emc/usr_intf/axis/scripts/axis.py @@ -1697,6 +1697,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/emc/usr_intf/gremlin/gremlin.py b/src/emc/usr_intf/gremlin/gremlin.py index 99b661c7724..a43ef3f003f 100755 --- a/src/emc/usr_intf/gremlin/gremlin.py +++ b/src/emc/usr_intf/gremlin/gremlin.py @@ -267,6 +267,19 @@ def load(self,filename = None): unitcode = "G%d" % (20 + (s.linear_units == 1)) initcode = self.inifile.find("RS274NGC", "RS274NGC_STARTUP_CODE") or "" result, seq = self.load_preview(filename, canon, unitcode, initcode) + # Warn user if spindle-synched feeds violate axis limits + axis_max_vel = tuple([ + float(self.inifile.find("AXIS_%d" % i,"MAX_VELOCITY") or 0.0)*60 + for i in range(9)]) + for line_no, delta, rpm, fpr in canon.feed_synched: + fpm = rpm * fpr # feed per minute + max_fpm = 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 result > gcode.MIN_ERROR: self.report_gcode_error(result, seq, filename) diff --git a/src/hal/classicladder/drawing_sequential.h b/src/hal/classicladder/drawing_sequential.h index 6762f77e737..cd67d2a72ca 100644 --- a/src/hal/classicladder/drawing_sequential.h +++ b/src/hal/classicladder/drawing_sequential.h @@ -13,6 +13,9 @@ // 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 +#include "sequential.h" +#include "gdk/gdktypes.h" + void DrawSeqStep(GdkPixmap * DrawPixmap,int x,int y,int Size,StrStep * pStep,char DrawingOption); void DrawSeqTransition(GdkPixmap * DrawPixmap,int x,int y,int Size,StrTransition * pTransi,char DrawingOption); diff --git a/src/hal/classicladder/files_sequential.c b/src/hal/classicladder/files_sequential.c index 95dad5a3901..a1ef0cf409b 100644 --- a/src/hal/classicladder/files_sequential.c +++ b/src/hal/classicladder/files_sequential.c @@ -28,6 +28,7 @@ #include "global.h" #include "files.h" #include "files_sequential.h" +#include "sequential.h" diff --git a/src/hal/classicladder/sequential.h b/src/hal/classicladder/sequential.h index 61623f1e8bd..1decf1a52be 100644 --- a/src/hal/classicladder/sequential.h +++ b/src/hal/classicladder/sequential.h @@ -18,6 +18,8 @@ /* License along with this library; if not, write to the Free Software */ /* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +#ifndef SEQUENTIAL_H +#define SEQUENTIAL_H #define NBR_SEQUENTIAL_PAGES 5 @@ -116,4 +118,4 @@ typedef struct StrSequential StrSeqComment SeqComment[ NBR_SEQ_COMMENTS ]; }StrSequential; - +#endif diff --git a/src/hal/components/d2dt2.comp b/src/hal/components/d2dt2.comp new file mode 100644 index 00000000000..e9a96ea8a6a --- /dev/null +++ b/src/hal/components/d2dt2.comp @@ -0,0 +1,37 @@ +// This is a component for EMC2 HAL +// Copyright 2019 Robert Ellenberg +// Based on Jeff Epler's ddt component +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// 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 d2dt2 "Compute two derivativse of the input function by first-order backward difference"; + +pin in float in; +pin out float out1; +pin out float out2; + +variable double old0; +variable double old1; + +function _; +license "GPL"; +;; +double tmp = in; +// Introduces slight rounding error but is faster than division +const double dt_inv = 1.0e9 / period; +// Cascaded, 1st-order-accuracy backward difference +out1 = (tmp - old0) * dt_inv; +out2 = (out1 - old1) * dt_inv; +old0 = tmp; +old1 = out1; 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/src/hal/utils/scope_rt.c b/src/hal/utils/scope_rt.c index 738e2007b3d..8530b8f9702 100644 --- a/src/hal/utils/scope_rt.c +++ b/src/hal/utils/scope_rt.c @@ -44,7 +44,7 @@ MODULE_AUTHOR("John Kasunich"); MODULE_DESCRIPTION("Oscilloscope for EMC HAL"); MODULE_LICENSE("GPL"); -long num_samples = 16000; +long num_samples = SCOPE_NUM_SAMPLES_DEFAULT; long shm_size; RTAPI_MP_LONG(num_samples, "Number of samples in the shared memory block") diff --git a/src/hal/utils/scope_shm.h b/src/hal/utils/scope_shm.h index 1e7cef19530..1c807ae0ce0 100644 --- a/src/hal/utils/scope_shm.h +++ b/src/hal/utils/scope_shm.h @@ -42,7 +42,7 @@ ************************************************************************/ #define SCOPE_SHM_KEY 0x130CF406 -#define SCOPE_NUM_SAMPLES_DEFAULT 16000 +#define SCOPE_NUM_SAMPLES_DEFAULT 128000 typedef enum { IDLE = 0, /* waiting for run command */ diff --git a/src/libnml/posemath/_posemath.c b/src/libnml/posemath/_posemath.c index 2d6ba394263..abfd50946fe 100644 --- a/src/libnml/posemath/_posemath.c +++ b/src/libnml/posemath/_posemath.c @@ -777,12 +777,9 @@ int pmHomPoseConvert(PmHomogeneous const * const h, PmPose * const p) int pmCartCartCompare(PmCartesian const * const v1, PmCartesian const * const v2) { - if (fabs(v1->x - v2->x) >= V_FUZZ || - fabs(v1->y - v2->y) >= V_FUZZ || fabs(v1->z - v2->z) >= V_FUZZ) { - return 0; - } - - return 1; + return fabs(v1->x - v2->x) < V_FUZZ + && fabs(v1->y - v2->y) < V_FUZZ + && fabs(v1->z - v2->z) < V_FUZZ; } int pmCartCartDot(PmCartesian const * const v1, PmCartesian const * const v2, double *d) @@ -924,7 +921,7 @@ int pmCartInvEq(PmCartesian * const v) if (size_sq == 0.0) { #ifdef PM_PRINT_ERROR - pmPrintError(&"Zero vector in pmCartInv\n"); + pmPrintError("Zero vector in pmCartInv\n"); #endif return pmErrno = PM_NORM_ERR; } @@ -991,7 +988,7 @@ int pmCartScalDivEq(PmCartesian * const v, double d) if (d == 0.0) { #ifdef PM_PRINT_ERROR - pmPrintError(&"Divide by 0 in pmCartScalDiv\n"); + pmPrintError("Divide by 0 in pmCartScalDiv\n"); #endif return pmErrno = PM_DIV_ERR; @@ -1455,7 +1452,7 @@ int pmQuatCartMult(PmQuaternion const * const q1, PmCartesian const * const v2, #ifdef PM_DEBUG if (!pmQuatIsNorm(q1)) { #ifdef PM_PRINT_ERROR - pmPrintError(&"Bad quaternion in pmQuatCartMult\n"); + pmPrintError("Bad quaternion in pmQuatCartMult\n"); #endif return pmErrno = PM_NORM_ERR; } @@ -1508,7 +1505,7 @@ int pmPoseCartMult(PmPose const * const p1, PmCartesian const * const v2, PmCart #ifdef PM_DEBUG if (!pmQuatIsNorm(&p1->rot)) { #ifdef PM_PRINT_ERROR - pmPrintError(&"Bad quaternion in pmPoseCartMult\n"); + pmPrintError("Bad quaternion in pmPoseCartMult\n"); #endif return pmErrno = PM_NORM_ERR; } diff --git a/src/libnml/posemath/posemath.h b/src/libnml/posemath/posemath.h index f64448f416e..86a1446d20f 100644 --- a/src/libnml/posemath/posemath.h +++ b/src/libnml/posemath/posemath.h @@ -123,7 +123,7 @@ struct PM_HOMOGENEOUS; /* Hom */ struct PM_CARTESIAN { /* ctors/dtors */ PM_CARTESIAN() { - }; + } PM_CARTESIAN(double _x, double _y, double _z); #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_CARTESIAN(PM_CCONST PM_CARTESIAN & cart); // added 7-May-1997 @@ -151,7 +151,7 @@ struct PM_CARTESIAN { struct PM_SPHERICAL { /* ctors/dtors */ PM_SPHERICAL() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_SPHERICAL(PM_CCONST PM_SPHERICAL & s); #endif @@ -171,7 +171,7 @@ struct PM_SPHERICAL { struct PM_CYLINDRICAL { /* ctors/dtors */ PM_CYLINDRICAL() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_CYLINDRICAL(PM_CCONST PM_CYLINDRICAL & c); #endif @@ -191,7 +191,7 @@ struct PM_CYLINDRICAL { struct PM_ROTATION_VECTOR { /* ctors/dtors */ PM_ROTATION_VECTOR() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_ROTATION_VECTOR(PM_CCONST PM_ROTATION_VECTOR & r); #endif @@ -211,7 +211,7 @@ struct PM_ROTATION_VECTOR { struct PM_ROTATION_MATRIX { /* ctors/dtors */ PM_ROTATION_MATRIX() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_ROTATION_MATRIX(PM_CCONST PM_ROTATION_MATRIX & mat); /* added 7-May-1997 @@ -244,7 +244,7 @@ enum PM_AXIS { PM_X, PM_Y, PM_Z }; struct PM_QUATERNION { /* ctors/dtors */ PM_QUATERNION() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_QUATERNION(PM_CCONST PM_QUATERNION & quat); /* added 7-May-1997 by WPS */ @@ -274,7 +274,7 @@ struct PM_QUATERNION { struct PM_EULER_ZYZ { /* ctors/dtors */ PM_EULER_ZYZ() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_EULER_ZYZ(PM_CCONST PM_EULER_ZYZ & zyz); #endif @@ -294,7 +294,7 @@ struct PM_EULER_ZYZ { struct PM_EULER_ZYX { /* ctors/dtors */ PM_EULER_ZYX() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_EULER_ZYX(PM_CCONST PM_EULER_ZYX & zyx); #endif @@ -314,7 +314,7 @@ struct PM_EULER_ZYX { struct PM_RPY { /* ctors/dtors */ PM_RPY() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_RPY(PM_CCONST PM_RPY PM_REF rpy); /* added 7-May-1997 by WPS */ #endif @@ -334,7 +334,7 @@ struct PM_RPY { struct PM_POSE { /* ctors/dtors */ PM_POSE() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_POSE(PM_CCONST PM_POSE & p); #endif @@ -356,7 +356,7 @@ struct PM_POSE { struct PM_HOMOGENEOUS { /* ctors/dtors */ PM_HOMOGENEOUS() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_HOMOGENEOUS(PM_CCONST PM_HOMOGENEOUS & h); #endif @@ -376,7 +376,7 @@ struct PM_HOMOGENEOUS { struct PM_LINE { /* ctors/dtors */ PM_LINE() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_LINE(PM_CCONST PM_LINE &); #endif @@ -396,7 +396,7 @@ struct PM_LINE { struct PM_CIRCLE { /* ctors/dtors */ PM_CIRCLE() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_CIRCLE(PM_CCONST PM_CIRCLE &); #endif @@ -686,6 +686,7 @@ extern "C" { #define pmClose(a, b, eps) ((fabs((a) - (b)) < (eps)) ? 1 : 0) #define pmSq(x) ((x)*(x)) +#define pmCb(x) ((x)*(x)*(x)) #ifdef TO_DEG #undef TO_DEG @@ -770,10 +771,14 @@ extern "C" { /* global error number and errors */ extern int pmErrno; extern void pmPerror(const char *fmt); -#define PM_ERR -1 /* unspecified error */ -#define PM_IMPL_ERR -2 /* not implemented */ -#define PM_NORM_ERR -3 /* arg should have been norm */ -#define PM_DIV_ERR -4 /* divide by zero error */ + + typedef enum { + PM_DIV_ERR = -4, /* divide by zero error */ + PM_NORM_ERR = -3, /* arg should have been norm */ + PM_IMPL_ERR = -2, /* not implemented */ + PM_ERR = -1, /* unspecified error */ + PM_OK = 0 + } PosemathErrCode; /* Scalar functions */ diff --git a/src/rtapi/uspace_common.h b/src/rtapi/uspace_common.h index 16805e212b4..2da17b4f9ef 100644 --- a/src/rtapi/uspace_common.h +++ b/src/rtapi/uspace_common.h @@ -20,6 +20,7 @@ #include #include #include +#include static int msg_level = RTAPI_MSG_ERR; /* message printing level */ diff --git a/tests/motion-logger/basic/expected.builtin-startup.in b/tests/motion-logger/basic/expected.builtin-startup.in index c67ef0154bb..79b4144a6b9 100644 --- a/tests/motion-logger/basic/expected.builtin-startup.in +++ b/tests/motion-logger/basic/expected.builtin-startup.in @@ -8,6 +8,7 @@ SET_VEL vel=0.000000, ini_maxvel=1.200000 SET_VEL_LIMIT vel=999999999999999967336168804116691273849533185806555472917961779471295845921727862608739868455469056.000000 SET_ACC acc=999999999999999967336168804116691273849533185806555472917961779471295845921727862608739868455469056.000000 SETUP_ARC_BLENDS +SETUP_CONSISTENCY_CHECKS enabled=0, position_drift=0.000100 SET_MAX_FEED_OVERRIDE 1.000000 SET_WORLD_HOME x=0.000000, y=0.000000, z=0.000000, a=0.000000, b=0.000000, c=0.000000, u=0.000000, v=0.000000, w=0.000000 SET_BACKLASH joint=0, backlash=0.000000 diff --git a/tests/motion-logger/mountaindew/expected.motion-logger b/tests/motion-logger/mountaindew/expected.motion-logger index de9e2a505a6..61b08859daf 100644 --- a/tests/motion-logger/mountaindew/expected.motion-logger +++ b/tests/motion-logger/mountaindew/expected.motion-logger @@ -3,6 +3,7 @@ SET_VEL vel=0.000000, ini_maxvel=120.000000 SET_VEL_LIMIT vel=999999999999999967336168804116691273849533185806555472917961779471295845921727862608739868455469056.000000 SET_ACC acc=999999999999999967336168804116691273849533185806555472917961779471295845921727862608739868455469056.000000 SETUP_ARC_BLENDS +SETUP_CONSISTENCY_CHECKS enabled=0, position_drift=0.000100 SET_MAX_FEED_OVERRIDE 1.000000 SET_WORLD_HOME x=0.000000, y=0.000000, z=0.000000, a=0.000000, b=0.000000, c=0.000000, u=0.000000, v=0.000000, w=0.000000 SET_BACKLASH joint=0, backlash=0.000000 diff --git a/tests/trajectory-planner/batch-test/.gitignore b/tests/trajectory-planner/batch-test/.gitignore new file mode 100644 index 00000000000..47e12288fc1 --- /dev/null +++ b/tests/trajectory-planner/batch-test/.gitignore @@ -0,0 +1,5 @@ +.idea +sim.var +axis*csv +test*.log +generated diff --git a/tests/trajectory-planner/batch-test/batch-test.ini b/tests/trajectory-planner/batch-test/batch-test.ini new file mode 100644 index 00000000000..a01f5158ce3 --- /dev/null +++ b/tests/trajectory-planner/batch-test/batch-test.ini @@ -0,0 +1,7 @@ +#INCLUDE ../common-config/mill_XYZA_base.ini + +# Choose the test program to run as the "display" which +# runs a headless instance, to test everything downstream of the GUI +[DISPLAY] +DISPLAY = ./headless_batch_test.py + diff --git a/tests/trajectory-planner/batch-test/checkresult b/tests/trajectory-planner/batch-test/checkresult new file mode 100755 index 00000000000..e534a8cf61d --- /dev/null +++ b/tests/trajectory-planner/batch-test/checkresult @@ -0,0 +1,3 @@ +#!/usr/bin/env bash +# Passthrough since the batch test catches errors as it reports them +exit 0 \ No newline at end of file diff --git a/tests/trajectory-planner/batch-test/headless_batch_test.py b/tests/trajectory-planner/batch-test/headless_batch_test.py new file mode 100755 index 00000000000..f082229238f --- /dev/null +++ b/tests/trajectory-planner/batch-test/headless_batch_test.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python +""" +Based on the control script in m61_test +""" + +# KLUDGE bake-in the path to common configs folder +import sys +sys.path.append('../common-config') + +import linuxcnc +from linuxcnc_control import LinuxcncControl + +from os import listdir +from os.path import isfile, join +import re +import glob + + +def find_test_nc_files(testpath='../nc_files', show=False): + files = [join(testpath, f) for f in listdir(testpath) if isfile(join(testpath, f)) and re.search('.ngc$|.nc$|.TAP$', f) is not None] + if show: + for fname in files: + print fname + return files + + +if __name__ == '__main__': + test_folders = glob.glob(join('nc_files', '*')) + + test_files = [] + + for f in test_folders: + test_files.extend(find_test_nc_files(f, True)) + + e = LinuxcncControl() + + e.do_startup(need_home=True) + + error_list = [] + for f in test_files: + e.log(f, 'info', 'loading program') + e.set_mode(linuxcnc.MODE_AUTO) + e.open_program(f) + e.set_mode(linuxcnc.MODE_AUTO) + e.run_full_program() + if not e.wait_on_program(f): + e.do_startup(need_home=False) + + # KLUDGE just in case any others creep in + e.flush_errors() + sys.exit(e.has_error()) diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/arc_maxvel_xy.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-feed/arc_maxvel_xy.ngc similarity index 77% rename from tests/trajectory-planner/circular-arcs/nc_files/arc-feed/arc_maxvel_xy.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-feed/arc_maxvel_xy.ngc index 1f292bd1fd6..22569c681fe 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/arc_maxvel_xy.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/arc-feed/arc_maxvel_xy.ngc @@ -1,9 +1,8 @@ -G20 G90 +o call F100000 G61 G0 X0 Y0 Z0 G1 X0 Y0.1 -G17 G2 Y-.1 J-0.1 Z5 I.5 P4 G0 X0 Y0 Z0 M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/arc_time.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-feed/arc_time.ngc similarity index 91% rename from tests/trajectory-planner/circular-arcs/nc_files/arc-feed/arc_time.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-feed/arc_time.ngc index 15910d7c6c6..fc249c5e082 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/arc_time.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/arc-feed/arc_time.ngc @@ -1,4 +1,4 @@ -G21 G90 +o call F100000 G0 X0 Y0 Z0 G1 X0 Y2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/basic_xy.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-feed/basic_xy.ngc similarity index 82% rename from tests/trajectory-planner/circular-arcs/nc_files/arc-feed/basic_xy.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-feed/basic_xy.ngc index a020e73e968..7a7e89f194e 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/basic_xy.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/arc-feed/basic_xy.ngc @@ -1,10 +1,9 @@ +o call (Basic ARC_FEED test using canon default units) -G21 G90 -G10 L2 P0 R0 +G61 F1000 G0 X0 Y0 Z0 G1 X10 -G17 G3 Y-10 R10 (270 deg. arc CCW) G0 X0 Y0 Z0 M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_xy.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-feed/full_xy.ngc similarity index 89% rename from tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_xy.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-feed/full_xy.ngc index 5b6b8a5fa51..57cb5a7dc13 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_xy.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/arc-feed/full_xy.ngc @@ -1,12 +1,12 @@ (Basic ARC_FEED test using english to test unit conv) -G20 G90 +o call +G61 (Arbitrary rotation of current csys) G10 L2 P0 X-2 Y1 R60 F1000 G0 X0 Y0 Z0 G1 X1.5 Y1.5 G1 X1.5 Y.5 -G17 G3 X.5 Y1.5 J1 (270 deg. arc CCW) G1 X1.5 Y1.5 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_xz.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-feed/full_xz.ngc similarity index 93% rename from tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_xz.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-feed/full_xz.ngc index ceda118f58c..11466e88416 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_xz.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/arc-feed/full_xz.ngc @@ -1,5 +1,5 @@ +o call (Basic ARC_FEED test using english to test unit conv) -G20 G90 (Force exact path to ignore blend effects) G61 (Arbitrary rotation of current csys) diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_yz.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-feed/full_yz.ngc similarity index 92% rename from tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_yz.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-feed/full_yz.ngc index 41141e78a43..5bf62916e1e 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_yz.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/arc-feed/full_yz.ngc @@ -1,5 +1,5 @@ +o call (Basic ARC_FEED test using english to test unit conv) -G20 G90 (Force exact path to ignore blend effects) G61 (Arbitrary rotation of current csys) diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-feed/optimal_circle_acc_ratio.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-feed/optimal_circle_acc_ratio.ngc new file mode 100644 index 00000000000..ae94957f276 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-feed/optimal_circle_acc_ratio.ngc @@ -0,0 +1,19 @@ +o call +(Disable NCD to force tangent segments to be sent separately) +G0 X0 Y0 Z0 +G91 +G2 Y-10 R5 F1000 +G3 Y-2 R1 +G2 Y-1 R.5 +G3 Y-.2 R.1 +G2 Y-.1 R.05 +G3 Y-.02 Z.5 R.01 +G3 Y+.02 Z.5 R.01 +G3 Y-.02 Z.5 R.01 +G1 X2 +G90 +G0 X0 +Y0 + +M5 +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/simple_arc2.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-feed/simple_arc2.ngc similarity index 83% rename from tests/trajectory-planner/circular-arcs/nc_files/arc-feed/simple_arc2.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-feed/simple_arc2.ngc index f906f5a6ecf..eaec8cd70bc 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/simple_arc2.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/arc-feed/simple_arc2.ngc @@ -1,4 +1,4 @@ -G21 G90 +o call F1000 G0 X0 Y0 Z0 G1 X10 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/simple_arc3.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-feed/simple_arc3.ngc similarity index 75% rename from tests/trajectory-planner/circular-arcs/nc_files/arc-feed/simple_arc3.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-feed/simple_arc3.ngc index 3f96ae2b924..019c57903bc 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/simple_arc3.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/arc-feed/simple_arc3.ngc @@ -1,4 +1,4 @@ -G21 G90 +o call F1000 G0 X0 Y0 Z0 G1 X0 Y10 diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-concave-convex.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-concave-convex.ngc new file mode 100644 index 00000000000..1ca8428f31a --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-concave-convex.ngc @@ -0,0 +1,13 @@ +(Intersection between two arcs where 1st is concave, 2nd is convex wrt intersection) +o call +F1000 +G0 X0 Y0 +Z0 +G1 X.1 +G2 X.2 R.1 +G3 X.3 R.2 +G1 X.8 +G1 Y.2 +X0 +Y0 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex-concave.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex-concave.ngc new file mode 100644 index 00000000000..5dce8b71164 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex-concave.ngc @@ -0,0 +1,13 @@ +(Intersection between two arcs, concave concave, tests arc blend creation) +o call +F1000 +G0 X0 Y0 +Z0 +G1 X.1 +G3 X.2 R.1 +G2 X.3 R.2 +G1 X.8 +G1 Y.2 +X0 +Y0 +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex-convex.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex-convex.ngc similarity index 73% rename from tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex-convex.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex-convex.ngc index 5c0067bacfa..006375c8834 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex-convex.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex-convex.ngc @@ -1,4 +1,4 @@ -G90 G20 G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex.ngc similarity index 76% rename from tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex.ngc index eca82217ad6..022c4b24d83 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex.ngc @@ -1,4 +1,4 @@ -G90 G20 G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-neartangent.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-neartangent.ngc similarity index 77% rename from tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-neartangent.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-neartangent.ngc index d31b544c5ff..b5f68a270cd 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-neartangent.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-neartangent.ngc @@ -1,4 +1,4 @@ -G90 G20 G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-planeswitch.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-planeswitch.ngc similarity index 80% rename from tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-planeswitch.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-planeswitch.ngc index 53f9f1706e7..a25e7d1ba89 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-planeswitch.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-planeswitch.ngc @@ -1,4 +1,4 @@ -G90 G20 G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-sawtooth.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-sawtooth.ngc similarity index 64% rename from tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-sawtooth.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-sawtooth.ngc index 9f5966af77d..d8530b78867 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-sawtooth.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-sawtooth.ngc @@ -1,5 +1,4 @@ -G90 G20 -G64 +o call F1000 G0 X0 Y0 Z0 @@ -35,22 +34,5 @@ G3 X2.7 Y-0.1 R0.2 G2 X2.8 Y0.1 R0.2 G3 X2.9 Y-0.1 R0.2 G2 X3 Y0.1 R0.2 -G3 X3.1 Y-0.1 R0.2 -G2 X3.2 Y0.1 R0.2 -G3 X3.3 Y-0.1 R0.2 -G2 X3.4 Y0.1 R0.2 -G3 X3.5 Y-0.1 R0.2 -G2 X3.6 Y0.1 R0.2 -G3 X3.7 Y-0.1 R0.2 -G2 X3.8 Y0.1 R0.2 -G3 X3.9 Y-0.1 R0.2 -G2 X4 Y0.1 R0.2 -G3 X4.1 Y-0.1 R0.2 -G2 X4.2 Y0.1 R0.2 -G3 X4.3 Y-0.1 R0.2 -G2 X4.4 Y0.1 R0.2 -G3 X4.5 Y-0.1 R0.2 -G2 X4.6 Y0.1 R0.2 -G3 X4.7 Y-0.1 R0.2 +G0 X0 Y0 M2 - diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-test.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-test.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-test.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-test.ngc diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-large-radii.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-large-radii.ngc new file mode 100644 index 00000000000..cdf4fb777ce --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-large-radii.ngc @@ -0,0 +1,13 @@ +o call +G64 P0 Q0 +F1000 +G0 X0 Y0 +Z0 +G1 X1 +G2 X2 R1 +G2 X3 R2 +G1 X4 +G1 Y2 +X0 +Y0 +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-terminal.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-terminal.ngc similarity index 80% rename from tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-terminal.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-terminal.ngc index adb38ff00cf..ccb6e51eb32 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-terminal.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-terminal.ngc @@ -1,5 +1,4 @@ -G20 G90 -G64 +o call F1000 G0 X0 Y0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex-concave.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-with-spiral.ngc similarity index 54% rename from tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex-concave.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-with-spiral.ngc index 5c6e86b0bb8..021162ebb8f 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex-concave.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-with-spiral.ngc @@ -2,11 +2,9 @@ G90 G20 G64 F1000 G0 X0 Y0 Z0 +G1 X-.1 G1 X.1 G3 X.2 R.1 -G2 X.3 R.2 -G1 X.8 -G1 Y.2 -X0 -Y0 +G2 X.3 Y-.1 I.0503535 J-.05 +G1 X.4 M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/line-arc-burnin.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/line-arc-burnin.ngc new file mode 100644 index 00000000000..01dd5fff6ab --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/line-arc-burnin.ngc @@ -0,0 +1,47 @@ +o call +F1000 +G0 X0 Y0 +Z0 +G1 X0.0 +G1 X0.1 +G2 X0.2 R0.1 +G1 X0.3 +G3 X0.4 R0.1 +G1 X0.5 +G2 X0.6 R0.1 +G1 X0.7 +G3 X0.8 R0.1 +G1 X0.9 +G2 X1.0 R0.1 +G1 X1.1 +G3 X1.2 R0.1 +G1 X1.3 +G2 X1.4 R0.1 +G1 X1.5 +G3 X1.6 R0.1 +G1 X1.7 +G2 X1.8 R0.1 +G1 X1.9 +G3 X2.0 R0.1 +G1 X2.1 +G2 X2.2 R0.1 +G1 X2.3 +G3 X2.4 R0.1 +G1 X2.5 +G2 X2.6 R0.1 +G1 X2.7 +G3 X2.8 R0.1 +G1 X2.9 +G2 X3.0 R0.1 +G1 X3.1 +G3 X3.2 R0.1 +G1 X3.3 +G2 X3.4 R0.1 +G1 X3.5 +G3 X3.6 R0.1 +G1 X3.7 +G2 X3.8 R0.1 +G1 X3.9 +G3 X4.0 R0.1 +G0 X0 Y0 Z0 +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-convex.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc-convex.ngc similarity index 63% rename from tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-convex.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc-convex.ngc index 8910b6202fc..460a43cd46c 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-convex.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc-convex.ngc @@ -1,6 +1,4 @@ -G20 G17 G40 -G90 G91.1 -G64 (Q0.001) +o call F999.0000 G0 X0 Y0 Z0 G1 X-1 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-end-overrun.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc-end-overrun.ngc similarity index 95% rename from tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-end-overrun.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc-end-overrun.ngc index 59c09770daa..f03391768b8 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-end-overrun.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc-end-overrun.ngc @@ -1,8 +1,5 @@ -G20 +o call G0 Z0.0120 -G17 G40 -G80 G90 G91.1 -G64 (Q0.001) F999.0000 G18 G0 X3.3390 Y4.8580 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc.ngc similarity index 79% rename from tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc.ngc index ee3a4a3d3f2..9845a9948a8 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc.ngc @@ -1,4 +1,4 @@ -G20 G90 +o call G64 P0.0 Q0.0 F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/performance/tort-sam-mach3.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/tort-sam-mach3.ngc similarity index 92% rename from tests/trajectory-planner/circular-arcs/nc_files/performance/tort-sam-mach3.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-intersections/tort-sam-mach3.ngc index c368be598fb..0e5a5b4214d 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/performance/tort-sam-mach3.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/tort-sam-mach3.ngc @@ -1,9 +1,8 @@ -g21 -G64 P0 (Q0.004) +o call G0 X0 Y0 Z20 G0 X5.394910 Y13.495489 Z20.332291 G19 G2 F12700 (225 29) J5.656854 K5.656854 X10.394910 Y26.080547 Z29.989145 G19 G2 (75 14) J-0.258819 K-0.965926 X6.894910 Y26.787653 Z29.282038 G17 G2 (329 285) I-4.330127 J2.500000 X3.858878 Y24.458024 Z31.282038 G0 X0 Y0 Z20 -m30 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/regressions/546_simplfied.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/546_simplfied.ngc new file mode 100644 index 00000000000..278c84e3006 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/546_simplfied.ngc @@ -0,0 +1,21 @@ +G90 +G21 +G17 G64 P0.0001 M3 S3000 +F800.0 +G0 Z2.000 +G0 X0.724 Y12.368 +G1 Z-0.127 +G1 X0.724 Y12.368 +G1 X0.678 Y12.278 +G1 X0.557 Y12.351 +G1 X0.564 Y12.384 +G1 X0.707 Y12.315 +G1 X0.608 Y12.398 +G1 X0.668 Y12.412 +G1 X0.723 Y12.372 +G1 X0.724 Y12.368 +G0 Z2.000 +G0 X0.858 Y12.449 +M5 +M2 + diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/arc_vel_violation2.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/arc_vel_violation2.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/arc_vel_violation2.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/arc_vel_violation2.ngc diff --git a/tests/trajectory-planner/batch-test/nc_files/regressions/blend_arc_simple_pos_test.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/blend_arc_simple_pos_test.ngc new file mode 100644 index 00000000000..ea0fc731703 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/blend_arc_simple_pos_test.ngc @@ -0,0 +1,12 @@ +G20 +G90 +G0 X0 Y0 Z0 +G64 P0.005 Q0.00000 +G91 +G1 X1 F2000 +G1 Y1 +G1 Z1 +G1 Z1 +G90 +M30 + diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/bug424_long.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/bug424_long.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/bug424_long.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/bug424_long.ngc diff --git a/tests/trajectory-planner/batch-test/nc_files/regressions/bug550_short_moves.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/bug550_short_moves.ngc new file mode 100644 index 00000000000..265b0803400 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/bug550_short_moves.ngc @@ -0,0 +1,13 @@ +G0 X0 Y0 A0 +G91 G20 G61 + +G1 X0.1 Y0.1 A-50.0 F100000 +G1 X0.000005 Y0.000005 A50.0 F100000 +G1 X0.0000005 Y0.0000005 A50.0 F100000 +G1 X0.00000005 Y0.00000005 A50.0 F100000 +G1 X0.000000005 Y0.000000005 A50.0 F100000 +G1 X0.1 Y0.1 A.1 F100000 +G90 +G1 X0 Y0 A0 F100000 +G64 +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/circular_tolerance_check.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/circular_tolerance_check.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/circular_tolerance_check.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/circular_tolerance_check.ngc diff --git a/tests/trajectory-planner/batch-test/nc_files/regressions/corner_decel.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/corner_decel.ngc new file mode 100644 index 00000000000..18cb58ec6d3 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/corner_decel.ngc @@ -0,0 +1,12 @@ +G21 G90 G64 G40 G17 +G0 Z3.0 +M3 S10000 +G0 X1.75781 Y0.5 +G1 F100.0 Z-2.0 +G1 F1200.0 X5.95508 Y20.54297 +G1 X10.07031 +G1 X6.58398 Y3.84961 +G1 X16.7832 +G1 X30 +X20 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/regressions/cradek_accel_issue.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/cradek_accel_issue.ngc new file mode 100644 index 00000000000..622d369ddb9 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/cradek_accel_issue.ngc @@ -0,0 +1,9 @@ +G20 G40 G80 G90 G94 G17 +G64 +F999 +G0 X0 Y0 Z0 +G0 X0.2320 Z-0.0771 Y0 +G1 X0.2560 Z-0.1203 +G18 G2 X0.2620 Y0 Z-0.1134 I-0.0025 K0.0082 +G1 X1 +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/feedtest.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/feedtest.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/feedtest.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/feedtest.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/full_reverse.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/full_reverse.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/full_reverse.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/full_reverse.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/machinekit_412.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/machinekit_412.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/machinekit_412.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/machinekit_412.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/maxvel_violation_helical.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/maxvel_violation_helical.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/maxvel_violation_helical.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/maxvel_violation_helical.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/maxvel_violation_helical2.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/maxvel_violation_helical2.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/maxvel_violation_helical2.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/maxvel_violation_helical2.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/maxvel_violation_line.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/maxvel_violation_line.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/maxvel_violation_line.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/maxvel_violation_line.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/maxvel_violation_line2.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/maxvel_violation_line2.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/maxvel_violation_line2.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/maxvel_violation_line2.ngc diff --git a/tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_bump.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_bump.ngc new file mode 100644 index 00000000000..51351807631 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_bump.ngc @@ -0,0 +1,10 @@ +G20 G64 G90 +G61 +G0 X0 Y0 Z0 +G64 +F9999 +G1 X4 +G1 X4.25 Y0.25 +G1 X4.5 Y0 +G1 X8 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_test.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_test.ngc new file mode 100644 index 00000000000..80eb16b2cdd --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_test.ngc @@ -0,0 +1,9 @@ +(Test code for new blend velocity function) +G20 G90 G61 +G0 X0 Y0 Z0 +G64 +G1 X5 Y5 F9999 +G1 Y6 +G1 X0 Y11 +G0 X0 Y0 Z0 +M30 diff --git a/tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_varying.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_varying.ngc new file mode 100644 index 00000000000..825148349eb --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_varying.ngc @@ -0,0 +1,12 @@ +(Test code for new blend velocity function) +G20 G90 G64 +G0 X0 Y0 Z0 +G64 +G1 X5 Y5 F9999 +G1 Y6 +G1 X0 Y11 +G1 Y10 +G1 X-.5 Y9.5 +G1 Y5 +G0 X0 Y0 Z0 +M30 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/quick_arc.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/quick_arc.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/quick_arc.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/quick_arc.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/rickg-arcs.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/rickg-arcs.ngc similarity index 89% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/rickg-arcs.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/rickg-arcs.ngc index 5bcf9f952a0..b963c958f09 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/rickg-arcs.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/rickg-arcs.ngc @@ -1,4 +1,4 @@ -G90 G20 +o call G64 P0.001 Q0.001 F100 G0 X2 Y0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam-master-violation.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/sam-master-violation.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam-master-violation.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/sam-master-violation.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_vel_violation.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/sam_vel_violation.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_vel_violation.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/sam_vel_violation.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_violation1.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/sam_violation1.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_violation1.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/sam_violation1.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_violation2.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/sam_violation2.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_violation2.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/sam_violation2.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_violation2_verbatim.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/sam_violation2_verbatim.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_violation2_verbatim.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/sam_violation2_verbatim.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_violation3.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/sam_violation3.ngc similarity index 86% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_violation3.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/sam_violation3.ngc index 869255fb836..5e11d466515 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_violation3.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/sam_violation3.ngc @@ -1,4 +1,5 @@ -G21 G64p.127 +o call +G64p.127 G0 Z0 S6000 M4 G1 X-11.952 Y112.062 F10000000 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_zero_length_move.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/sam_zero_length_move.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_zero_length_move.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/sam_zero_length_move.ngc diff --git a/tests/trajectory-planner/batch-test/nc_files/regressions/split-cycle-check.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/split-cycle-check.ngc new file mode 100644 index 00000000000..9dc4b93cd2a --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/split-cycle-check.ngc @@ -0,0 +1,515 @@ +G20 +G0 Z0.0120 +G17 G40 +G80 G90 G94 +S1000 M3 +(G04 P3) +G64 +F999.0000 +G18 +F999.0000 +G0 X3.3390 Y0 +G1 Z-0.0319 +G3 X3.214000 Z-0.156892 R0.125000 +F999.0000 +G1 X3.2100 Z-0.1530 + X3.2040 Z-0.1539 + X3.2000 Z-0.1569 + X3.1940 Z-0.1539 + X3.1840 Z-0.1588 + X3.1680 Z-0.1549 + X3.1620 Z-0.1565 + X3.1380 Z-0.1539 + X3.1280 Z-0.1589 + X3.1260 Z-0.1575 + X3.1240 Z-0.1520 + X3.1200 Z-0.1510 + X3.1160 Z-0.1549 + X3.1060 Z-0.1588 +G3 X3.0940 Z-0.1540 I0.0013 K0.0205 +G1 X3.0860 Z-0.1556 + X3.0800 Z-0.1539 + X3.0760 Z-0.1569 + X3.0700 Z-0.1539 + X3.0660 Z-0.1549 + X3.0620 Z-0.1529 + X3.0560 Z-0.1539 + X3.0520 Z-0.1520 + X3.0440 Z-0.1520 + X3.0400 Z-0.1549 + X3.0360 Z-0.1556 + X3.0280 Z-0.1549 + X3.0240 Z-0.1529 + X3.0040 Z-0.1549 + X2.9900 Z-0.1490 + X2.9860 Z-0.1529 + X2.9800 Z-0.1510 + X2.9740 Z-0.1546 + X2.9380 Z-0.1510 + X2.9300 Z-0.1526 + X2.9220 Z-0.1520 + X2.9180 Z-0.1559 + X2.9100 Z-0.1556 + X2.9080 Z-0.1536 + X2.9040 Z-0.1530 + X2.9000 Z-0.1559 + X2.8940 Z-0.1529 + X2.8860 Z-0.1520 + X2.8780 Z-0.1546 + X2.8760 Z-0.1569 + X2.8640 +G2 X2.8540 Z-0.1510 I-0.0110 K-0.0073 +G1 X2.8500 Z-0.1516 + X2.8480 Z-0.1546 + X2.8320 Z-0.1539 + X2.8220 Z-0.1556 + X2.8200 Z-0.1536 + X2.8160 Z-0.1530 + X2.8140 Z-0.1536 + X2.8100 Z-0.1598 + X2.8060 Z-0.1526 + X2.8040 Z-0.1520 + X2.8000 Z-0.1526 + X2.7980 Z-0.1556 + X2.7960 Z-0.1559 + X2.7920 Z-0.1529 + X2.7740 Z-0.1559 + X2.7700 Z-0.1529 + X2.7600 Z-0.1549 + X2.7560 Z-0.1490 + X2.7520 Z-0.1480 + X2.7500 Z-0.1490 + X2.7480 Z-0.1539 + X2.7420 Z-0.1559 + X2.7200 Z-0.1539 + X2.7040 Z-0.1575 + X2.6980 Z-0.1529 + X2.6920 Z-0.1549 + X2.6820 Z-0.1530 + X2.6740 Z-0.1559 + X2.6600 Z-0.1529 + X2.6560 Z-0.1579 + X2.6540 Z-0.1556 + X2.6480 Z-0.1546 + X2.6460 Z-0.1487 + X2.6440 Z-0.1481 + X2.6400 Z-0.1487 + X2.6360 Z-0.1540 + X2.6300 Z-0.1529 + X2.6220 Z-0.1559 + X2.6140 Z-0.1539 + X2.6120 Z-0.1497 + X2.6080 Z-0.1490 + X2.6040 Z-0.1530 + X2.6000 Z-0.1520 + X2.5960 Z-0.1559 + X2.5920 Z-0.1559 + X2.5900 Z-0.1500 + X2.5860 Z-0.1471 + X2.5820 Z-0.1481 + X2.5800 Z-0.1530 + X2.5760 Z-0.1539 + X2.5720 Z-0.1530 + X2.5700 Z-0.1471 + X2.5680 Z-0.1461 + X2.5640 Z-0.1471 + X2.5600 Z-0.1539 + X2.5540 Z-0.1540 + X2.5460 Z-0.1510 + X2.5420 Z-0.1546 + X2.5360 Z-0.1530 + X2.5320 Z-0.1549 + X2.5260 Z-0.1520 + X2.5140 Z-0.1529 + X2.5080 Z-0.1510 + X2.5040 Z-0.1526 + X2.4920 Z-0.1490 + X2.4760 Z-0.1510 + X2.4640 Z-0.1480 + X2.4580 Z-0.1510 + X2.4480 Z-0.1510 + X2.4420 Z-0.1536 + X2.4380 Z-0.1487 + X2.4340 Z-0.1480 + X2.4240 Z-0.1529 + X2.4180 Z-0.1490 +G3 X2.4020 Z-0.1529 I-0.0164 K0.0321 +G1 X2.3860 Z-0.1490 + X2.3760 Z-0.1516 + X2.3700 Z-0.1500 + X2.3660 Z-0.1529 + X2.3620 + X2.3580 Z-0.1510 + X2.3540 Z-0.1520 + X2.3500 Z-0.1510 + X2.3460 Z-0.1480 + X2.3400 Z-0.1500 + X2.3380 Z-0.1526 + X2.3360 Z-0.1497 + X2.3260 Z-0.1500 + X2.3200 Z-0.1461 + X2.3140 Z-0.1481 + X2.3060 Z-0.1461 + X2.3020 Z-0.1500 + X2.2800 Z-0.1497 + X2.2760 Z-0.1490 + X2.2720 Z-0.1461 + X2.2680 Z-0.1467 + X2.2640 Z-0.1441 + X2.2560 Z-0.1438 + X2.2520 Z-0.1392 + X2.2460 Z-0.1412 + X2.2420 Z-0.1402 + X2.2320 Z-0.1412 + X2.2260 Z-0.1373 + X2.2140 + X2.2080 Z-0.1353 + X2.2000 Z-0.1369 + X2.1940 Z-0.1333 + X2.1880 Z-0.1363 + X2.1740 Z-0.1373 + X2.1680 Z-0.1343 + X2.1600 Z-0.1363 + X2.1520 Z-0.1334 + X2.1480 Z-0.1340 + X2.1440 Z-0.1314 + X2.1380 Z-0.1340 + X2.1280 Z-0.1343 + X2.1220 Z-0.1324 + X2.1160 Z-0.1340 + X2.1060 Z-0.1324 + X2.0940 Z-0.1350 + X2.0920 Z-0.1324 + X2.0880 Z-0.1314 + X2.0820 Z-0.1343 + X2.0780 Z-0.1333 + X2.0660 Z-0.1369 + X2.0600 Z-0.1324 + X2.0540 Z-0.1350 + X2.0500 Z-0.1333 + X2.0400 +G2 X2.0340 Z-0.1275 I-0.0061 K-0.0003 +G1 X2.0300 Z-0.1281 + X2.0260 Z-0.1359 + X2.0180 + X2.0040 Z-0.1333 + X1.9980 Z-0.1304 + X1.9900 Z-0.1324 + X1.9860 Z-0.1353 + X1.9740 Z-0.1324 + X1.9680 Z-0.1340 + X1.9560 Z-0.1314 + X1.9520 + X1.9480 Z-0.1343 + X1.9340 Z-0.1353 + X1.9320 Z-0.1304 + X1.9300 Z-0.1294 + X1.9260 Z-0.1304 + X1.9240 Z-0.1333 + X1.9140 Z-0.1334 + X1.9100 Z-0.1350 + X1.9020 Z-0.1314 + X1.8940 Z-0.1324 + X1.8900 Z-0.1285 + X1.8840 + X1.8800 Z-0.1314 + X1.8740 Z-0.1324 + X1.8660 Z-0.1304 + X1.8600 Z-0.1324 + X1.8440 Z-0.1314 + X1.8400 Z-0.1294 + X1.8340 Z-0.1324 + X1.8280 Z-0.1304 + X1.8200 Z-0.1314 + X1.8180 Z-0.1304 + X1.7820 Z-0.1324 + X1.7760 Z-0.1304 +G2 X1.7640 Z-0.1343 I0.0005 K-0.0218 +G1 X1.7460 Z-0.1304 + X1.7420 + X1.7360 Z-0.1330 + X1.7300 Z-0.1294 + X1.7260 Z-0.1301 + X1.7240 Z-0.1334 + X1.7200 Z-0.1333 + X1.7180 Z-0.1304 + X1.7140 Z-0.1294 +G3 X1.7040 Z-0.1343 I-0.0122 K0.0123 +G1 X1.7000 Z-0.1314 + X1.6800 + X1.6760 Z-0.1275 + X1.6700 Z-0.1304 + X1.6560 Z-0.1275 + X1.6520 Z-0.1324 + X1.6340 Z-0.1320 + X1.6280 Z-0.1275 + X1.6120 Z-0.1314 + X1.6000 Z-0.1304 + X1.5960 Z-0.1324 + X1.5800 Z-0.1284 + X1.5680 Z-0.1320 + X1.5580 Z-0.1324 + X1.5540 Z-0.1304 + X1.5460 Z-0.1314 + X1.5420 Z-0.1294 + X1.5340 Z-0.1310 + X1.5280 Z-0.1275 + X1.5240 + X1.5200 Z-0.1314 + X1.5100 Z-0.1284 + X1.5020 Z-0.1314 + X1.4960 Z-0.1310 + X1.4900 Z-0.1284 + X1.4860 Z-0.1294 + X1.4840 Z-0.1271 + X1.4800 Z-0.1265 + X1.4760 Z-0.1304 + X1.4620 + X1.4520 Z-0.1275 + X1.4480 Z-0.1304 + X1.4340 Z-0.1294 + X1.4180 Z-0.1324 + X1.4120 Z-0.1304 + X1.3980 Z-0.1294 + X1.3960 Z-0.1271 + X1.3920 Z-0.1265 + X1.3900 Z-0.1271 + X1.3880 Z-0.1310 + X1.3820 Z-0.1324 + X1.3740 Z-0.1304 + X1.3600 Z-0.1314 + X1.3560 Z-0.1333 + X1.3500 Z-0.1304 + X1.3400 Z-0.1333 + X1.3340 Z-0.1304 + X1.3300 Z-0.1353 + X1.3220 Z-0.1324 + X1.3100 Z-0.1412 + X1.3040 Z-0.1418 + X1.2980 Z-0.1500 + X1.2940 Z-0.1507 + X1.2920 Z-0.1546 + X1.2860 Z-0.1556 + X1.2840 Z-0.1585 + X1.2780 + X1.2760 Z-0.1647 + X1.2720 Z-0.1654 + X1.2700 Z-0.1722 + X1.2640 Z-0.1765 + X1.2580 Z-0.1755 + X1.2520 Z-0.1771 + X1.2500 Z-0.1732 + X1.2460 Z-0.1726 + X1.2340 Z-0.1801 + X1.2280 Z-0.1771 +G3 X1.2220 Z-0.1657 I0.0155 K0.0154 +G1 X1.2200 Z-0.1647 + X1.2140 Z-0.1667 + X1.2100 Z-0.1637 + X1.2020 Z-0.1647 + X1.1980 Z-0.1627 + X1.1940 Z-0.1638 + X1.1840 Z-0.1628 +G2 X1.1760 Z-0.1706 I0.0021 K-0.0101 +G1 X1.1720 Z-0.1676 + X1.1660 Z-0.1696 +G3 X1.1560 Z-0.1624 I0.0019 K0.0132 +G1 X1.1520 Z-0.1618 + X1.1480 Z-0.1588 + X1.1440 Z-0.1598 + X1.1380 Z-0.1578 + X1.1320 Z-0.1627 + X1.1280 Z-0.1638 + X1.1220 Z-0.1706 + X1.1180 Z-0.1722 + X1.1160 Z-0.1703 + X1.1120 Z-0.1696 + X1.1060 Z-0.1736 + X1.1040 Z-0.1687 + X1.0940 Z-0.1667 + X1.0900 Z-0.1638 + X1.0820 Z-0.1657 + X1.0780 Z-0.1686 +G2 X1.0640 Z-0.1736 I0.0011 K-0.0254 +G1 X1.0620 Z-0.1693 + X1.0600 Z-0.1687 + X1.0540 Z-0.1686 + X1.0500 Z-0.1706 + X1.0320 Z-0.1676 + X1.0240 Z-0.1761 + X1.0220 Z-0.1804 + X1.0200 Z-0.1810 + X1.0140 Z-0.1654 + X1.0120 Z-0.1647 + X1.0060 Z-0.1667 + X1.0000 Z-0.1647 + X0.9960 Z-0.1657 + X0.9920 Z-0.1608 + X0.9860 + X0.9820 Z-0.1657 + X0.9800 Z-0.1742 + X0.9740 Z-0.1775 + X0.9660 Z-0.1677 + X0.9620 Z-0.1657 + X0.9560 Z-0.1667 + X0.9520 Z-0.1657 + X0.9500 Z-0.1624 + X0.9460 Z-0.1618 + X0.9400 Z-0.1654 + X0.9360 Z-0.1647 +G3 X0.9260 Z-0.1687 I-0.0103 K0.0116 +G1 X0.9220 Z-0.1676 + X0.9180 Z-0.1687 +G2 X0.9100 Z-0.1863 I0.0719 K-0.0433 +G1 X0.9040 Z-0.1869 + X0.9020 Z-0.1889 + X0.8960 Z-0.1820 + X0.8860 Z-0.1804 + X0.8820 Z-0.1814 + X0.8720 Z-0.1703 + X0.8600 Z-0.1706 + X0.8500 Z-0.1608 + X0.8460 Z-0.1618 + X0.8400 Z-0.1608 + X0.8360 Z-0.1654 + X0.8320 Z-0.1647 + X0.8300 Z-0.1614 + X0.8240 + X0.8140 Z-0.1771 + X0.8100 Z-0.1794 + X0.8040 Z-0.1775 +G2 X0.7920 Z-0.1686 I-0.0196 K-0.0141 +G1 X0.7840 Z-0.1706 + X0.7820 Z-0.1667 + X0.7740 Z-0.1657 + X0.7700 Z-0.1627 + X0.7620 Z-0.1654 + X0.7520 Z-0.1608 + X0.7460 Z-0.1624 + X0.7360 Z-0.1588 + X0.7200 Z-0.1628 + X0.7180 Z-0.1716 + X0.7160 Z-0.1725 + X0.7140 Z-0.1693 + X0.7060 + X0.7000 Z-0.1647 + X0.6900 Z-0.1657 + X0.6880 Z-0.1608 + X0.6820 Z-0.1598 + X0.6800 Z-0.1608 + X0.6780 Z-0.1673 + X0.6720 Z-0.1732 + X0.6640 Z-0.1735 + X0.6560 Z-0.1703 + X0.6540 Z-0.1667 + X0.6480 Z-0.1657 + X0.6440 Z-0.1627 + X0.6280 Z-0.1686 + X0.6240 Z-0.1657 + X0.6180 Z-0.1667 + X0.6140 Z-0.1637 + X0.6060 Z-0.1638 + X0.6020 Z-0.1608 + X0.5980 Z-0.1614 + X0.5960 Z-0.1579 + X0.5900 Z-0.1559 + X0.5860 Z-0.1608 + X0.5780 Z-0.1634 + X0.5740 Z-0.1608 + X0.5620 Z-0.1598 + X0.5480 Z-0.1706 + X0.5360 Z-0.1745 + X0.5340 Z-0.1735 + X0.5300 Z-0.1745 + X0.5260 Z-0.1824 + X0.5240 + X0.5220 Z-0.1801 + X0.5180 Z-0.1791 + X0.5160 Z-0.1752 + X0.5120 Z-0.1745 + X0.5080 Z-0.1765 + X0.5020 Z-0.1712 + X0.4940 Z-0.1696 + X0.4900 Z-0.1667 + X0.4840 Z-0.1676 + X0.4800 Z-0.1634 + X0.4740 + X0.4700 Z-0.1608 + X0.4620 Z-0.1598 + X0.4540 Z-0.1618 + X0.4500 Z-0.1578 + X0.4340 Z-0.1605 + X0.4300 Z-0.1683 + X0.4260 Z-0.1677 + X0.4220 Z-0.1706 + X0.4160 Z-0.1696 + X0.4140 Z-0.1706 + X0.4120 Z-0.1755 + X0.4080 + X0.4040 Z-0.1726 + X0.4000 Z-0.1732 + X0.3980 Z-0.1761 + X0.3860 Z-0.1736 + X0.3780 Z-0.1742 + X0.3740 Z-0.1732 + X0.3720 Z-0.1677 + X0.3680 Z-0.1657 + X0.3600 Z-0.1676 + X0.3560 Z-0.1667 + X0.3520 Z-0.1696 + X0.3480 Z-0.1696 + X0.3380 Z-0.1676 + X0.3260 Z-0.1677 + X0.3200 Z-0.1627 + X0.3140 Z-0.1638 + X0.3080 Z-0.1618 +G3 X0.2900 Z-0.1663 I-0.0180 K0.0331 +G1 X0.2840 Z-0.1618 + X0.2780 Z-0.1628 + X0.2720 Z-0.1608 + X0.2660 Z-0.1647 +G3 X0.2540 Z-0.1589 I0.0049 K0.0252 +G1 X0.2500 Z-0.1595 + X0.2480 Z-0.1618 + X0.2380 Z-0.1588 + X0.2320 Z-0.1598 + X0.2280 Z-0.1578 + X0.2240 Z-0.1589 + X0.2200 Z-0.1732 + X0.2180 +G3 X0.2140 Z-0.1569 I0.0473 K0.0202 +G1 X0.2120 Z-0.1559 + X0.2080 Z-0.1471 + X0.2060 Z-0.1461 + X0.2020 Z-0.1471 + X0.1980 Z-0.1441 + X0.1940 Z-0.1448 + X0.1900 Z-0.1487 + X0.1860 Z-0.1461 + X0.1720 Z-0.1471 + X0.1680 Z-0.1441 + X0.1580 Z-0.1458 + X0.1520 Z-0.1441 + X0.1460 Z-0.1461 + X0.1400 Z-0.1431 + X0.1320 Z-0.1458 + X0.1180 Z-0.1412 + X0.1100 Z-0.1441 + X0.1060 Z-0.1402 + X0.1000 Z-0.1422 + X0.0880 Z-0.1392 + X0.0820 Z-0.1412 + X0.0740 + X0.0700 Z-0.1451 + X0.0660 Z-0.1431 + X0.0580 Z-0.1448 + X0.0540 Z-0.1432 + X0.0420 Z-0.1441 + X0.0380 Z-0.1422 + X0.0240 Z-0.1441 + X0.0200 Z-0.1402 + X0.0120 Z-0.1408 + X0.0060 Z-0.1373 + X0.0020 Z-0.1379 + X0.0000 Z-0.1431 +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/stellabee-kink-test.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/stellabee-kink-test.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/stellabee-kink-test.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/stellabee-kink-test.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/stellabee-violation-spirals.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/stellabee-violation-spirals.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/stellabee-violation-spirals.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/stellabee-violation-spirals.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/stop-err.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/stop-err.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/stop-err.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/stop-err.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/tangent_lines_smooth.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/tangent_lines_smooth.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/tangent_lines_smooth.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/tangent_lines_smooth.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/tap-discrimant-error-orig.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/tap-discrimant-error-orig.ngc similarity index 98% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/tap-discrimant-error-orig.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/tap-discrimant-error-orig.ngc index 211bc595941..d328d4dfc82 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/tap-discrimant-error-orig.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/tap-discrimant-error-orig.ngc @@ -18,7 +18,7 @@ # = #5 (Spindle RPM) # = #6 (Tool Number) # = #7 (=8 Coolant 8 On 9 Off) - G7 G17 G20 G40 G80 + G17 G20 G40 G80 T# M6 G43 o<053o110> if [# EQ 0] # = [1 / #] diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/tkalcevic_realtime_error.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/tkalcevic_realtime_error.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/tkalcevic_realtime_error.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/tkalcevic_realtime_error.ngc diff --git a/tests/trajectory-planner/batch-test/nc_files/regressions/warnings_g81.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/warnings_g81.ngc new file mode 100644 index 00000000000..faa1c30199d --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/warnings_g81.ngc @@ -0,0 +1,20 @@ +G90 G20 G40 + +G64 P0 Q0 +/G61.1 + +G0 Z1 +G81 X0 Z-1 R1 F100 P2 +G81 X0 Z-1 R1 F100 P2 +G81 X0 Z-1 R1 F100 P2 +G81 X0 Z-1 R1 F100 P2 +G81 X0 Z-1 R1 F100 P2 +G81 X0 Z-1 R1 F100 P2 +G81 X0 Z-1 R1 F100 P2 +G81 X0 Z-1 R1 F100 P2 +G81 X0 Z-1 R1 F100 P2 +X1 +G0 X0 + +M2 + diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/z_jump_tangent_improvement.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/z_jump_tangent_improvement.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/z_jump_tangent_improvement.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/z_jump_tangent_improvement.ngc diff --git a/tests/trajectory-planner/batch-test/nc_files/spindle/g33_blend.ngc b/tests/trajectory-planner/batch-test/nc_files/spindle/g33_blend.ngc new file mode 100644 index 00000000000..40847f81382 --- /dev/null +++ b/tests/trajectory-planner/batch-test/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/batch-test/nc_files/spindle/g33_simple.ngc b/tests/trajectory-planner/batch-test/nc_files/spindle/g33_simple.ngc new file mode 100644 index 00000000000..48add847aaf --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/spindle/g33_simple.ngc @@ -0,0 +1,8 @@ +o call +G0 X1 Z0.1 (rapid to position) +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) +M5 +M2 (end program) diff --git a/tests/trajectory-planner/batch-test/nc_files/spindle/g95_blend.ngc b/tests/trajectory-planner/batch-test/nc_files/spindle/g95_blend.ngc new file mode 100644 index 00000000000..b29e907260f --- /dev/null +++ b/tests/trajectory-planner/batch-test/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 + diff --git a/tests/trajectory-planner/batch-test/nc_files/spindle/rigidtap_test.ngc b/tests/trajectory-planner/batch-test/nc_files/spindle/rigidtap_test.ngc new file mode 100644 index 00000000000..53f9dbb3ad7 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/spindle/rigidtap_test.ngc @@ -0,0 +1,11 @@ +o call +M3 S200 +G0 X1.000 Y1.000 Z0.100 +G33.1 Z-0.750 K0.05 +S500 +G0 X2 Y1 Z0.100 +G33.1 Z-0.750 K0.05 +G0 X3 Y1 Z0.100 +G33.1 Z-0.750 K0.1 +M5 +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/spindle-wait.ngc b/tests/trajectory-planner/batch-test/nc_files/spindle/spindle-wait.ngc similarity index 94% rename from tests/trajectory-planner/circular-arcs/nc_files/quick-tests/spindle-wait.ngc rename to tests/trajectory-planner/batch-test/nc_files/spindle/spindle-wait.ngc index 7991e4ac16c..e0fd5af447c 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/spindle-wait.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/spindle/spindle-wait.ngc @@ -1,6 +1,6 @@ +o call (Test of atspeed) (Setup code) -G20 G90 G64 G17 (Move to standard initial position) G0 x0 y0 z1 (Tangent segments allow speed above "triangle" velocity) diff --git a/tests/trajectory-planner/batch-test/nc_files/spindle/tapping.ngc b/tests/trajectory-planner/batch-test/nc_files/spindle/tapping.ngc new file mode 100644 index 00000000000..c7ec7344c2b --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/spindle/tapping.ngc @@ -0,0 +1,10 @@ +o call +M3 S200 +G0 X1.000 Y1.000 Z0.100 +G33.1 Z-0.750 K0.05 +G0 X2.000 Y1.000 Z0.100 +G33.1 Z-0.750 K0.05 +G0 X3.000 Y1.000 Z0.100 +G33.1 Z-0.750 K0.05 +M5 +M2 diff --git a/tests/trajectory-planner/batch-test/position.blank b/tests/trajectory-planner/batch-test/position.blank new file mode 100644 index 00000000000..9bdc9d4bca4 --- /dev/null +++ b/tests/trajectory-planner/batch-test/position.blank @@ -0,0 +1,9 @@ +0.00000000000000000 +0.00000000000000000 +0.00000000000000000 +0.00000000000000000 +0.00000000000000000 +0.00000000000000000 +0.00000000000000000 +0.00000000000000000 +0.00000000000000000 diff --git a/tests/trajectory-planner/batch-test/print_load_times.py b/tests/trajectory-planner/batch-test/print_load_times.py new file mode 100755 index 00000000000..5c169188f23 --- /dev/null +++ b/tests/trajectory-planner/batch-test/print_load_times.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python +import datetime +import re + +if __name__ == '__main__': + with open('batch-test-err.log') as err_log: + t_old = datetime.datetime(1970, 1, 1) + for l in err_log.readlines(): + dt_str = l.split('|')[0].rstrip() + try: + t = datetime.datetime.strptime(dt_str, "%Y-%m-%d %H:%M:%S.%f") + msg = l.split('|')[1].rstrip() + except ValueError: + continue + + if re.search('loading', l): + # print dt before recalculating (since dt here is for the next program + dt = (t-t_old).total_seconds() + print("{} seconds | {}".format(dt, msg)) + t_old = t diff --git a/tests/trajectory-planner/batch-test/sim.tbl b/tests/trajectory-planner/batch-test/sim.tbl new file mode 120000 index 00000000000..7ad328ea5e4 --- /dev/null +++ b/tests/trajectory-planner/batch-test/sim.tbl @@ -0,0 +1 @@ +../common-config/sim.tbl \ No newline at end of file diff --git a/tests/trajectory-planner/batch-test/test.sh b/tests/trajectory-planner/batch-test/test.sh new file mode 100755 index 00000000000..e027086322b --- /dev/null +++ b/tests/trajectory-planner/batch-test/test.sh @@ -0,0 +1,4 @@ +#!/usr/bin/env bash +cp position.blank position.txt +linuxcnc batch-test.ini 2> batch-test-err.log | tee batch-test-out.log +exit $? diff --git a/tests/trajectory-planner/circular-arcs/.gitignore b/tests/trajectory-planner/circular-arcs/.gitignore new file mode 100644 index 00000000000..47e12288fc1 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/.gitignore @@ -0,0 +1,5 @@ +.idea +sim.var +axis*csv +test*.log +generated diff --git a/tests/trajectory-planner/circular-arcs/README.md b/tests/trajectory-planner/circular-arcs/README.md new file mode 100644 index 00000000000..c4bf3f91c5b --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/README.md @@ -0,0 +1,21 @@ +# Decoding JSON5 logsa + +TP debug log output can be compile-time enabled with the following make arguments: + +``` +make ... EXTRA_DEBUG='-DTP_DEBUG -DTC_DEBUG -DTP_INFO_LOGGING' +``` + +These log files use JSON5 format, a superset of JSON. The reason we use this format is that it makes the debug macros easier to write (leave in trailing commas, and don't have to quote field names). + +An example of how to parse the log files is shown in parse\_tp\_logs.py. To run this python file, you need python 3 installed, along with some dependencies. A virtual environment is the easiest way to do this: + +``` +sudo apt install python3 python3-tk python3-pip python3-virtualenv python3-numpy +mkvirtualenv -p `which python3` lcnc_tp_test +``` + +Once the virtual environment is running, you can install requirements from the config file: +``` +pip install -r tp_log_parsing.cfg +``` diff --git a/tests/trajectory-planner/circular-arcs/build-debug.sh b/tests/trajectory-planner/circular-arcs/build-debug.sh index aee2aad0e97..0ed90ff67f9 100755 --- a/tests/trajectory-planner/circular-arcs/build-debug.sh +++ b/tests/trajectory-planner/circular-arcs/build-debug.sh @@ -2,7 +2,7 @@ cd ../../../src #Ugly way to force rebuild of kinematics, which assumes that tp_debug isn't #used anywhere else... -touch emc/tp/t[cp]*.[ch] +touch emc/tp/tp_debug.h MAKECMD="make -j4 " # Show debug info for each timestep, as well as segment creation #$MAKECMD EXTRA_DEBUG='-DTC_DEBUG -DTP_DEBUG' diff --git a/tests/trajectory-planner/circular-arcs/build-release.sh b/tests/trajectory-planner/circular-arcs/build-release.sh index 02e83500642..5964cccdf76 100755 --- a/tests/trajectory-planner/circular-arcs/build-release.sh +++ b/tests/trajectory-planner/circular-arcs/build-release.sh @@ -2,5 +2,5 @@ cd ../../../src #Ugly way to force rebuild of kinematics, which assumes that tp_debug isn't #used anywhere else... -touch emc/tp/t[cp]*.[ch] -make EXTRA_DEBUG='' +touch emc/tp/tp_debug.h +make -j12 EXTRA_DEBUG='' diff --git a/tests/trajectory-planner/circular-arcs/configs/6axis_rotary.ini b/tests/trajectory-planner/circular-arcs/configs/6axis_rotary.ini new file mode 100644 index 00000000000..9c004c1965e --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/6axis_rotary.ini @@ -0,0 +1,270 @@ +# EMC controller parameters for a simulated machine. + +# General note: Comments can either be preceded with a # or ; - either is +# acceptable, although # is in keeping with most linux config files. + +# General section ------------------------------------------------------------- +[EMC] + +# Version of this INI file +VERSION = $Revision$ + +# Name of machine, for use with display, etc. +MACHINE = LinuxCNC-Circular-Blend-Tester-XYZ + +# Debug level, 0 means no messages. See src/emc/nml_int/emcglb.h for others +#DEBUG = 0x7FFFFFFF +DEBUG = 0 + +# Sections for display options ------------------------------------------------ +[DISPLAY] +PYVCP = vcp-XYZABC.xml +# Name of display program, e.g., xemc +DISPLAY = axis + +GEOMETRY = XYZABC + +# Cycle time, in seconds, that display will sleep between polls +CYCLE_TIME = 0.066666666666 + +# Path to help file +HELP_FILE = doc/help.txt + +# Initial display setting for position, RELATIVE or MACHINE +POSITION_OFFSET = RELATIVE + +# Initial display setting for position, COMMANDED or ACTUAL +POSITION_FEEDBACK = ACTUAL + +# Highest value that will be allowed for feed override, 1.0 = 100% +MAX_FEED_OVERRIDE = 2.0 +MAX_SPINDLE_OVERRIDE = 1.0 + +MAX_LINEAR_VELOCITY = 12 +DEFAULT_LINEAR_VELOCITY = .25 +# Prefix to be used +PROGRAM_PREFIX = ../nc_files/ + +EDITOR = gedit +TOOL_EDITOR = tooledit + +INCREMENTS = 1 in, 0.1 in, 10 mil, 1 mil, 1mm, .1mm, 1/8000 in + +[FILTER] +PROGRAM_EXTENSION = .png,.gif,.jpg Grayscale Depth Image +PROGRAM_EXTENSION = .py Python Script + +png = image-to-gcode +gif = image-to-gcode +jpg = image-to-gcode +py = python + +# Task controller section ----------------------------------------------------- +[TASK] + +# Name of task controller program, e.g., milltask +TASK = milltask + +# Cycle time, in seconds, that task controller will sleep between polls +CYCLE_TIME = 0.001 + +# Part program interpreter section -------------------------------------------- +[RS274NGC] + +# File containing interpreter variables +PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs + +# Motion control section ------------------------------------------------------ +[EMCMOT] + +EMCMOT = motmod + +# Timeout for comm to emcmot, in seconds +COMM_TIMEOUT = 1.0 + +# Interval between tries to emcmot, in seconds +COMM_WAIT = 0.010 + +# BASE_PERIOD is unused in this configuration but specified in core_sim.hal +BASE_PERIOD = 0 +# Servo task period, in nano-seconds +SERVO_PERIOD = 1000000 + +# Hardware Abstraction Layer section -------------------------------------------------- +[HAL] + +# The run script first uses halcmd to execute any HALFILE +# files, and then to execute any individual HALCMD commands. +# +# list of hal config files to run through halcmd +# files are executed in the order in which they appear +HALFILE = core_sim_components.hal +HALFILE = load_constraint_components.tcl +HALFILE = axis-X.tcl +HALFILE = axis-Y.tcl +HALFILE = axis-Z.tcl +HALFILE = axis-A.tcl +HALFILE = axis-B.tcl +HALFILE = axis-C.tcl + +# Other HAL files +HALFILE = sim_spindle_encoder.hal + +HALUI = halui + +# Single file that is executed after the GUI has started. Only supported by +# AXIS at this time (only AXIS creates a HAL component of its own) +POSTGUI_HALFILE = postgui-XYZABC.tcl + +# Trajectory planner section -------------------------------------------------- +[TRAJ] + +AXES = 6 +COORDINATES = X Y Z A B C +HOME = 0 0 0 0 0 0 +LINEAR_UNITS = inch +ANGULAR_UNITS = degree +CYCLE_TIME = 0.010 +DEFAULT_VELOCITY = 1.0 +DEFAULT_ANGULAR_VELOCITY = 45.0 +POSITION_FILE = position9.txt +MAX_LINEAR_VELOCITY = 1.2 +MAX_ANGULAR_VELOCITY = 90.0 + +ARC_BLEND_ENABLE = 1 +ARC_BLEND_FALLBACK_ENABLE = 0 +ARC_BLEND_OPTIMIZATION_DEPTH = 100 +ARC_BLEND_GAP_CYCLES = 4 +ARC_BLEND_RAMP_FREQ = 100 + + +# Axes sections --------------------------------------------------------------- + +# First axis +[AXIS_0] + +TYPE = LINEAR +HOME = 0.000 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.000 +INPUT_SCALE = 2000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +# Second axis +[AXIS_1] + +TYPE = LINEAR +HOME = 0.000 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.000 +INPUT_SCALE = 2000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +# Third axis +[AXIS_2] + +TYPE = LINEAR +HOME = 0.0 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.0 +INPUT_SCALE = 2000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -10.0 +MAX_LIMIT = 10.0001 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +[AXIS_3] + +TYPE = ANGULAR +HOME = 0.0 +MAX_VELOCITY = 90.0 +MAX_ACCELERATION = 1200.0 +BACKLASH = 0.000 +INPUT_SCALE = 40 +OUTPUT_SCALE = 1.000 +FERROR = 5.0 +MIN_FERROR = 1.0 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_IGNORE_LIMITS = NO +HOME_SEQUENCE = 0 + +[AXIS_4] + +TYPE = ANGULAR +HOME = 0.0 +MAX_VELOCITY = 90.0 +MAX_ACCELERATION = 1200.0 +BACKLASH = 0.000 +INPUT_SCALE = 40 +OUTPUT_SCALE = 1.000 +FERROR = 5.0 +MIN_FERROR = 1.0 +HOME_OFFSET = 45.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_IGNORE_LIMITS = NO +HOME_SEQUENCE = 0 + +[AXIS_5] + +TYPE = ANGULAR +HOME = 0.0 +MAX_VELOCITY = 90.0 +MAX_ACCELERATION = 1200.0 +BACKLASH = 0.000 +INPUT_SCALE = 40 +OUTPUT_SCALE = 1.000 +FERROR = 5.0 +MIN_FERROR = 1.0 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_IGNORE_LIMITS = NO +HOME_SEQUENCE = 0 + +# section for main IO controller parameters ----------------------------------- +[EMCIO] + +# Name of IO controller program, e.g., io +EMCIO = io + +# cycle time, in seconds +CYCLE_TIME = 0.066666666666 + +# tool table file +TOOL_TABLE = sim.tbl +TOOL_CHANGE_POSITION = 0 0 0 +TOOL_CHANGE_QUILL_UP = 1 diff --git a/tests/trajectory-planner/circular-arcs/configs/9axis.ini b/tests/trajectory-planner/circular-arcs/configs/9axis.ini index 4f8a973edd6..19062374aca 100644 --- a/tests/trajectory-planner/circular-arcs/configs/9axis.ini +++ b/tests/trajectory-planner/circular-arcs/configs/9axis.ini @@ -18,12 +18,12 @@ DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] -PYVCP = vcp_9axis.xml +PYVCP = vcp-XYZABCUVW.xml # Name of display program, e.g., xemc DISPLAY = axis # Cycle time, in seconds, that display will sleep between polls -CYCLE_TIME = 0.100 +CYCLE_TIME = 0.066666666666 # Path to help file HELP_FILE = doc/help.txt @@ -35,18 +35,18 @@ POSITION_OFFSET = RELATIVE POSITION_FEEDBACK = ACTUAL # Highest value that will be allowed for feed override, 1.0 = 100% -MAX_FEED_OVERRIDE = 1.2 +MAX_FEED_OVERRIDE = 2.0 MAX_SPINDLE_OVERRIDE = 1.0 + +MAX_LINEAR_VELOCITY = 12 +DEFAULT_LINEAR_VELOCITY = .25 # Prefix to be used PROGRAM_PREFIX = ../nc_files/ -# Introductory graphic -INTRO_GRAPHIC = linuxcnc.gif -INTRO_TIME = 5 - EDITOR = gedit +TOOL_EDITOR = tooledit -INCREMENTS = 1 mm, .01 in, .1mm, 1 mil, .1 mil, 1/8000 in +INCREMENTS = 1 in, 0.1 in, 10 mil, 1 mil, 1mm, .1mm, 1/8000 in GEOMETRY = XYZABCUVW @@ -73,6 +73,7 @@ CYCLE_TIME = 0.001 # File containing interpreter variables PARAMETER_FILE = sim-9axis.var +SUBROUTINE_PATH = ../nc_files/test_subs # Motion control section ------------------------------------------------------ [EMCMOT] @@ -85,6 +86,8 @@ COMM_TIMEOUT = 1.0 # Interval between tries to emcmot, in seconds COMM_WAIT = 0.010 +# BASE_PERIOD is unused in this configuration but specified in core_sim.hal +BASE_PERIOD = 0 # Servo task period, in nano-seconds SERVO_PERIOD = 1000000 @@ -97,15 +100,23 @@ SERVO_PERIOD = 1000000 # list of hal config files to run through halcmd # files are executed in the order in which they appear -HALFILE = core_sim9.hal -HALFILE = axis_manualtoolchange.hal -HALFILE = simulated_home.hal -HALFILE = locking_indexer.hal -HALFILE = constraint_check_9axis.tcl +HALFILE = core_sim_components.hal +HALFILE = load_constraint_components.tcl +HALFILE = axis-X.tcl +HALFILE = axis-Y.tcl +HALFILE = axis-Z.tcl +HALFILE = axis-A.tcl +HALFILE = axis-B.tcl +HALFILE = axis-C.tcl +HALFILE = axis-U.tcl +HALFILE = axis-V.tcl +HALFILE = axis-W.tcl +# Other HAL files +HALFILE = sim_spindle_encoder.hal # Single file that is executed after the GUI has started. Only supported by # AXIS at this time (only AXIS creates a HAL component of its own) -POSTGUI_HALFILE = postgui_9axis.hal +POSTGUI_HALFILE = postgui-XYZABCUVW.tcl HALUI = halui @@ -126,12 +137,14 @@ MAX_ANGULAR_VELOCITY = 90.0 ARC_BLEND_ENABLE = 1 ARC_BLEND_FALLBACK_ENABLE = 0 -ARC_BLEND_OPTIMIZATION_DEPTH = 50 +ARC_BLEND_OPTIMIZATION_DEPTH = 100 ARC_BLEND_GAP_CYCLES = 4 -ARC_BLEND_RAMP_FREQ = 20 +ARC_BLEND_RAMP_FREQ = 100 + # Axes sections --------------------------------------------------------------- +# First axis [AXIS_0] TYPE = LINEAR @@ -146,12 +159,12 @@ MAX_LIMIT = 10.0 FERROR = 0.050 MIN_FERROR = 0.010 HOME_OFFSET = 0.0 -HOME_SEARCH_VEL = 5.0 -HOME_LATCH_VEL = 1.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 HOME_USE_INDEX = NO HOME_IGNORE_LIMITS = NO -HOME_SEQUENCE = 1 -HOME_IS_SHARED = 1 +HOME_SEQUENCE = 0 +HOME_IS_SHARED = 0 [AXIS_1] @@ -167,11 +180,11 @@ MAX_LIMIT = 10.0 FERROR = 0.050 MIN_FERROR = 0.010 HOME_OFFSET = 0.0 -HOME_SEARCH_VEL = 5.0 -HOME_LATCH_VEL = 1.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 HOME_USE_INDEX = NO HOME_IGNORE_LIMITS = NO -HOME_SEQUENCE = 1 +HOME_SEQUENCE = 0 [AXIS_2] @@ -186,13 +199,13 @@ MIN_LIMIT = -2.0 MAX_LIMIT = 4.0 FERROR = 0.050 MIN_FERROR = 0.010 -HOME_OFFSET = 1.0 -HOME_SEARCH_VEL = 5.0 -HOME_LATCH_VEL = 1.0 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 HOME_USE_INDEX = NO HOME_IGNORE_LIMITS = NO HOME_SEQUENCE = 0 -HOME_IS_SHARED = 1 +HOME_IS_SHARED = 0 [AXIS_3] @@ -210,14 +223,14 @@ HOME_SEARCH_VEL = 0.0 HOME_LATCH_VEL = 0.0 HOME_USE_INDEX = NO HOME_IGNORE_LIMITS = NO -HOME_SEQUENCE = 1 +HOME_SEQUENCE = 0 [AXIS_4] TYPE = ANGULAR HOME = 0.0 -MAX_VELOCITY = 90.0 -MAX_ACCELERATION = 1200.0 +MAX_VELOCITY = 100.0 +MAX_ACCELERATION = 1000.0 BACKLASH = 0.000 INPUT_SCALE = 40 OUTPUT_SCALE = 1.000 @@ -228,15 +241,14 @@ HOME_SEARCH_VEL = 0.0 HOME_LATCH_VEL = 0.0 HOME_USE_INDEX = NO HOME_IGNORE_LIMITS = NO -HOME_SEQUENCE = 1 -LOCKING_INDEXER = 1 +HOME_SEQUENCE = 0 [AXIS_5] TYPE = ANGULAR HOME = 0.0 -MAX_VELOCITY = 90.0 -MAX_ACCELERATION = 1200.0 +MAX_VELOCITY = 50.0 +MAX_ACCELERATION = 1000.0 BACKLASH = 0.000 INPUT_SCALE = 40 OUTPUT_SCALE = 1.000 @@ -247,7 +259,7 @@ HOME_SEARCH_VEL = 0.0 HOME_LATCH_VEL = 0.0 HOME_USE_INDEX = NO HOME_IGNORE_LIMITS = NO -HOME_SEQUENCE = 1 +HOME_SEQUENCE = 0 [AXIS_6] diff --git a/tests/trajectory-planner/circular-arcs/configs/XY.ini b/tests/trajectory-planner/circular-arcs/configs/XY.ini index 4d2678ff4f3..5d6594b811d 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XY.ini +++ b/tests/trajectory-planner/circular-arcs/configs/XY.ini @@ -19,7 +19,7 @@ DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] #Includes acceleration / velocity -PYVCP = vcp.xml +PYVCP = vcp-XYZA.xml # Name of display program, e.g., xemc DISPLAY = axis @@ -72,6 +72,7 @@ CYCLE_TIME = 0.001 # File containing interpreter variables PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs # Motion control section ------------------------------------------------------ [EMCMOT] @@ -98,13 +99,12 @@ SERVO_PERIOD = 1000000 # list of hal config files to run through halcmd # files are executed in the order in which they appear -# Core sim stuff for various loaded axes HALFILE = core_sim_components.hal -HALFILE = test_status.tcl +HALFILE = load_constraint_components.tcl HALFILE = axis-X.tcl HALFILE = axis-Y.tcl +HALFILE = axis-Z.tcl # Other HAL files -HALFILE = axis_manualtoolchange.hal HALFILE = sim_spindle_encoder.hal # list of halcmd commands to execute @@ -113,7 +113,7 @@ HALFILE = sim_spindle_encoder.hal # Single file that is executed after the GUI has started. Only supported by # AXIS at this time (only AXIS creates a HAL component of its own) -POSTGUI_HALFILE = postgui.hal +POSTGUI_HALFILE = postgui-XYZA.ini HALUI = halui @@ -122,7 +122,7 @@ HALUI = halui AXES = 2 COORDINATES = X Y -HOME = 0 0 0 +HOME = 0 0 LINEAR_UNITS = inch ANGULAR_UNITS = degree CYCLE_TIME = 0.010 diff --git a/tests/trajectory-planner/circular-arcs/configs/XYZ_fast.ini b/tests/trajectory-planner/circular-arcs/configs/XYZ_fast.ini index a7aae358939..9409f960cf0 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XYZ_fast.ini +++ b/tests/trajectory-planner/circular-arcs/configs/XYZ_fast.ini @@ -18,7 +18,7 @@ DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] -PYVCP = vcp.xml +PYVCP = vcp-XYZA.xml # Name of display program, e.g., xemc DISPLAY = axis @@ -71,6 +71,7 @@ CYCLE_TIME = 0.001 # File containing interpreter variables PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs # Motion control section ------------------------------------------------------ [EMCMOT] diff --git a/tests/trajectory-planner/circular-arcs/configs/XYZ_medium.ini b/tests/trajectory-planner/circular-arcs/configs/XYZ_medium.ini index d41f40290a9..17cf1b90bb1 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XYZ_medium.ini +++ b/tests/trajectory-planner/circular-arcs/configs/XYZ_medium.ini @@ -18,7 +18,7 @@ DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] -PYVCP = vcp.xml +PYVCP = vcp-XYZA.xml # Name of display program, e.g., xemc DISPLAY = axis @@ -71,6 +71,7 @@ CYCLE_TIME = 0.001 # File containing interpreter variables PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs # Motion control section ------------------------------------------------------ [EMCMOT] diff --git a/tests/trajectory-planner/circular-arcs/configs/XYZ_nospindle.ini b/tests/trajectory-planner/circular-arcs/configs/XYZ_nospindle.ini index b1e63f1b14e..5400a102d8f 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XYZ_nospindle.ini +++ b/tests/trajectory-planner/circular-arcs/configs/XYZ_nospindle.ini @@ -18,7 +18,7 @@ DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] -PYVCP = vcp.xml +PYVCP = vcp-XYZA.xml # Name of display program, e.g., xemc DISPLAY = axis @@ -71,6 +71,8 @@ CYCLE_TIME = 0.001 # File containing interpreter variables PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs + # Motion control section ------------------------------------------------------ [EMCMOT] diff --git a/tests/trajectory-planner/circular-arcs/configs/XYZ_uneven.ini b/tests/trajectory-planner/circular-arcs/configs/XYZ_uneven.ini index dea3b9d7015..f85248d7e0b 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XYZ_uneven.ini +++ b/tests/trajectory-planner/circular-arcs/configs/XYZ_uneven.ini @@ -71,6 +71,7 @@ CYCLE_TIME = 0.001 # File containing interpreter variables PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs # Motion control section ------------------------------------------------------ [EMCMOT] diff --git a/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini b/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini index 1185e66118a..d61e62449db 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini +++ b/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini @@ -18,7 +18,7 @@ DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] -PYVCP = vcp.xml +PYVCP = vcp-XYZA.xml # Name of display program, e.g., xemc DISPLAY = axis @@ -48,6 +48,8 @@ TOOL_EDITOR = tooledit INCREMENTS = 1 in, 0.1 in, 10 mil, 1 mil, 1mm, .1mm, 1/8000 in +GEOMETRY = XYZA + [FILTER] PROGRAM_EXTENSION = .png,.gif,.jpg Grayscale Depth Image PROGRAM_EXTENSION = .py Python Script @@ -71,6 +73,7 @@ CYCLE_TIME = 0.001 # File containing interpreter variables PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs # Motion control section ------------------------------------------------------ [EMCMOT] @@ -98,12 +101,12 @@ SERVO_PERIOD = 1000000 # list of hal config files to run through halcmd # files are executed in the order in which they appear HALFILE = core_sim_components.hal -HALFILE = test_status.tcl +HALFILE = load_constraint_components.tcl HALFILE = axis-X.tcl HALFILE = axis-Y.tcl HALFILE = axis-Z.tcl +HALFILE = axis-A.tcl # Other HAL files -HALFILE = axis_manualtoolchange.hal HALFILE = sim_spindle_encoder.hal # list of halcmd commands to execute @@ -112,7 +115,7 @@ HALFILE = sim_spindle_encoder.hal # Single file that is executed after the GUI has started. Only supported by # AXIS at this time (only AXIS creates a HAL component of its own) -POSTGUI_HALFILE = postgui.hal +POSTGUI_HALFILE = postgui-XYZA.tcl HALUI = halui @@ -121,7 +124,7 @@ HALUI = halui AXES = 4 COORDINATES = X Y Z A -HOME = 0 0 0 +HOME = 0 0 0 0 LINEAR_UNITS = inch ANGULAR_UNITS = degree CYCLE_TIME = 0.010 @@ -131,9 +134,10 @@ MAX_LINEAR_VELOCITY = 12 ARC_BLEND_ENABLE = 1 ARC_BLEND_FALLBACK_ENABLE = 0 -ARC_BLEND_OPTIMIZATION_DEPTH = 50 +ARC_BLEND_OPTIMIZATION_DEPTH = 100 ARC_BLEND_GAP_CYCLES = 4 -ARC_BLEND_RAMP_FREQ = 20 +ARC_BLEND_RAMP_FREQ = 100 + # Axes sections --------------------------------------------------------------- @@ -183,8 +187,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/configs/add_hal_symlinks.sh b/tests/trajectory-planner/circular-arcs/configs/add_hal_symlinks.sh deleted file mode 100755 index 224b40c8e2e..00000000000 --- a/tests/trajectory-planner/circular-arcs/configs/add_hal_symlinks.sh +++ /dev/null @@ -1,3 +0,0 @@ -ln -sf ../../../../configs/sim/core_sim.hal -ln -sf ../../../../configs/sim/axis_manualtoolchange.hal -ln -sf ../../../../configs/sim/sim_spindle_encoder.hal diff --git a/tests/trajectory-planner/circular-arcs/configs/apply_limits_to_enable.tcl b/tests/trajectory-planner/circular-arcs/configs/apply_limits_to_enable.tcl new file mode 100644 index 00000000000..d7ce71c27e7 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/apply_limits_to_enable.tcl @@ -0,0 +1 @@ +net constraints-ok <= match_all.out -> motion.enable diff --git a/tests/trajectory-planner/circular-arcs/configs/axis_mm.ini b/tests/trajectory-planner/circular-arcs/configs/autotest_mill_XYZA.ini similarity index 56% rename from tests/trajectory-planner/circular-arcs/configs/axis_mm.ini rename to tests/trajectory-planner/circular-arcs/configs/autotest_mill_XYZA.ini index e6e12a71e51..4ec415df1ce 100644 --- a/tests/trajectory-planner/circular-arcs/configs/axis_mm.ini +++ b/tests/trajectory-planner/circular-arcs/configs/autotest_mill_XYZA.ini @@ -10,20 +10,19 @@ VERSION = $Revision$ # Name of machine, for use with display, etc. -MACHINE = LinuxCNC-HAL-SIM-AXIS +MACHINE = LinuxCNC-Circular-Blend-Tester-XYZ # Debug level, 0 means no messages. See src/emc/nml_int/emcglb.h for others -# DEBUG = 0x7FFFFFFF +#DEBUG = 0x7FFFFFFF DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] -PYVCP = vcp.xml # Name of display program, e.g., xemc -DISPLAY = axis +DISPLAY = ../run_all_tests.py # Cycle time, in seconds, that display will sleep between polls -CYCLE_TIME = 0.100 +CYCLE_TIME = 0.066666666666 # Path to help file HELP_FILE = doc/help.txt @@ -35,18 +34,21 @@ POSITION_OFFSET = RELATIVE POSITION_FEEDBACK = ACTUAL # Highest value that will be allowed for feed override, 1.0 = 100% -MAX_FEED_OVERRIDE = 1.2 +MAX_FEED_OVERRIDE = 2.0 MAX_SPINDLE_OVERRIDE = 1.0 -# Prefix to be used -PROGRAM_PREFIX = ../../../nc_files/ -# Introductory graphic -INTRO_GRAPHIC = linuxcnc.gif -INTRO_TIME = 5 +MAX_LINEAR_VELOCITY = 12 +DEFAULT_LINEAR_VELOCITY = .25 +# Prefix to be used +PROGRAM_PREFIX = ../nc_files/ EDITOR = gedit +TOOL_EDITOR = tooledit + +INCREMENTS = 1 in, 0.1 in, 10 mil, 1 mil, 1mm, .1mm, 1/8000 in + +GEOMETRY = XYZA -INCREMENTS = 1 mm, .01 in, .1mm, 1 mil, .1 mil, 1/8000 in [FILTER] PROGRAM_EXTENSION = .png,.gif,.jpg Grayscale Depth Image PROGRAM_EXTENSION = .py Python Script @@ -69,7 +71,8 @@ CYCLE_TIME = 0.001 [RS274NGC] # File containing interpreter variables -PARAMETER_FILE = sim_mm.var +PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs # Motion control section ------------------------------------------------------ [EMCMOT] @@ -90,47 +93,35 @@ SERVO_PERIOD = 1000000 # Hardware Abstraction Layer section -------------------------------------------------- [HAL] -# The run script first uses halcmd to execute any HALFILE -# files, and then to execute any individual HALCMD commands. -# - -# list of hal config files to run through halcmd -# files are executed in the order in which they appear -HALFILE = core_sim.hal +HALFILE = core_sim_components.hal +HALFILE = load_constraint_components.tcl +HALFILE = axis-X.tcl +HALFILE = axis-Y.tcl +HALFILE = axis-Z.tcl +HALFILE = axis-A.tcl HALFILE = sim_spindle_encoder.hal -HALFILE = axis_manualtoolchange.hal -HALFILE = simulated_home.hal -HALFILE = test_status.tcl - -# list of halcmd commands to execute -# commands are executed in the order in which they appear -#HALCMD = save neta - -# Single file that is executed after the GUI has started. Only supported by -# AXIS at this time (only AXIS creates a HAL component of its own) -POSTGUI_HALFILE = postgui.hal HALUI = halui # Trajectory planner section -------------------------------------------------- [TRAJ] -AXES = 3 -COORDINATES = X Y Z -HOME = 0 0 0 -LINEAR_UNITS = mm +AXES = 4 +COORDINATES = X Y Z A +HOME = 0 0 0 0 +LINEAR_UNITS = inch ANGULAR_UNITS = degree CYCLE_TIME = 0.010 -DEFAULT_VELOCITY = 30.48 -MAX_VELOCITY = 53.34 -DEFAULT_ACCELERATION = 508 -MAX_ACCELERATION = 508 -POSITION_FILE = position_mm.txt +DEFAULT_VELOCITY = 1.2 +POSITION_FILE = position.txt +MAX_LINEAR_VELOCITY = 12 + ARC_BLEND_ENABLE = 1 ARC_BLEND_FALLBACK_ENABLE = 0 -ARC_BLEND_OPTIMIZATION_DEPTH = 100 -ARC_BLEND_GAP_CYCLES = 2 -ARC_BLEND_RAMP_FREQ = 20 +(??) +ARC_BLEND_GAP_CYCLES = 4 +ARC_BLEND_RAMP_FREQ = 100 + # Axes sections --------------------------------------------------------------- @@ -139,65 +130,76 @@ ARC_BLEND_RAMP_FREQ = 20 TYPE = LINEAR HOME = 0.000 -MAX_VELOCITY = 30.48 -MAX_ACCELERATION = 508 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 BACKLASH = 0.000 -INPUT_SCALE = 157.48 +INPUT_SCALE = 2000 OUTPUT_SCALE = 1.000 -MIN_LIMIT = -254 -MAX_LIMIT = 254 -FERROR = 1.27 -MIN_FERROR = .254 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 HOME_OFFSET = 0.0 -HOME_SEARCH_VEL = 127 -HOME_LATCH_VEL = 25.4 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 HOME_USE_INDEX = NO -HOME_IGNORE_LIMITS = NO -HOME_SEQUENCE = 1 -HOME_IS_SHARED = 1 +HOME_SEQUENCE = 0 # Second axis [AXIS_1] TYPE = LINEAR HOME = 0.000 -MAX_VELOCITY = 30.48 -MAX_ACCELERATION = 508 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 BACKLASH = 0.000 -INPUT_SCALE = 157.48 +INPUT_SCALE = 2000 OUTPUT_SCALE = 1.000 -MIN_LIMIT = -254 -MAX_LIMIT = 254 -FERROR = 1.27 -MIN_FERROR = .254 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 HOME_OFFSET = 0.0 -HOME_SEARCH_VEL = 127 -HOME_LATCH_VEL = 25.4 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 HOME_USE_INDEX = NO -HOME_IGNORE_LIMITS = NO -HOME_SEQUENCE = 1 +HOME_SEQUENCE = 0 # Third axis [AXIS_2] TYPE = LINEAR HOME = 0.0 -MAX_VELOCITY = 30.48 -MAX_ACCELERATION = 508 -BACKLASH = 0.000 -INPUT_SCALE = 157.48 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.0 +INPUT_SCALE = 2000 OUTPUT_SCALE = 1.000 -MIN_LIMIT = -50.8 -MAX_LIMIT = 101.6 -FERROR = 1.27 -MIN_FERROR = .254 -HOME_OFFSET = 25.4 -HOME_SEARCH_VEL = 127 -HOME_LATCH_VEL = 25.4 +MIN_LIMIT = -10.0 +MAX_LIMIT = 10.0001 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +[AXIS_3] +TYPE = ANGULAR +HOME = 0.0 +MAX_VELOCITY = 25 +MAX_ACCELERATION = 800 +SCALE = 500.0 +FERROR = 5.0 +MIN_FERROR = 2.5 +MIN_LIMIT = -9999.0 +MAX_LIMIT = 9999.0 +HOME_OFFSET = 0.000000 +HOME_SEARCH_VEL = 0.00000 +HOME_LATCH_VEL = 0.00000 HOME_USE_INDEX = NO -HOME_IGNORE_LIMITS = NO HOME_SEQUENCE = 0 -HOME_IS_SHARED = 1 # section for main IO controller parameters ----------------------------------- [EMCIO] @@ -206,8 +208,12 @@ HOME_IS_SHARED = 1 EMCIO = io # cycle time, in seconds -CYCLE_TIME = 0.100 +CYCLE_TIME = 0.066666666666 # tool table file -TOOL_TABLE = sim_mm.tbl -TOOL_CHANGE_POSITION = 0 0 50.8 +TOOL_TABLE = sim.tbl +TOOL_CHANGE_POSITION = 0 0 0 +TOOL_CHANGE_QUILL_UP = 1 + +[REDIS] +DISABLE_SERVER=1 diff --git a/tests/trajectory-planner/circular-arcs/configs/autotest_mill_XYZA_template.ini b/tests/trajectory-planner/circular-arcs/configs/autotest_mill_XYZA_template.ini new file mode 100644 index 00000000000..8189e8e97f5 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/autotest_mill_XYZA_template.ini @@ -0,0 +1,192 @@ +# LinuxCNC controller parameters for a simulated 4-axis milling machine + +# General section ------------------------------------------------------------- +[EMC] + +# Version of this INI file +VERSION = $Revision$ + +# Name of machine, for use with display, etc. +MACHINE = LinuxCNC-Auto-Test-Config + +# Debug level, 0 means no messages. See src/emc/nml_int/emcglb.h for others +#DEBUG = 0x7FFFFFFF +DEBUG = 0 + +# NOTE can only handle raw G-code files + +# Task controller section ----------------------------------------------------- +[TASK] + +# Name of task controller program, e.g., milltask +TASK = milltask + +# Cycle time, in seconds, that task controller will sleep between polls +CYCLE_TIME = 0.001 + +# Part program interpreter section -------------------------------------------- +[RS274NGC] + +# File containing interpreter variables +PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs + +# Motion control section ------------------------------------------------------ +[EMCMOT] + +EMCMOT = motmod + +# Timeout for comm to emcmot, in seconds +COMM_TIMEOUT = 1.0 + +# Interval between tries to emcmot, in seconds +COMM_WAIT = 0.010 + +# BASE_PERIOD is unused in this configuration but specified in core_sim.hal +BASE_PERIOD = 0 +# Servo task period, in nano-seconds +SERVO_PERIOD = 1000000 + +# Hardware Abstraction Layer section -------------------------------------------------- +[HAL] + +# The run script first uses halcmd to execute any HALFILE +# files, and then to execute any individual HALCMD commands. +# + +# list of hal config files to run through halcmd +# files are executed in the order in which they appear +HALFILE = core_sim_components.hal +HALFILE = load_constraint_components.tcl +HALFILE = axis-X.tcl +HALFILE = axis-Y.tcl +HALFILE = axis-Z.tcl +HALFILE = axis-A.tcl +# Other HAL files +(??) +HALFILE = sim_spindle_encoder.hal + +# list of halcmd commands to execute +# commands are executed in the order in which they appear +#HALCMD = save neta + +# Single file that is executed after the GUI has started. Only supported by +# AXIS at this time (only AXIS creates a HAL component of its own) +# POSTGUI_HALFILE = postgui-XYZA.tcl + +HALUI = halui + +# Trajectory planner section -------------------------------------------------- +[TRAJ] + +AXES = 4 +COORDINATES = X Y Z A +HOME = 0 0 0 0 +LINEAR_UNITS = inch +ANGULAR_UNITS = degree +CYCLE_TIME = 0.010 +DEFAULT_VELOCITY = 1.2 +POSITION_FILE = position.txt +MAX_LINEAR_VELOCITY = 12 + +ARC_BLEND_ENABLE = 1 +ARC_BLEND_FALLBACK_ENABLE = 0 +(??) +ARC_BLEND_GAP_CYCLES = 4 +ARC_BLEND_RAMP_FREQ = 100 + + +# Axes sections --------------------------------------------------------------- + +# First axis +[AXIS_0] + +TYPE = LINEAR +HOME = 0.000 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.000 +INPUT_SCALE = 2000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +# Second axis +[AXIS_1] + +TYPE = LINEAR +HOME = 0.000 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.000 +INPUT_SCALE = 2000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +# Third axis +[AXIS_2] + +TYPE = LINEAR +HOME = 0.0 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.0 +INPUT_SCALE = 2000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -10.0 +MAX_LIMIT = 10.0001 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +[AXIS_3] +TYPE = ANGULAR +HOME = 0.0 +MAX_VELOCITY = 25 +MAX_ACCELERATION = 800 +SCALE = 500.0 +FERROR = 5.0 +MIN_FERROR = 2.5 +MIN_LIMIT = -9999.0 +MAX_LIMIT = 9999.0 +HOME_OFFSET = 0.000000 +HOME_SEARCH_VEL = 0.00000 +HOME_LATCH_VEL = 0.00000 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +# section for main IO controller parameters ----------------------------------- +[EMCIO] + +# Name of IO controller program, e.g., io +EMCIO = io + +# cycle time, in seconds +CYCLE_TIME = 0.066666666666 + +# tool table file +TOOL_TABLE = sim.tbl +TOOL_CHANGE_POSITION = 0 0 0 +TOOL_CHANGE_QUILL_UP = 1 + +[REDIS] +DISABLE_SERVER=1 diff --git a/tests/trajectory-planner/circular-arcs/configs/axis-A.tcl b/tests/trajectory-planner/circular-arcs/configs/axis-A.tcl index 1719f57583c..9a593cc73b9 100644 --- a/tests/trajectory-planner/circular-arcs/configs/axis-A.tcl +++ b/tests/trajectory-planner/circular-arcs/configs/axis-A.tcl @@ -1,21 +1,35 @@ +set av_id 3 +set aa_id 13 + +# Add axis components to servo thread +addf wcomp.$av_id servo-thread +addf wcomp.$aa_id servo-thread +addf minmax.$av_id servo-thread +addf minmax.$aa_id servo-thread +addf d2dt2.$av_id servo-thread + # create HAL signals for position commands from motion module # loop position commands back to motion module feedback -net Apos axis.3.motor-pos-cmd => axis.3.motor-pos-fb ddt_a.in +net a_pos axis.$av_id.motor-pos-cmd => axis.$av_id.motor-pos-fb d2dt2.$av_id.in # send the position commands thru differentiators to # generate velocity and accel signals -net Avel ddt_a.out => ddt_av.in -net Aacc <= ddt_av.out +net a_vel <= d2dt2.$av_id.out1 => wcomp.$av_id.in minmax.$av_id.in +net a_acc <= d2dt2.$av_id.out2 => wcomp.$aa_id.in minmax.$aa_id.in #Conservative limits set acc_limit 1.0001 set vel_limit 1.01 -setp wcomp_aacc.max $::AXIS_3(MAX_ACCELERATION)*$acc_limit -setp wcomp_aacc.min $::AXIS_3(MAX_ACCELERATION)*-1.0*$acc_limit -setp wcomp_avel.max $::AXIS_3(MAX_VELOCITY)*$vel_limit -setp wcomp_avel.min $::AXIS_3(MAX_VELOCITY)*-1.0*$vel_limit +setp wcomp.$aa_id.max $::AXIS_3(MAX_ACCELERATION)*$acc_limit +setp wcomp.$aa_id.min $::AXIS_3(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$av_id.max $::AXIS_3(MAX_VELOCITY)*$vel_limit +setp wcomp.$av_id.min $::AXIS_3(MAX_VELOCITY)*-1.0*$vel_limit + +net a_acc-ok <= wcomp.$av_id.out => match_abc.a0 +net a_vel-ok <= wcomp.$aa_id.out => match_abc.a1 + +# Enable match_all pins for axis +setp match_abc.b0 1 +setp match_abc.b1 1 -# Enable match_all pins for A axis -setp match_all.b6 1 -setp match_all.b7 1 diff --git a/tests/trajectory-planner/circular-arcs/configs/axis-B.tcl b/tests/trajectory-planner/circular-arcs/configs/axis-B.tcl new file mode 100644 index 00000000000..72c3e5a358f --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/axis-B.tcl @@ -0,0 +1,35 @@ +set bv_id 4 +set ba_id 14 + +# Add axis components to servo thread +addf wcomp.$bv_id servo-thread +addf wcomp.$ba_id servo-thread +addf minmax.$bv_id servo-thread +addf minmax.$ba_id servo-thread +addf d2dt2.$bv_id servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net b_pos axis.$bv_id.motor-pos-cmd => axis.$bv_id.motor-pos-fb d2dt2.$bv_id.in + +# send the position commands thru differentiators to +# generate velocity and accel signals +net b_vel <= d2dt2.$bv_id.out1 => wcomp.$bv_id.in minmax.$bv_id.in +net b_acc <= d2dt2.$bv_id.out2 => wcomp.$ba_id.in minmax.$ba_id.in + +#Conservative limits +set acc_limit 1.0001 +set vel_limit 1.01 + +setp wcomp.$ba_id.max $::AXIS_4(MAX_ACCELERATION)*$acc_limit +setp wcomp.$ba_id.min $::AXIS_4(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$bv_id.max $::AXIS_4(MAX_VELOCITY)*$vel_limit +setp wcomp.$bv_id.min $::AXIS_4(MAX_VELOCITY)*-1.0*$vel_limit + +net b_acc-ok <= wcomp.$bv_id.out => match_abc.a2 +net b_vel-ok <= wcomp.$ba_id.out => match_abc.a3 + +# Enable match_all pins for axis +setp match_abc.b2 1 +setp match_abc.b3 1 + diff --git a/tests/trajectory-planner/circular-arcs/configs/axis-C.tcl b/tests/trajectory-planner/circular-arcs/configs/axis-C.tcl new file mode 100644 index 00000000000..f08ea4873cb --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/axis-C.tcl @@ -0,0 +1,35 @@ +set cv_id 5 +set ca_id 15 + +# Add axis components to servo thread +addf wcomp.$cv_id servo-thread +addf wcomp.$ca_id servo-thread +addf minmax.$cv_id servo-thread +addf minmax.$ca_id servo-thread +addf d2dt2.$cv_id servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net c_pos axis.$cv_id.motor-pos-cmd => axis.$cv_id.motor-pos-fb d2dt2.$cv_id.in + +# send the position commands thru differentiators to +# generate velocity and accel signals +net c_vel <= d2dt2.$cv_id.out1 => wcomp.$cv_id.in minmax.$cv_id.in +net c_acc <= d2dt2.$cv_id.out2 => wcomp.$ca_id.in minmax.$ca_id.in + +#Conservative limits +set acc_limit 1.0001 +set vel_limit 1.01 + +setp wcomp.$ca_id.max $::AXIS_5(MAX_ACCELERATION)*$acc_limit +setp wcomp.$ca_id.min $::AXIS_5(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$cv_id.max $::AXIS_5(MAX_VELOCITY)*$vel_limit +setp wcomp.$cv_id.min $::AXIS_5(MAX_VELOCITY)*-1.0*$vel_limit + +net c_acc-ok <= wcomp.$cv_id.out => match_abc.a4 +net c_vel-ok <= wcomp.$ca_id.out => match_abc.a5 + +# Enable match_all pins for axis +setp match_abc.b4 1 +setp match_abc.b5 1 + diff --git a/tests/trajectory-planner/circular-arcs/configs/axis-U.tcl b/tests/trajectory-planner/circular-arcs/configs/axis-U.tcl new file mode 100644 index 00000000000..b8ee079f57b --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/axis-U.tcl @@ -0,0 +1,35 @@ +set uv_id 6 +set ua_id 16 + +# Add axis components to servo thread +addf wcomp.$uv_id servo-thread +addf wcomp.$ua_id servo-thread +addf minmax.$uv_id servo-thread +addf minmax.$ua_id servo-thread +addf d2dt2.$uv_id servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net u_pos axis.$uv_id.motor-pos-cmd => axis.$uv_id.motor-pos-fb d2dt2.$uv_id.in + +# send the position commands thru differentiators to +# generate velocity and accel signals +net u_vel <= d2dt2.$uv_id.out1 => wcomp.$uv_id.in minmax.$uv_id.in +net u_acc <= d2dt2.$uv_id.out2 => wcomp.$ua_id.in minmax.$ua_id.in + +#Conservative limits +set acc_limit 1.0001 +set vel_limit 1.01 + +setp wcomp.$ua_id.max $::AXIS_6(MAX_ACCELERATION)*$acc_limit +setp wcomp.$ua_id.min $::AXIS_6(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$uv_id.max $::AXIS_6(MAX_VELOCITY)*$vel_limit +setp wcomp.$uv_id.min $::AXIS_6(MAX_VELOCITY)*-1.0*$vel_limit + +net u_acc-ok <= wcomp.$uv_id.out => match_uvw.a0 +net u_vel-ok <= wcomp.$ua_id.out => match_uvw.a1 + +# Enable match_all pins for axis +setp match_uvw.b0 1 +setp match_uvw.b1 1 + diff --git a/tests/trajectory-planner/circular-arcs/configs/axis-V.tcl b/tests/trajectory-planner/circular-arcs/configs/axis-V.tcl new file mode 100644 index 00000000000..8edd456d68c --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/axis-V.tcl @@ -0,0 +1,35 @@ +set vv_id 7 +set va_id 17 + +# Add axis components to servo thread +addf wcomp.$vv_id servo-thread +addf wcomp.$va_id servo-thread +addf minmax.$vv_id servo-thread +addf minmax.$va_id servo-thread +addf d2dt2.$vv_id servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net v_pos axis.$vv_id.motor-pos-cmd => axis.$vv_id.motor-pos-fb d2dt2.$vv_id.in + +# send the position commands thru differentiators to +# generate velocity and accel signals +net v_vel <= d2dt2.$vv_id.out1 => wcomp.$vv_id.in minmax.$vv_id.in +net v_acc <= d2dt2.$vv_id.out2 => wcomp.$va_id.in minmax.$va_id.in + +#Conservative limits +set acc_limit 1.0001 +set vel_limit 1.01 + +setp wcomp.$va_id.max $::AXIS_7(MAX_ACCELERATION)*$acc_limit +setp wcomp.$va_id.min $::AXIS_7(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$vv_id.max $::AXIS_7(MAX_VELOCITY)*$vel_limit +setp wcomp.$vv_id.min $::AXIS_7(MAX_VELOCITY)*-1.0*$vel_limit + +net v_acc-ok <= wcomp.$vv_id.out => match_uvw.a2 +net v_vel-ok <= wcomp.$va_id.out => match_uvw.a3 + +# Enable match_all pins for axis +setp match_uvw.b2 1 +setp match_uvw.b3 1 + diff --git a/tests/trajectory-planner/circular-arcs/configs/axis-W.tcl b/tests/trajectory-planner/circular-arcs/configs/axis-W.tcl new file mode 100644 index 00000000000..037be82939b --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/axis-W.tcl @@ -0,0 +1,35 @@ +set wv_id 8 +set wa_id 18 + +# Add axis components to servo thread +addf wcomp.$wv_id servo-thread +addf wcomp.$wa_id servo-thread +addf minmax.$wv_id servo-thread +addf minmax.$wa_id servo-thread +addf d2dt2.$wv_id servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net w_pos axis.$wv_id.motor-pos-cmd => axis.$wv_id.motor-pos-fb d2dt2.$wv_id.in + +# send the position commands thru differentiators to +# generate velocity and accel signals +net w_vel <= d2dt2.$wv_id.out1 => wcomp.$wv_id.in minmax.$wv_id.in +net w_acc <= d2dt2.$wv_id.out2 => wcomp.$wa_id.in minmax.$wa_id.in + +#Conservative limits +set acc_limit 1.0001 +set vel_limit 1.01 + +setp wcomp.$wa_id.max $::AXIS_8(MAX_ACCELERATION)*$acc_limit +setp wcomp.$wa_id.min $::AXIS_8(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$wv_id.max $::AXIS_8(MAX_VELOCITY)*$vel_limit +setp wcomp.$wv_id.min $::AXIS_8(MAX_VELOCITY)*-1.0*$vel_limit + +net w_acc-ok <= wcomp.$wv_id.out => match_uvw.a4 +net w_vel-ok <= wcomp.$wa_id.out => match_uvw.a5 + +# Enable match_all pins for axis +setp match_uvw.b4 1 +setp match_uvw.b5 1 + diff --git a/tests/trajectory-planner/circular-arcs/configs/axis-X.tcl b/tests/trajectory-planner/circular-arcs/configs/axis-X.tcl index e39ca202ce2..0c549683595 100644 --- a/tests/trajectory-planner/circular-arcs/configs/axis-X.tcl +++ b/tests/trajectory-planner/circular-arcs/configs/axis-X.tcl @@ -1,21 +1,35 @@ +set xv_id 0 +set xa_id 10 + +# Add axis components to servo thread +addf wcomp.$xv_id servo-thread +addf wcomp.$xa_id servo-thread +addf minmax.$xv_id servo-thread +addf minmax.$xa_id servo-thread +addf d2dt2.$xv_id servo-thread + # create HAL signals for position commands from motion module # loop position commands back to motion module feedback -net Xpos axis.0.motor-pos-cmd => axis.0.motor-pos-fb ddt_x.in +net x_pos axis.$xv_id.motor-pos-cmd => axis.$xv_id.motor-pos-fb d2dt2.$xv_id.in # send the position commands thru differentiators to # generate velocity and accel signals -net Xvel ddt_x.out => ddt_xv.in vel_xy.in0 -net Xacc <= ddt_xv.out +net x_vel <= d2dt2.$xv_id.out1 => vel_xyz.in$xv_id wcomp.$xv_id.in minmax.$xv_id.in +net x_acc <= d2dt2.$xv_id.out2 => wcomp.$xa_id.in minmax.$xa_id.in #Conservative limits set acc_limit 1.0001 set vel_limit 1.01 -setp wcomp_xacc.max $::AXIS_0(MAX_ACCELERATION)*$acc_limit -setp wcomp_xacc.min $::AXIS_0(MAX_ACCELERATION)*-1.0*$acc_limit -setp wcomp_xvel.max $::AXIS_0(MAX_VELOCITY)*$vel_limit -setp wcomp_xvel.min $::AXIS_0(MAX_VELOCITY)*-1.0*$vel_limit +setp wcomp.$xa_id.max $::AXIS_0(MAX_ACCELERATION)*$acc_limit +setp wcomp.$xa_id.min $::AXIS_0(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$xv_id.max $::AXIS_0(MAX_VELOCITY)*$vel_limit +setp wcomp.$xv_id.min $::AXIS_0(MAX_VELOCITY)*-1.0*$vel_limit + +net x_acc-ok <= wcomp.$xv_id.out => match_xyz.a0 +net x_vel-ok <= wcomp.$xa_id.out => match_xyz.a1 + +# Enable match_all pins for axis +setp match_xyz.b0 1 +setp match_xyz.b1 1 -# Enable match_all pins for X axis -setp match_all.b0 1 -setp match_all.b1 1 diff --git a/tests/trajectory-planner/circular-arcs/configs/axis-Y.tcl b/tests/trajectory-planner/circular-arcs/configs/axis-Y.tcl index e18ff0207ff..0c393ed57c6 100644 --- a/tests/trajectory-planner/circular-arcs/configs/axis-Y.tcl +++ b/tests/trajectory-planner/circular-arcs/configs/axis-Y.tcl @@ -1,21 +1,35 @@ +set yv_id 1 +set ya_id 11 + +# Add axis components to servo thread +addf wcomp.$yv_id servo-thread +addf wcomp.$ya_id servo-thread +addf minmax.$yv_id servo-thread +addf minmax.$ya_id servo-thread +addf d2dt2.$yv_id servo-thread + # create HAL signals for position commands from motion module # loop position commands back to motion module feedback -net Ypos axis.1.motor-pos-cmd => axis.1.motor-pos-fb ddt_y.in +net y_pos axis.$yv_id.motor-pos-cmd => axis.$yv_id.motor-pos-fb d2dt2.$yv_id.in # send the position commands thru differentiators to # generate velocity and accel signals -net Yvel ddt_y.out => ddt_yv.in vel_xy.in1 -net Yacc <= ddt_yv.out +net y_vel <= d2dt2.$yv_id.out1 => vel_xyz.in$yv_id wcomp.$yv_id.in minmax.$yv_id.in +net y_acc <= d2dt2.$yv_id.out2 => wcomp.$ya_id.in minmax.$ya_id.in #Conservative limits set acc_limit 1.0001 set vel_limit 1.01 -setp wcomp_yacc.max $::AXIS_1(MAX_ACCELERATION)*$acc_limit -setp wcomp_yacc.min $::AXIS_1(MAX_ACCELERATION)*-1.0*$acc_limit -setp wcomp_yvel.max $::AXIS_1(MAX_VELOCITY)*$vel_limit -setp wcomp_yvel.min $::AXIS_1(MAX_VELOCITY)*-1.0*$vel_limit +setp wcomp.$ya_id.max $::AXIS_1(MAX_ACCELERATION)*$acc_limit +setp wcomp.$ya_id.min $::AXIS_1(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$yv_id.max $::AXIS_1(MAX_VELOCITY)*$vel_limit +setp wcomp.$yv_id.min $::AXIS_1(MAX_VELOCITY)*-1.0*$vel_limit + +net y_acc-ok <= wcomp.$yv_id.out => match_xyz.a2 +net y_vel-ok <= wcomp.$ya_id.out => match_xyz.a3 + +# Enable match_all pins for axis +setp match_xyz.b2 1 +setp match_xyz.b3 1 -# Enable match_all pins for Y axis -setp match_all.b2 1 -setp match_all.b3 1 diff --git a/tests/trajectory-planner/circular-arcs/configs/axis-Z.tcl b/tests/trajectory-planner/circular-arcs/configs/axis-Z.tcl index 2e166eb624c..87e186ce971 100644 --- a/tests/trajectory-planner/circular-arcs/configs/axis-Z.tcl +++ b/tests/trajectory-planner/circular-arcs/configs/axis-Z.tcl @@ -1,22 +1,34 @@ +set zv_id 2 +set za_id 12 + +# Add axis components to servo thread +addf wcomp.$zv_id servo-thread +addf wcomp.$za_id servo-thread +addf minmax.$zv_id servo-thread +addf minmax.$za_id servo-thread +addf d2dt2.$zv_id servo-thread + # create HAL signals for position commands from motion module # loop position commands back to motion module feedback -net Zpos axis.2.motor-pos-cmd => axis.2.motor-pos-fb ddt_z.in +net z_pos axis.$zv_id.motor-pos-cmd => axis.$zv_id.motor-pos-fb d2dt2.$zv_id.in # send the position commands thru differentiators to # generate velocity and accel signals -net Zvel ddt_z.out => ddt_zv.in vel_xyz.in0 -net Zacc <= ddt_zv.out +net z_vel <= d2dt2.$zv_id.out1 => vel_xyz.in$zv_id wcomp.$zv_id.in minmax.$zv_id.in +net z_acc <= d2dt2.$zv_id.out2 => wcomp.$za_id.in minmax.$za_id.in #Conservative limits set acc_limit 1.0001 set vel_limit 1.01 -setp wcomp_zacc.max $::AXIS_2(MAX_ACCELERATION)*$acc_limit -setp wcomp_zacc.min $::AXIS_2(MAX_ACCELERATION)*-1.0*$acc_limit -setp wcomp_zvel.max $::AXIS_2(MAX_VELOCITY)*$vel_limit -setp wcomp_zvel.min $::AXIS_2(MAX_VELOCITY)*-1.0*$vel_limit +setp wcomp.$za_id.max $::AXIS_2(MAX_ACCELERATION)*$acc_limit +setp wcomp.$za_id.min $::AXIS_2(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$zv_id.max $::AXIS_2(MAX_VELOCITY)*$vel_limit +setp wcomp.$zv_id.min $::AXIS_2(MAX_VELOCITY)*-1.0*$vel_limit -# Enable match_all pins for Z axis -setp match_all.b4 1 -setp match_all.b5 1 +net z_acc-ok <= wcomp.$zv_id.out => match_xyz.a4 +net z_vel-ok <= wcomp.$za_id.out => match_xyz.a5 +# Enable match_all pins for axis +setp match_xyz.b4 1 +setp match_xyz.b5 1 diff --git a/tests/trajectory-planner/circular-arcs/configs/axis_manualtoolchange.hal b/tests/trajectory-planner/circular-arcs/configs/axis_manualtoolchange.hal index 356965ee0ad..ba56cdd7fc7 120000 --- a/tests/trajectory-planner/circular-arcs/configs/axis_manualtoolchange.hal +++ b/tests/trajectory-planner/circular-arcs/configs/axis_manualtoolchange.hal @@ -1 +1 @@ -../../../../configs/sim/axis_manualtoolchange.hal \ No newline at end of file +../../../../lib/hallib/axis_manualtoolchange.hal \ No newline at end of file diff --git a/tests/trajectory-planner/circular-arcs/configs/core_sim9.hal b/tests/trajectory-planner/circular-arcs/configs/core_sim9.hal deleted file mode 120000 index 578484434cc..00000000000 --- a/tests/trajectory-planner/circular-arcs/configs/core_sim9.hal +++ /dev/null @@ -1 +0,0 @@ -../../../../configs/sim/core_sim9.hal \ No newline at end of file diff --git a/tests/trajectory-planner/circular-arcs/configs/core_sim_components.hal b/tests/trajectory-planner/circular-arcs/configs/core_sim_components.hal index fad98ec4126..7f32d4a7384 100644 --- a/tests/trajectory-planner/circular-arcs/configs/core_sim_components.hal +++ b/tests/trajectory-planner/circular-arcs/configs/core_sim_components.hal @@ -5,40 +5,11 @@ loadrt trivkins # motion controller, get name and thread periods from ini file loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[TRAJ]AXES -# load 6 differentiators (for velocity and accel signals -loadrt ddt names=ddt_x,ddt_xv,ddt_y,ddt_yv,ddt_z,ddt_zv,ddt_a,ddt_av -# load additional blocks -loadrt hypot names=vel_xy,vel_xyz # add motion controller functions to servo thread addf motion-command-handler servo-thread addf motion-controller servo-thread -# link the differentiator functions into the code -addf ddt_x servo-thread -addf ddt_xv servo-thread -addf ddt_y servo-thread -addf ddt_yv servo-thread -addf ddt_z servo-thread -addf ddt_zv servo-thread -addf ddt_a servo-thread -addf ddt_av servo-thread - -addf vel_xy servo-thread -addf vel_xyz servo-thread - -# create HAL signals for position commands from motion module -# loop position commands back to motion module feedback -# NOTE handled in axis-[XYZ].hal - -# send the position commands thru differentiators to -# generate velocity and accel signals -# NOTE handled in axis-[XYZ].hal - -# Cartesian 2- and 3-axis velocities -net XYvel vel_xy.out => vel_xyz.in1 -net XYZvel <= vel_xyz.out - # estop loopback net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in diff --git a/tests/trajectory-planner/circular-arcs/configs/XZ.ini b/tests/trajectory-planner/circular-arcs/configs/lathe_XZ.ini similarity index 97% rename from tests/trajectory-planner/circular-arcs/configs/XZ.ini rename to tests/trajectory-planner/circular-arcs/configs/lathe_XZ.ini index 974a24d4e2e..5fecf1a51d5 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XZ.ini +++ b/tests/trajectory-planner/circular-arcs/configs/lathe_XZ.ini @@ -19,7 +19,7 @@ DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] #Includes acceleration / velocity -PYVCP = vcp.xml +PYVCP = vcp-XZ.xml # Name of display program, e.g., xemc DISPLAY = axis @@ -72,6 +72,7 @@ CYCLE_TIME = 0.001 # File containing interpreter variables PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs # Motion control section ------------------------------------------------------ [EMCMOT] @@ -100,11 +101,10 @@ SERVO_PERIOD = 1000000 # files are executed in the order in which they appear # Core sim stuff for various loaded axes HALFILE = core_sim_components.hal -HALFILE = test_status.tcl +HALFILE = load_constraint_components.tcl HALFILE = axis-X.tcl HALFILE = axis-Z.tcl # Other HAL files -HALFILE = axis_manualtoolchange.hal HALFILE = sim_spindle_encoder.hal # list of halcmd commands to execute @@ -113,7 +113,7 @@ HALFILE = sim_spindle_encoder.hal # Single file that is executed after the GUI has started. Only supported by # AXIS at this time (only AXIS creates a HAL component of its own) -POSTGUI_HALFILE = postgui.hal +POSTGUI_HALFILE = postgui_XZ.tcl HALUI = halui diff --git a/tests/trajectory-planner/circular-arcs/configs/load_constraint_components.tcl b/tests/trajectory-planner/circular-arcs/configs/load_constraint_components.tcl new file mode 100644 index 00000000000..5fa13c938db --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/load_constraint_components.tcl @@ -0,0 +1,57 @@ +# Load all constraint-checking components used in HAL files for various sim configurations +# NOTE: add only the actively used ones to the RT thread in axis TCL files + +loadrt d2dt2 count=10 +# load additional blocks +loadrt hypot names=vel_xy,vel_xyz +addf vel_xy servo-thread +addf vel_xyz servo-thread + +# Compares computed velocities / accelerations to limits +loadrt wcomp count=20 + +# Track the overall min / max value of each signal reached (for VCP display) +loadrt minmax count=20 + +# Ties all limit checks together (used to force a motion enable / disable) +loadrt match8 names=match_xyz,match_abc,match_uvw,match_all + +net XYZ-limits-ok <= match_xyz.out => match_all.a0 +net ABC-limits-ok <= match_abc.out => match_all.a1 +net UVW-limits-ok <= match_uvw.out => match_all.a2 + +setp match_xyz.b0 0 +setp match_xyz.b1 0 +setp match_xyz.b2 0 +setp match_xyz.b3 0 +setp match_xyz.b4 0 +setp match_xyz.b5 0 +setp match_xyz.b6 0 +setp match_xyz.b7 0 + +setp match_abc.b0 0 +setp match_abc.b1 0 +setp match_abc.b2 0 +setp match_abc.b3 0 +setp match_abc.b4 0 +setp match_abc.b5 0 +setp match_abc.b6 0 +setp match_abc.b7 0 + +setp match_uvw.b0 0 +setp match_uvw.b1 0 +setp match_uvw.b2 0 +setp match_uvw.b3 0 +setp match_uvw.b4 0 +setp match_uvw.b5 0 +setp match_uvw.b6 0 +setp match_uvw.b7 0 + +setp match_all.b0 1 +setp match_all.b1 1 +setp match_all.b2 1 +setp match_all.b3 0 +setp match_all.b4 0 +setp match_all.b5 0 +setp match_all.b6 0 +setp match_all.b7 0 diff --git a/tests/trajectory-planner/circular-arcs/configs/XYZ.ini b/tests/trajectory-planner/circular-arcs/configs/mill_XYZA.ini similarity index 96% rename from tests/trajectory-planner/circular-arcs/configs/XYZ.ini rename to tests/trajectory-planner/circular-arcs/configs/mill_XYZA.ini index 9c527b7337d..2a656822e84 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XYZ.ini +++ b/tests/trajectory-planner/circular-arcs/configs/mill_XYZA.ini @@ -18,9 +18,9 @@ DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] -PYVCP = vcp.xml +PYVCP = vcp-XYZA.xml # Name of display program, e.g., xemc -DISPLAY = axis +DISPLAY = mini # Cycle time, in seconds, that display will sleep between polls CYCLE_TIME = 0.066666666666 @@ -48,6 +48,8 @@ TOOL_EDITOR = tooledit INCREMENTS = 1 in, 0.1 in, 10 mil, 1 mil, 1mm, .1mm, 1/8000 in +GEOMETRY = XYZA + [FILTER] PROGRAM_EXTENSION = .png,.gif,.jpg Grayscale Depth Image PROGRAM_EXTENSION = .py Python Script @@ -71,6 +73,7 @@ CYCLE_TIME = 0.001 # File containing interpreter variables PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs # Motion control section ------------------------------------------------------ [EMCMOT] @@ -98,7 +101,7 @@ SERVO_PERIOD = 1000000 # list of hal config files to run through halcmd # files are executed in the order in which they appear HALFILE = core_sim_components.hal -HALFILE = test_status.tcl +HALFILE = load_constraint_components.tcl HALFILE = axis-X.tcl HALFILE = axis-Y.tcl HALFILE = axis-Z.tcl @@ -112,7 +115,7 @@ HALFILE = sim_spindle_encoder.hal # Single file that is executed after the GUI has started. Only supported by # AXIS at this time (only AXIS creates a HAL component of its own) -POSTGUI_HALFILE = postgui.hal +POSTGUI_HALFILE = postgui-XYZA.tcl HALUI = halui @@ -121,7 +124,7 @@ HALUI = halui AXES = 4 COORDINATES = X Y Z A -HOME = 0 0 0 +HOME = 0 0 0 0 LINEAR_UNITS = inch ANGULAR_UNITS = degree CYCLE_TIME = 0.010 @@ -227,3 +230,6 @@ CYCLE_TIME = 0.066666666666 TOOL_TABLE = sim.tbl TOOL_CHANGE_POSITION = 0 0 0 TOOL_CHANGE_QUILL_UP = 1 + +[REDIS] +DISABLE_SERVER=1 diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-A.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-A.tcl new file mode 100644 index 00000000000..c7135c8543a --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-A.tcl @@ -0,0 +1,5 @@ +net paacc minmax.13.max pyvcp.paacc +net naacc minmax.13.min pyvcp.naacc + +net pavel minmax.3.max pyvcp.pavel +net navel minmax.3.min pyvcp.navel diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-B.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-B.tcl new file mode 100644 index 00000000000..478759c5f82 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-B.tcl @@ -0,0 +1,6 @@ + +net pbacc minmax.14.max pyvcp.pbacc +net nbacc minmax.14.min pyvcp.nbacc + +net pbvel minmax.4.max pyvcp.pbvel +net nbvel minmax.4.min pyvcp.nbvel diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-C.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-C.tcl new file mode 100644 index 00000000000..b26e7928e57 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-C.tcl @@ -0,0 +1,5 @@ +net pcacc minmax.15.max pyvcp.pcacc +net ncacc minmax.15.min pyvcp.ncacc + +net pcvel minmax.5.max pyvcp.pcvel +net ncvel minmax.5.min pyvcp.ncvel diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-U.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-U.tcl new file mode 100644 index 00000000000..a109ebac3c7 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-U.tcl @@ -0,0 +1,5 @@ +net puacc minmax.16.max pyvcp.puacc +net nuacc minmax.16.min pyvcp.nuacc + +net puvel minmax.6.max pyvcp.puvel +net nuvel minmax.6.min pyvcp.nuvel diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-V.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-V.tcl new file mode 100644 index 00000000000..47b20bfb004 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-V.tcl @@ -0,0 +1,5 @@ +net pvacc minmax.17.max pyvcp.pvacc +net nvacc minmax.17.min pyvcp.nvacc + +net pvvel minmax.7.max pyvcp.pvvel +net nvvel minmax.7.min pyvcp.nvvel diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-W.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-W.tcl new file mode 100644 index 00000000000..7c47ef29bfc --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-W.tcl @@ -0,0 +1,5 @@ +net pwacc minmax.18.max pyvcp.pwacc +net nwacc minmax.18.min pyvcp.nwacc + +net pwvel minmax.8.max pyvcp.pwvel +net nwvel minmax.8.min pyvcp.nwvel diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-X.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-X.tcl new file mode 100644 index 00000000000..e3d58c6915d --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-X.tcl @@ -0,0 +1,5 @@ +net pxacc minmax.10.max pyvcp.pxacc +net nxacc minmax.10.min pyvcp.nxacc + +net pxvel minmax.0.max pyvcp.pxvel +net nxvel minmax.0.min pyvcp.nxvel diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-XYZA.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-XYZA.tcl new file mode 100644 index 00000000000..bf18663953e --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-XYZA.tcl @@ -0,0 +1,5 @@ +source postgui-base.tcl +source postgui-X.tcl +source postgui-Y.tcl +source postgui-Z.tcl +source postgui-A.tcl diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-XYZABC.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-XYZABC.tcl new file mode 100644 index 00000000000..a3c91db0c34 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-XYZABC.tcl @@ -0,0 +1,3 @@ +source postgui-XYZA.tcl +source postgui-B.tcl +source postgui-C.tcl diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-XYZABCUVW.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-XYZABCUVW.tcl new file mode 100644 index 00000000000..be4e9a1624c --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-XYZABCUVW.tcl @@ -0,0 +1,4 @@ +source postgui-XYZABC.tcl +source postgui-U.tcl +source postgui-V.tcl +source postgui-W.tcl diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-XZ.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-XZ.tcl new file mode 100644 index 00000000000..de58c0c86fc --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-XZ.tcl @@ -0,0 +1,3 @@ +source postgui-base.tcl +source postgui-X.tcl +source postgui-Z.tcl diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-Y.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-Y.tcl new file mode 100644 index 00000000000..1b1aa7aed0c --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-Y.tcl @@ -0,0 +1,5 @@ +net pyacc minmax.11.max pyvcp.pyacc +net nyacc minmax.11.min pyvcp.nyacc + +net pyvel minmax.1.max pyvcp.pyvel +net nyvel minmax.1.min pyvcp.nyvel diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-Z.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-Z.tcl new file mode 100644 index 00000000000..71284baee8b --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-Z.tcl @@ -0,0 +1,5 @@ +net pzacc minmax.12.max pyvcp.pzacc +net nzacc minmax.12.min pyvcp.nzacc + +net pzvel minmax.2.max pyvcp.pzvel +net nzvel minmax.2.min pyvcp.nzvel diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-base.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-base.tcl new file mode 100644 index 00000000000..d2bd2925de3 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-base.tcl @@ -0,0 +1,14 @@ +# net constraints-ok <= match_all.out => motion.enable + +net resetall minmax.0.reset minmax.1.reset minmax.2.reset minmax.3.reset minmax.4.reset minmax.5.reset minmax.6.reset minmax.7.reset minmax.8.reset minmax.9.reset minmax.10.reset minmax.11.reset minmax.12.reset minmax.13.reset minmax.14.reset minmax.15.reset minmax.16.reset minmax.17.reset minmax.18.reset minmax.19.reset => pyvcp.resetmm + +loadrt time +loadrt not +addf time.0 servo-thread +addf not.0 servo-thread +net prog-running not.0.in <= halui.program.is-idle +net cycle-timer time.0.start <= not.0.out +net cycle-seconds pyvcp.time-seconds <= time.0.seconds +net cycle-minutes pyvcp.time-minutes <= time.0.minutes +net cycle-hours pyvcp.time-hours <= time.0.hours + diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui.hal b/tests/trajectory-planner/circular-arcs/configs/postgui.hal deleted file mode 100644 index 6c376e7fc06..00000000000 --- a/tests/trajectory-planner/circular-arcs/configs/postgui.hal +++ /dev/null @@ -1,35 +0,0 @@ -#NOTE include appropriate postgui-axis-[XYZ].hal here for each axis used - -net constraints-ok <= match_all.out => motion.enable - -net resetall mm_xacc.reset mm_xvel.reset mm_yacc.reset mm_yvel.reset mm_zacc.reset mm_zvel.reset mm_aacc.reset mm_avel.reset pyvcp.resetmm - -loadrt time -loadrt not -addf time.0 servo-thread -addf not.0 servo-thread -net prog-running not.0.in <= halui.program.is-idle -net cycle-timer time.0.start <= not.0.out -net cycle-seconds pyvcp.time-seconds <= time.0.seconds -net cycle-minutes pyvcp.time-minutes <= time.0.minutes -net cycle-hours pyvcp.time-hours <= time.0.hours - -net pxacc mm_xacc.max pyvcp.pxacc -net pxvel mm_xvel.max pyvcp.pxvel -net nxacc mm_xacc.min pyvcp.nxacc -net nxvel mm_xvel.min pyvcp.nxvel - -net pyacc mm_yacc.max pyvcp.pyacc -net pyvel mm_yvel.max pyvcp.pyvel -net nyacc mm_yacc.min pyvcp.nyacc -net nyvel mm_yvel.min pyvcp.nyvel - -net pzacc mm_zacc.max pyvcp.pzacc -net pzvel mm_zvel.max pyvcp.pzvel -net nzacc mm_zacc.min pyvcp.nzacc -net nzvel mm_zvel.min pyvcp.nzvel - -net paacc mm_aacc.max pyvcp.paacc -net pavel mm_avel.max pyvcp.pavel -net naacc mm_aacc.min pyvcp.naacc -net navel mm_avel.min pyvcp.navel diff --git a/tests/trajectory-planner/circular-arcs/configs/test_XYZA_all_programs.ini b/tests/trajectory-planner/circular-arcs/configs/test_XYZA_all_programs.ini new file mode 100644 index 00000000000..f38662b63c8 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/test_XYZA_all_programs.ini @@ -0,0 +1,7 @@ +#INCLUDE autotest_mill_XYZA_template.ini + +# Choose the test program to run as the "display" which +# runs a headless instance, to test everything downstream of the GUI +[DISPLAY] +DISPLAY = ./test_run_all_programs.py --base-dir ../nc_files/auto_test + diff --git a/tests/trajectory-planner/circular-arcs/configs/test_run_all_programs.py b/tests/trajectory-planner/circular-arcs/configs/test_run_all_programs.py new file mode 100755 index 00000000000..2664aba0790 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/test_run_all_programs.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python +""" +Based on the control script in m61_test +""" + +import linuxcnc +from linuxcnc_control import LinuxcncControl +import sys + +from os import listdir +from os.path import isfile, join +import re +import glob + + +def find_test_nc_files(testpath='../nc_files', show=False): + files = [join(testpath, f) for f in listdir(testpath) if isfile(join(testpath, f)) and re.search('.ngc$|.nc$|.TAP$', f) is not None] + if show: + print "In folder {0}, found {1} files:".format(testpath,len(files)) + for fname in files: + print fname + return files + + +if __name__ == '__main__': + """ Run batch tests (user specifies which ones interactively) """ + run_sts = 0 + + # raw_input("Press any key when the LinuxCNC GUI has loaded") + # KLUDGE this lists all subfolders in the auto-test directory + # this dir should be populated with symlinks to any folders of test files to + # run + test_folders = glob.glob(join('nc_files', '*')) + + test_files = [] + + for f in test_folders: + test_files.extend(find_test_nc_files(f, True)) + + e = LinuxcncControl(continue_after_error=args.continue_after_error) + + e.do_startup(need_home=True) + + error_list = [] + for f in test_files: + e.write_error_log() + + print "Loading program {0}".format(f) + e.set_mode(linuxcnc.MODE_AUTO) + e.open_program(f) + e.set_mode(linuxcnc.MODE_AUTO) + e.run_full_program() + if not e.wait_on_program(f) and e.continue_after_error: + e.do_startup(need_home=False) + else: + break + + # KLUDGE just in case any others creep in + e.flush_errors() + e.write_error_log() + sys.exit(e.has_error()) diff --git a/tests/trajectory-planner/circular-arcs/configs/vcp-XYZA.xml b/tests/trajectory-planner/circular-arcs/configs/vcp-XYZA.xml new file mode 100644 index 00000000000..8d3e6dcb26b --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/vcp-XYZA.xml @@ -0,0 +1,148 @@ + + + + + + + + + + + + + + + "pxacc" + ("Helvetica",24) + "+4.3f" + + + "nxacc" + ("Helvetica",24) + "+4.3f" + + + "pxvel" + ("Helvetica",24) + "+4.2f" + + + "nxvel" + ("Helvetica",24) + "+4.2f" + + + + + "pyacc" + ("Helvetica",24) + "+4.3f" + + + "nyacc" + ("Helvetica",24) + "+4.3f" + + + "pyvel" + ("Helvetica",24) + "+4.2f" + + + "nyvel" + ("Helvetica",24) + "+4.2f" + + + + + "pzacc" + ("Helvetica",24) + "+4.3f" + + + "nzacc" + ("Helvetica",24) + "+4.3f" + + + "pzvel" + ("Helvetica",24) + "+4.2f" + + + "nzvel" + ("Helvetica",24) + "+4.2f" + + + + + "paacc" + ("Helvetica",24) + "+4.3f" + + + "naacc" + ("Helvetica",24) + "+4.3f" + + + "pavel" + ("Helvetica",24) + "+4.2f" + + + "navel" + ("Helvetica",24) + "+4.2f" + +
+ + + + + + "time-hours" + ("Helvetica",14) + ".2d :" + + + "time-minutes" + ("Helvetica",14) + ".2d :" + + + "time-seconds" + ("Helvetica",14) + ".2d" + + + diff --git a/tests/trajectory-planner/circular-arcs/configs/vcp-XYZABC.xml b/tests/trajectory-planner/circular-arcs/configs/vcp-XYZABC.xml new file mode 100644 index 00000000000..2ffc71e3033 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/vcp-XYZABC.xml @@ -0,0 +1,198 @@ + + + + + + + + + + + + + + + "pxacc" + ("Helvetica",24) + "+4.3f" + + + "nxacc" + ("Helvetica",24) + "+4.3f" + + + "pxvel" + ("Helvetica",24) + "+4.2f" + + + "nxvel" + ("Helvetica",24) + "+4.2f" + + + + + "pyacc" + ("Helvetica",24) + "+4.3f" + + + "nyacc" + ("Helvetica",24) + "+4.3f" + + + "pyvel" + ("Helvetica",24) + "+4.2f" + + + "nyvel" + ("Helvetica",24) + "+4.2f" + + + + + "pzacc" + ("Helvetica",24) + "+4.3f" + + + "nzacc" + ("Helvetica",24) + "+4.3f" + + + "pzvel" + ("Helvetica",24) + "+4.2f" + + + "nzvel" + ("Helvetica",24) + "+4.2f" + + + + + "paacc" + ("Helvetica",24) + "+4.3f" + + + "naacc" + ("Helvetica",24) + "+4.3f" + + + "pavel" + ("Helvetica",24) + "+4.2f" + + + "navel" + ("Helvetica",24) + "+4.2f" + + + + + "pbacc" + ("Helvetica",24) + "+4.3f" + + + "nbacc" + ("Helvetica",24) + "+4.3f" + + + "pbvel" + ("Helvetica",24) + "+4.2f" + + + "nbvel" + ("Helvetica",24) + "+4.2f" + + + + + "pcacc" + ("Helvetica",24) + "+4.3f" + + + "ncacc" + ("Helvetica",24) + "+4.3f" + + + "pcvel" + ("Helvetica",24) + "+4.2f" + + + "ncvel" + ("Helvetica",24) + "+4.2f" + +
+ + + + + + "time-hours" + ("Helvetica",14) + ".2d :" + + + "time-minutes" + ("Helvetica",14) + ".2d :" + + + "time-seconds" + ("Helvetica",14) + ".2d" + + + diff --git a/tests/trajectory-planner/circular-arcs/configs/vcp-XYZABCUVW.xml b/tests/trajectory-planner/circular-arcs/configs/vcp-XYZABCUVW.xml new file mode 100644 index 00000000000..57f3a326566 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/vcp-XYZABCUVW.xml @@ -0,0 +1,273 @@ + + + + + + + + + + + + + + + "pxacc" + ("Helvetica",24) + "+4.3f" + + + "nxacc" + ("Helvetica",24) + "+4.3f" + + + "pxvel" + ("Helvetica",24) + "+4.2f" + + + "nxvel" + ("Helvetica",24) + "+4.2f" + + + + + "pyacc" + ("Helvetica",24) + "+4.3f" + + + "nyacc" + ("Helvetica",24) + "+4.3f" + + + "pyvel" + ("Helvetica",24) + "+4.2f" + + + "nyvel" + ("Helvetica",24) + "+4.2f" + + + + + "pzacc" + ("Helvetica",24) + "+4.3f" + + + "nzacc" + ("Helvetica",24) + "+4.3f" + + + "pzvel" + ("Helvetica",24) + "+4.2f" + + + "nzvel" + ("Helvetica",24) + "+4.2f" + + + + + "paacc" + ("Helvetica",24) + "+4.3f" + + + "naacc" + ("Helvetica",24) + "+4.3f" + + + "pavel" + ("Helvetica",24) + "+4.2f" + + + "navel" + ("Helvetica",24) + "+4.2f" + + + + + "pbacc" + ("Helvetica",24) + "+4.3f" + + + "nbacc" + ("Helvetica",24) + "+4.3f" + + + "pbvel" + ("Helvetica",24) + "+4.2f" + + + "nbvel" + ("Helvetica",24) + "+4.2f" + + + + + "pcacc" + ("Helvetica",24) + "+4.3f" + + + "ncacc" + ("Helvetica",24) + "+4.3f" + + + "pcvel" + ("Helvetica",24) + "+4.2f" + + + "ncvel" + ("Helvetica",24) + "+4.2f" + + + + + "puacc" + ("Helvetica",24) + "+4.3f" + + + "nuacc" + ("Helvetica",24) + "+4.3f" + + + "puvel" + ("Helvetica",24) + "+4.2f" + + + "nuvel" + ("Helvetica",24) + "+4.2f" + + + + + "pvacc" + ("Helvetica",24) + "+4.3f" + + + "nvacc" + ("Helvetica",24) + "+4.3f" + + + "pvvel" + ("Helvetica",24) + "+4.2f" + + + "nvvel" + ("Helvetica",24) + "+4.2f" + + + + + "pwacc" + ("Helvetica",24) + "+4.3f" + + + "nwacc" + ("Helvetica",24) + "+4.3f" + + + "pwvel" + ("Helvetica",24) + "+4.2f" + + + "nwvel" + ("Helvetica",24) + "+4.2f" + +
+ + + + + + "time-hours" + ("Helvetica",14) + ".2d :" + + + "time-minutes" + ("Helvetica",14) + ".2d :" + + + "time-seconds" + ("Helvetica",14) + ".2d" + + + diff --git a/tests/trajectory-planner/circular-arcs/configs/vcp-XZ.xml b/tests/trajectory-planner/circular-arcs/configs/vcp-XZ.xml new file mode 100644 index 00000000000..4c2cd304a00 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/vcp-XZ.xml @@ -0,0 +1,98 @@ + + + + + + + + + + + + + + + "pxacc" + ("Helvetica",24) + "+4.3f" + + + "nxacc" + ("Helvetica",24) + "+4.3f" + + + "pxvel" + ("Helvetica",24) + "+4.2f" + + + "nxvel" + ("Helvetica",24) + "+4.2f" + + + + + "pzacc" + ("Helvetica",24) + "+4.3f" + + + "nzacc" + ("Helvetica",24) + "+4.3f" + + + "pzvel" + ("Helvetica",24) + "+4.2f" + + + "nzvel" + ("Helvetica",24) + "+4.2f" + +
+ + + + + + "time-hours" + ("Helvetica",14) + ".2d :" + + + "time-minutes" + ("Helvetica",14) + ".2d :" + + + "time-seconds" + ("Helvetica",14) + ".2d" + + + diff --git a/tests/trajectory-planner/circular-arcs/configs/vcp.xml b/tests/trajectory-planner/circular-arcs/configs/vcp.xml index ae91989fc32..58f629a01b4 100644 --- a/tests/trajectory-planner/circular-arcs/configs/vcp.xml +++ b/tests/trajectory-planner/circular-arcs/configs/vcp.xml @@ -1,171 +1,74 @@ - - - - - - "pxacc" - ("Helvetica",24) - "+4.4f" - - - - "pxvel" - ("Helvetica",24) - "+4.4f" - - - - - - "nxacc" - ("Helvetica",24) - "+4.4f" - - - - "nxvel" - ("Helvetica",24) - "+4.4f" - - - - - - - "pyacc" - ("Helvetica",24) - "+4.4f" - - - - "pyvel" - ("Helvetica",24) - "+4.4f" - - - - - - "nyacc" - ("Helvetica",24) - "+4.4f" - - - - "nyvel" - ("Helvetica",24) - "+4.4f" - - - - - - - "pzacc" - ("Helvetica",24) - "+4.4f" - - - - "pzvel" - ("Helvetica",24) - "+4.4f" - - - - - - "nzacc" - ("Helvetica",24) - "+4.4f" - - - - "nzvel" - ("Helvetica",24) - "+4.4f" - - - - - - - - - "paacc" - ("Helvetica",24) - "+4.4f" - - - - "pavel" - ("Helvetica",24) - "+4.4f" - - - - - - "naacc" - ("Helvetica",24) - "+4.4f" - - - - "navel" - ("Helvetica",24) - "+4.4f" - - - - + + + + + + + + + + + + + "pxacc" + ("Helvetica",24) + "+4.3f" + + + "nxacc" + ("Helvetica",24) + "+4.3f" + + + "pxvel" + ("Helvetica",24) + "+4.2f" + + + "nxvel" + ("Helvetica",24) + "+4.2f" + + + + + "pyacc" + ("Helvetica",24) + "+4.3f" + + + "nyacc" + ("Helvetica",24) + "+4.3f" + + + "pyvel" + ("Helvetica",24) + "+4.2f" + + + "nyvel" + ("Helvetica",24) + "+4.2f" + +
+ + diff --git a/tests/trajectory-planner/circular-arcs/extract_all.sh b/tests/trajectory-planner/circular-arcs/extract_all.sh new file mode 100755 index 00000000000..83d16d19d47 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/extract_all.sh @@ -0,0 +1,7 @@ +#!/usr/bin/env bash +LOGFILE=${1-test.log} +echo "Extracting data from $LOGFILE" +for f in spindle tc_state displacement +do + awk -f extract_"$f".awk $LOGFILE > ./octave/"$f"_data.txt +done diff --git a/tests/trajectory-planner/circular-arcs/extract_displacement.awk b/tests/trajectory-planner/circular-arcs/extract_displacement.awk new file mode 100644 index 00000000000..cbeae5cecb5 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/extract_displacement.awk @@ -0,0 +1,2 @@ +#EXAMPLE: tp_displacement = -9.497722863738e-06 0.000000000000e+00 -9.506120496661e-06 time = 2.108000000000e+00 +/tp_displacement/{print $8","$3","$4","$5} diff --git a/tests/trajectory-planner/circular-arcs/extract_spindle.awk b/tests/trajectory-planner/circular-arcs/extract_spindle.awk new file mode 100644 index 00000000000..3d8f7a4f7a2 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/extract_spindle.awk @@ -0,0 +1 @@ +/spindle_displacement/{print $2$4$6$8$10} diff --git a/tests/trajectory-planner/circular-arcs/extract_tc_state.awk b/tests/trajectory-planner/circular-arcs/extract_tc_state.awk new file mode 100644 index 00000000000..647196dcf9b --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/extract_tc_state.awk @@ -0,0 +1 @@ +/tc state:/{print $5 $8 $11 $14 $17 $20 $23 $26 $29 $32 $35} diff --git a/tests/trajectory-planner/circular-arcs/linuxcnc_control.py b/tests/trajectory-planner/circular-arcs/linuxcnc_control.py index 33196931680..43daed38280 100644 --- a/tests/trajectory-planner/circular-arcs/linuxcnc_control.py +++ b/tests/trajectory-planner/circular-arcs/linuxcnc_control.py @@ -1,22 +1,32 @@ #!/usr/bin/env python -'''Copied from m61-test''' +"""Copied from m61-test""" import linuxcnc import os +import sys +import re from time import sleep -# this is how long we wait for linuxcnc to do our bidding - class LinuxcncError(Exception): - pass -# def __init__(self, value): -# self.value = value -# def __str__(self): -# return repr(self.value) + def __init__(self, value): + self.value = value + + def __str__(self): + return repr(self.value) + + +class CommandTimeoutError(Exception): + def __init__(self, message): + # Call the base class constructor with the parameters it needs + self.message = message + + def __str__(self): + return self.message + class LinuxcncControl: - ''' + """ issue G-Code commands make sure important modes are saved and restored mode is saved only once, and can be restored only once @@ -27,70 +37,103 @@ class LinuxcncControl: any internal sub using e.g("G0.....") e.finish_mdi() - ''' + """ + + def __init__(self, timeout=2, max_runtime=24 * 60 * 60, throw_lcnc_exceptions=False, continue_after_error=True): + self.c = None + self.e = None + self.s = None + self.cmd_timeout = timeout + self.error_list = [] + self.raise_on_error = throw_lcnc_exceptions + # Ignore blank errors by default + self.err_ignore_mask = '^$' + self.try_init(ignore_error=True) + # Don't allow a test to run for more than + self.max_runtime = max_runtime + self.continue_after_error = continue_after_error + self.has_error_ = False + + def try_init(self, ignore_error=False): + try: + self.c = linuxcnc.command() + self.e = linuxcnc.error_channel() + self.s = linuxcnc.stat() + self.s.poll() + except linuxcnc.error as e: + if ignore_error: + return False - def __init__(self,timeout=2): - self.c = linuxcnc.command() - self.e = linuxcnc.error_channel() - self.s = linuxcnc.stat() - self.timeout = timeout + return True + + def wait_for_backend(self, timeout_sec): + k = 0 + print('Waiting for LinuxCNC...') + while k < 25: + k += 1 + sleep(1) + if self.try_init(ignore_error=True): + return True + + return False + + def await_done(self, timeout=None): + sts = self.c.wait_complete(timeout if timeout is not None else self.cmd_timeout) + if sts == -1: + raise CommandTimeoutError("command exceeded timeout of {}".format(self.cmd_timeout)) + return sts def running(self, do_poll=True): - ''' - check wether interpreter is running. + """ + check if interpreter is running. If so, cant switch to MDI mode. - ''' + """ if do_poll: self.s.poll() return (self.s.task_mode == linuxcnc.MODE_AUTO and self.s.interp_state != linuxcnc.INTERP_IDLE) - def set_mode(self,m): - ''' + def set_mode(self, m): + """ set EMC mode if possible, else throw LinuxcncError - return current mode - ''' + """ self.s.poll() if self.s.task_mode == m : - return m + return 1 if self.running(do_poll=False): raise LinuxcncError("interpreter running - cant change mode") self.c.mode(m) - self.c.wait_complete() + return self.await_done() - return m - def set_state(self,m): - ''' + def set_state(self, m): + """ set EMC mode if possible, else throw LinuxcncError - return current mode - ''' + """ self.s.poll() if self.s.task_mode == m : - return m + return 1 self.c.state(m) - self.c.wait_complete(self.timeout) - return m + return self.await_done() - def do_home(self,axismask): + def do_home(self, axismask): self.s.poll() self.c.home(axismask) - self.c.wait_complete(self.timeout) - + return self.await_done() def ok_for_mdi(self): - ''' + """ check wether ok to run MDI commands. - ''' + """ self.s.poll() return not self.s.estop and self.s.enabled and self.s.homed def prepare_for_mdi(self): - ''' + """ check wether ok to run MDI commands. throw LinuxcncError if told so. return current mode - ''' + """ self.s.poll() if self.s.estop: @@ -109,32 +152,23 @@ def prepare_for_mdi(self): g_raise_except = True - def g(self,code,wait=False): - ''' + def g(self, code, wait=False): + """ issue G-Code as MDI command. wait for completion if reqested - ''' + """ self.c.mdi(code) if wait: try: - while self.c.wait_complete(self.timeout) == -1: + while self.c.wait_complete(self.cmd_timeout) == -1: pass return True except KeyboardInterrupt: - print "interrupted by keyboard in c.wait_complete(self.timeout)" + self.error_list.append("warning: interrupted by keyboard in c.wait_complete(self.timeout)") return False - self.error = self.e.poll() - if self.error: - kind, text = self.error - if kind in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): - if self.g_raise_except: - raise LinuxcncError(text) - else: - print ("error " + text) - else: - print ("info " + text) + self.poll_and_log_error(code) return False def get_current_tool(self): @@ -142,7 +176,7 @@ def get_current_tool(self): return self.s.tool_in_spindle def active_codes(self): - self.e.poll() + self.s.poll() return self.s.gcodes def get_current_system(self): @@ -154,48 +188,110 @@ def get_current_system(self): return i - 584 return 1 - def open_program(self,filename): - '''Open an nc file''' + def open_program(self, filename): + """Open an nc file""" self.s.poll() self.set_mode(linuxcnc.MODE_AUTO) - self.c.wait_complete() - sleep(.25) - self.c.program_open(filename) - self.c.wait_complete() + full_file = os.path.join(os.getcwd(), filename) + self.c.program_open(full_file) + return self.await_done() def run_full_program(self): - '''Start a loaded program''' + """Start a loaded program""" self.s.poll() self.c.auto(linuxcnc.AUTO_RUN, 0) - self.c.wait_complete(self.timeout) - return self.check_rcs_error() + # WARNING: does not await completion of the command (use wait_on_program for this) - def set_feed_scale(self,scale): - '''Assign a feed scale''' + def set_feed_scale(self, scale): + """Assign a feed scale""" self.s.poll() self.c.feedrate(scale) - self.c.wait_complete(self.timeout) + return self.await_done() - def wait_on_program(self): - self.s.poll() - while self.s.exec_state != linuxcnc.EXEC_DONE or self.s.state != linuxcnc.RCS_DONE and self.s.task_state == linuxcnc.STATE_ON: - sleep(.25) + def wait_on_program(self, f): + iter_count = 0 + update_rate = 50. # Hz + max_iter = int(update_rate * self.max_runtime) + while iter_count < max_iter: self.s.poll() + self.flush_errors(f) if self.s.task_state != linuxcnc.STATE_ON: + self.error_list.append('{}: error: motion disabled'.format(f)) return False - if self.check_rcs_error(): - print "Found RCS error while waiting, running again" - self.run_full_program() - return True + if self.s.exec_state == linuxcnc.EXEC_ERROR or self.s.state == linuxcnc.RCS_ERROR: + self.error_list.append('{}: warning: unhandled linuxcnc error, exec_state = {}, rcs_state = {}, aborting test'.format(f, self.s.exec_state, self.s.state)) + return False + + if self.s.exec_state == linuxcnc.EXEC_DONE and self.s.state == linuxcnc.RCS_DONE: + return True + + sleep(1. / update_rate) + else: + self.error_list.append('{}: error: exceeded max allowed run time of {} seconds'.format(f, self.max_runtime)) + return False + + def run_until_done(self, f): + self.set_mode(linuxcnc.MODE_AUTO) + self.open_program(f) + self.set_mode(linuxcnc.MODE_AUTO) + self.run_full_program() + return self.wait_on_program(f) def check_rcs_error(self): self.s.poll() if self.s.state == linuxcnc.RCS_ERROR: - print "detected RCS error" return True return False -def introspect(): - os.system("halcmd show pin python-ui") + def set_error_ignore_pattern(self, pattern): + self.err_ignore_mask = pattern + + def poll_and_log_error(self, f): + error = self.e.poll() + if error is None: + return False + + err_type, msg_text = error + err_type_str = "error" if err_type in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR) else "info" + err_str = "{}: {}: {}".format(f, err_type_str, msg_text) + self.error_list.append(err_str) + if self.raise_on_error: + raise LinuxcncError(err_str) + return True + + def flush_errors(self, filename=""): + while self.poll_and_log_error(filename): + sleep(0.001) + continue + + def has_error(self): + self.has_error_ = self.has_error_ or self.find_new_error() + return self.has_error_ + + def find_new_error(self): + for msg in self.error_list: + if re.search(self.err_ignore_mask, msg): + continue + if re.search('error: ', msg): + return True + else: + return False + + def write_error_log(self): + sys.stderr.writelines(self.error_list) + # Cache the error state since we flush the error list to std err + self.has_error_ = self.has_error() + self.error_list = [] + + def do_startup(self, need_home=False): + self.set_mode(linuxcnc.MODE_MANUAL) + self.set_state(linuxcnc.STATE_ESTOP_RESET) + self.set_state(linuxcnc.STATE_ON) + if need_home: + self.do_home(-1) + sleep(0.1) + self.set_mode(linuxcnc.MODE_AUTO) + + diff --git a/tests/trajectory-planner/circular-arcs/nc_files/XYtests/51MeanderAve.ngc b/tests/trajectory-planner/circular-arcs/nc_files/XYtests/51MeanderAve.ngc index 6d16d6ac067..11a2655812d 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/XYtests/51MeanderAve.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/XYtests/51MeanderAve.ngc @@ -4,7 +4,7 @@ G21 (Units: Metric) G40 G90 F1 S1 -G64 P0.01 q00.01 +G64 P0.01 q0.01 (Part: 51 Meander Ave) (Operation: Outside Offset, LAYER_1, T210: Stainless 2.0mm) G00 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-concave-convex.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-concave-convex.ngc index eca82217ad6..1ca8428f31a 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-concave-convex.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-concave-convex.ngc @@ -1,4 +1,5 @@ -G90 G20 G64 +(Intersection between two arcs where 1st is concave, 2nd is convex wrt intersection) +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex-concave.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex-concave.ngc index 5c6e86b0bb8..5dce8b71164 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex-concave.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex-concave.ngc @@ -1,4 +1,5 @@ -G90 G20 G64 +(Intersection between two arcs, concave concave, tests arc blend creation) +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex-convex.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex-convex.ngc index 5c0067bacfa..006375c8834 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex-convex.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex-convex.ngc @@ -1,4 +1,4 @@ -G90 G20 G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex.ngc index eca82217ad6..022c4b24d83 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex.ngc @@ -1,4 +1,4 @@ -G90 G20 G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-neartangent.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-neartangent.ngc index d31b544c5ff..b5f68a270cd 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-neartangent.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-neartangent.ngc @@ -1,4 +1,4 @@ -G90 G20 G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-planeswitch.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-planeswitch.ngc index 53f9f1706e7..a25e7d1ba89 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-planeswitch.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-planeswitch.ngc @@ -1,4 +1,4 @@ -G90 G20 G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-sawtooth.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-sawtooth.ngc index 127e2ddadde..608ac952304 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-sawtooth.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-sawtooth.ngc @@ -1,5 +1,4 @@ -G90 G20 -G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-large-radii.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-large-radii.ngc new file mode 100644 index 00000000000..cdf4fb777ce --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-large-radii.ngc @@ -0,0 +1,13 @@ +o call +G64 P0 Q0 +F1000 +G0 X0 Y0 +Z0 +G1 X1 +G2 X2 R1 +G2 X3 R2 +G1 X4 +G1 Y2 +X0 +Y0 +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-terminal.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-terminal.ngc index adb38ff00cf..ccb6e51eb32 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-terminal.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-terminal.ngc @@ -1,5 +1,4 @@ -G20 G90 -G64 +o call F1000 G0 X0 Y0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/blur_torus.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/blur_torus.ngc deleted file mode 100644 index 378acbf84d8..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/blur_torus.ngc +++ /dev/null @@ -1,1905 +0,0 @@ -G20 -G0 Z0.0120 -G17 G40 -G80 G90 G94 -G64 P0.0010 -F1000 -G18 -G0 X1.5550 Y1.4350 -G1 Z-0.3750 -G3 X1.430000 Z-0.500000 R0.125000 -G1 X0.7450 - X0.7300 Z-0.4888 - X0.7250 Z-0.4870 - X0.7050 - X0.7000 Z-0.4888 - X0.6850 Z-0.5000 - X0.0000 - Y1.3950 - X0.4700 - X0.4750 Z-0.4976 -G2 X0.4900 Y1.3950 Z-0.4564 I-0.2697 K0.1216 -G3 X0.5250 Y1.3950 Z-0.3270 I1.7704 K-0.4094 -G3 X0.5500 Y1.3950 Z-0.2760 I0.2712 K-0.1014 -G3 X0.5700 Y1.3950 Z-0.2535 I0.1210 K-0.0876 -G1 X0.5900 Z-0.2398 - X0.6350 Z-0.2223 - X0.6400 Z-0.2222 - X0.6600 Z-0.2164 - X0.6700 Z-0.2163 - X0.6900 Z-0.2125 - X0.7400 - X0.7450 Z-0.2143 - X0.7700 Z-0.2164 - X0.7900 Z-0.2222 - X0.7950 Z-0.2223 - X0.8400 Z-0.2398 -G2 X0.8550 Y1.3950 Z-0.2496 I0.0238 K0.0201 -G1 X0.8650 Z-0.2583 -G3 X0.8850 Y1.3950 Z-0.2838 I-0.0825 K-0.0853 -G3 X0.9050 Y1.3950 Z-0.3270 I-0.2987 K-0.1647 -G1 X0.9400 Z-0.4564 -G2 X0.9550 Y1.3950 Z-0.4976 I0.3224 K0.0941 -G1 X0.9600 Z-0.5000 - X1.4300 - Y1.3550 - X1.0500 -G3 X1.0400 Y1.3550 Z-0.4740 I0.0429 K0.0314 -G1 X1.0150 Z-0.3387 -G2 X1.0050 Y1.3550 Z-0.2956 I-0.5400 K-0.1025 -G2 X0.9750 Y1.3550 Z-0.2387 I-0.1176 K-0.0257 -G1 X0.9600 Z-0.2241 - X0.9350 Z-0.2065 - X0.9050 Z-0.1911 - X0.8750 Z-0.1793 -G3 X0.8500 Y1.3550 Z-0.1723 I0.0197 K0.1174 -G1 X0.8000 Z-0.1624 - X0.7600 Z-0.1585 - X0.7350 Z-0.1582 - X0.6700 Z-0.1585 - X0.6450 Z-0.1605 - X0.5750 Z-0.1734 -G3 X0.5550 Y1.3550 Z-0.1793 I-0.0295 K0.0633 -G2 X0.5050 Y1.3550 Z-0.2006 I0.1272 K-0.3685 -G1 X0.4800 Z-0.2163 - X0.4650 Z-0.2289 -G2 X0.4250 Y1.3550 Z-0.2956 I0.0772 K-0.0917 -G2 X0.4150 Y1.3550 Z-0.3387 I0.5300 K-0.1456 -G1 X0.3900 Z-0.4740 -G3 X0.3800 Y1.3550 Z-0.5000 I-0.0529 K0.0055 -G1 X0.0000 - Y1.3150 - X0.3150 Z-0.4995 -G2 X0.3250 Y1.3150 Z-0.4544 I-0.3974 K0.1118 -G3 X0.3500 Y1.3150 Z-0.2956 I2.1684 K-0.2599 -G3 X0.3850 Y1.3150 Z-0.2241 I0.1418 K-0.0251 -G3 X0.4250 Y1.3150 Z-0.1898 I0.1743 K-0.1626 -G3 X0.4550 Y1.3150 Z-0.1731 I0.1309 K-0.1996 -G1 X0.5050 Z-0.1533 - X0.5850 Z-0.1367 - X0.6650 Z-0.1300 - X0.7850 Z-0.1308 - X0.8450 Z-0.1367 - X0.9100 Z-0.1494 - X0.9250 Z-0.1533 - X0.9800 Z-0.1754 -G3 X1.0300 Y1.3150 Z-0.2094 I-0.0933 K-0.1909 -G3 X1.0750 Y1.3150 Z-0.2779 I-0.0924 K-0.1097 -G3 X1.0850 Y1.3150 Z-0.3191 I-0.3032 K-0.0954 -G1 X1.1050 Z-0.4544 -G2 X1.1150 Y1.3150 Z-0.4995 I0.4074 K0.0667 -G1 X1.4300 Z-0.5000 - Y1.2750 - X1.1700 -G3 X1.1600 Y1.2750 Z-0.4642 I0.0626 K0.0368 -G3 X1.1450 Y1.2750 Z-0.3427 I2.9199 K0.4220 -G2 X1.1350 Y1.2750 Z-0.2838 I-0.6610 K-0.0821 -G2 X1.1000 Y1.2750 Z-0.2124 I-0.1548 K-0.0315 -G2 X1.0350 Y1.2750 Z-0.1633 I-0.1705 K-0.1582 -G1 X0.9700 Z-0.1376 -G2 X0.8850 Y1.2750 Z-0.1221 I-0.1478 K-0.5692 -G1 X0.8150 Z-0.1165 - X0.7800 Z-0.1157 - X0.6150 Z-0.1165 - X0.5450 Z-0.1221 -G2 X0.4600 Y1.2750 Z-0.1376 I0.0752 K-0.6524 -G1 X0.3950 Z-0.1633 -G2 X0.3300 Y1.2750 Z-0.2124 I0.1055 K-0.2073 -G2 X0.3050 Y1.2750 Z-0.2535 I0.0916 K-0.0838 -G2 X0.2900 Y1.2750 Z-0.3093 I0.1787 K-0.0780 -G1 X0.2700 Z-0.4642 -G3 X0.2600 Y1.2750 Z-0.5000 I-0.0726 K0.0010 -G1 X0.0000 - Y1.2350 - X0.2150 - X0.2200 Z-0.4928 - X0.2250 Z-0.4653 - X0.2400 Z-0.3359 -G3 X0.2500 Y1.2350 Z-0.2731 I0.5248 K-0.0515 -G3 X0.2900 Y1.2350 Z-0.1986 I0.1408 K-0.0276 -G3 X0.3400 Y1.2350 Z-0.1594 I0.1732 K-0.1691 -G1 X0.3500 Z-0.1535 - X0.4000 Z-0.1337 -G3 X0.4700 Y1.2350 Z-0.1185 I0.1316 K-0.4356 -G1 X0.5500 Z-0.1118 - X0.8800 - X0.9350 Z-0.1157 -G3 X1.0800 Y1.2350 Z-0.1535 I-0.0280 K-0.4044 -G3 X1.1350 Y1.2350 Z-0.1937 I-0.1305 K-0.2360 -G3 X1.1700 Y1.2350 Z-0.2418 I-0.1052 K-0.1134 -G3 X1.1850 Y1.2350 Z-0.2986 I-0.2253 K-0.0898 -G1 X1.2050 Z-0.4653 - X1.2100 Z-0.4928 - X1.2150 Z-0.5000 - X1.4300 - Y1.1950 - X1.2550 - X1.2500 Z-0.4928 - X1.2450 Z-0.4673 -G3 X1.2300 Y1.1950 Z-0.3359 I3.4995 K0.4661 -G2 X1.2200 Y1.1950 Z-0.2712 I-0.6965 K-0.0745 -G2 X1.1950 Y1.1950 Z-0.2124 I-0.1428 K-0.0260 -G2 X1.1650 Y1.1950 Z-0.1786 I-0.1849 K-0.1343 -G1 X1.1300 Z-0.1535 - X1.1050 Z-0.1406 - X1.0800 Z-0.1309 - X1.0450 Z-0.1213 -G2 X0.9750 Y1.1950 Z-0.1118 I-0.0919 K-0.4146 -G1 X0.8600 - X0.7550 Z-0.1165 - X0.6750 - X0.5700 Z-0.1118 - X0.4550 -G2 X0.3850 Y1.1950 Z-0.1213 I0.0219 K-0.4241 -G1 X0.3500 Z-0.1309 - X0.3250 Z-0.1406 - X0.3100 Z-0.1476 - X0.2850 Z-0.1633 -G2 X0.2550 Y1.1950 Z-0.1884 I0.0888 K-0.1363 -G2 X0.2150 Y1.1950 Z-0.2516 I0.1181 K-0.1190 -G2 X0.2050 Y1.1950 Z-0.2986 I0.3330 K-0.0953 -G1 X0.1850 Z-0.4673 - X0.1800 Z-0.4928 - X0.1750 Z-0.5000 - X0.0000 - Y1.1550 - X0.1450 - X0.1500 Z-0.4740 -G2 X0.1650 Y1.1550 Z-0.3368 I-3.9987 K0.5064 -G3 X0.1750 Y1.1550 Z-0.2692 I0.7103 K-0.0706 -G3 X0.2000 Y1.1550 Z-0.2104 I0.1642 K-0.0351 -G3 X0.2800 Y1.1550 Z-0.1406 I0.1736 K-0.1181 -G3 X0.3950 Y1.1550 Z-0.1118 I0.1349 K-0.2944 -G1 X0.4800 - X0.5750 Z-0.1204 - X0.6150 Z-0.1260 - X0.6600 Z-0.1280 - X0.6700 Z-0.1300 - X0.7600 - X0.7700 Z-0.1280 - X0.8150 Z-0.1260 - X0.8550 Z-0.1204 - X0.9500 Z-0.1118 - X1.0350 -G3 X1.1200 Y1.1550 Z-0.1288 I-0.0197 K-0.3186 -G3 X1.2450 Y1.1550 Z-0.2378 I-0.0584 K-0.1932 -G3 X1.2600 Y1.1550 Z-0.2976 I-0.2120 K-0.0850 -G1 X1.2800 Z-0.4740 - X1.2850 Z-0.5000 - X1.4300 - Y1.1150 - X1.3150 - X1.3100 Z-0.4799 - X1.2900 Z-0.2936 -G2 X1.2650 Y1.1150 Z-0.2163 I-0.2038 K-0.0232 -G2 X1.2300 Y1.1150 Z-0.1728 I-0.1489 K-0.0839 -G2 X1.1850 Y1.1150 Z-0.1406 I-0.1464 K-0.1571 -G1 X1.1400 Z-0.1224 - X1.0850 Z-0.1118 - X1.0150 - X0.9950 Z-0.1137 - X0.9500 Z-0.1202 - X0.8250 Z-0.1435 - X0.7500 Z-0.1523 - X0.7250 - X0.7150 Z-0.1543 - X0.7050 Z-0.1523 - X0.6800 - X0.6050 Z-0.1435 - X0.4700 Z-0.1185 - X0.4150 Z-0.1118 - X0.3450 - X0.2850 Z-0.1241 - X0.2450 Z-0.1406 -G2 X0.2000 Y1.1150 Z-0.1728 I0.1014 K-0.1893 -G2 X0.1550 Y1.1150 Z-0.2359 I0.1213 K-0.1340 -G2 X0.1400 Y1.1150 Z-0.2936 I0.2205 K-0.0881 -G1 X0.1200 Z-0.4799 - X0.1150 Z-0.5000 - X0.0000 - Y1.0750 - X0.0900 - X0.0950 Z-0.4799 - X0.1150 Z-0.2897 -G3 X0.1400 Y1.0750 Z-0.2143 I0.1916 K-0.0217 -G3 X0.2000 Y1.0750 Z-0.1494 I0.1504 K-0.0789 -G3 X0.3050 Y1.0750 Z-0.1118 I0.1253 K-0.1843 -G1 X0.3700 - X0.3950 Z-0.1154 - X0.4550 Z-0.1271 - X0.5750 Z-0.1631 - X0.6200 Z-0.1749 - X0.6500 Z-0.1813 - X0.7100 Z-0.1876 - X0.7500 Z-0.1852 - X0.7900 Z-0.1793 - X0.8550 Z-0.1631 - X0.9750 Z-0.1271 - X1.0450 Z-0.1137 - X1.0600 Z-0.1118 - X1.1250 -G3 X1.1700 Y1.0750 Z-0.1213 I-0.0434 K-0.3169 -G3 X1.3000 Y1.0750 Z-0.2339 I-0.0515 K-0.1908 -G3 X1.3150 Y1.0750 Z-0.2897 I-0.1787 K-0.0780 -G1 X1.3350 Z-0.4799 - X1.3400 Z-0.5000 - X1.4300 - Y1.0350 - X1.3600 - X1.3550 Z-0.4740 - X1.3400 Z-0.3132 - X1.3350 Z-0.2779 -G2 X1.3000 Y1.0350 Z-0.1937 I-0.1775 K-0.0244 -G2 X1.2250 Y1.0350 Z-0.1309 I-0.1616 K-0.1167 -G2 X1.1550 Y1.0350 Z-0.1118 I-0.0976 K-0.2200 -G1 X1.1000 -G2 X1.0350 Y1.0350 Z-0.1232 I0.0666 K-0.5680 -G3 X1.0000 Y1.0350 Z-0.1348 I-0.0530 K0.1018 -G1 X0.9450 Z-0.1563 - X0.8500 Z-0.2004 -G3 X0.8050 Y1.0350 Z-0.2185 I-0.2314 K0.5086 -G1 X0.7650 Z-0.2301 - X0.7250 Z-0.2360 - X0.7050 - X0.6750 Z-0.2321 - X0.6350 Z-0.2222 -G3 X0.5900 Y1.0350 Z-0.2048 I0.3040 K0.8558 -G1 X0.4850 Z-0.1563 - X0.4300 Z-0.1348 -G3 X0.3950 Y1.0350 Z-0.1232 I0.0171 K0.1106 -G2 X0.3300 Y1.0350 Z-0.1118 I-0.1470 K-0.6436 -G1 X0.2750 -G2 X0.2250 Y1.0350 Z-0.1232 I0.0232 K-0.2163 -G2 X0.1550 Y1.0350 Z-0.1652 I0.0517 K-0.1655 -G2 X0.1050 Y1.0350 Z-0.2387 I0.1443 K-0.1519 -G2 X0.0900 Y1.0350 Z-0.3132 I0.2565 K-0.0904 -G1 X0.0750 Z-0.4740 - X0.0700 Z-0.5000 - X0.0000 - Y0.9950 - X0.0500 - X0.0550 Z-0.4858 - X0.0750 Z-0.2819 -G3 X0.1100 Y0.9950 Z-0.1937 I0.2098 K-0.0323 -G1 X0.1300 Z-0.1695 - X0.1700 Z-0.1376 -G3 X0.2500 Y0.9950 Z-0.1118 I0.0973 K-0.1642 -G1 X0.3000 - X0.3250 Z-0.1154 -G3 X0.3600 Y0.9950 Z-0.1232 I-0.0456 K-0.2854 -G3 X0.4100 Y0.9950 Z-0.1425 I-0.5357 K-1.4614 -G1 X0.4500 Z-0.1621 -G2 X0.4850 Y0.9950 Z-0.1829 I0.1837 K0.2701 -G1 X0.5450 Z-0.2231 - X0.6150 Z-0.2731 - X0.6450 Z-0.2908 - X0.6700 Z-0.3026 - X0.6750 Z-0.3027 - X0.6950 Z-0.3084 - X0.7150 Z-0.3105 - X0.7350 Z-0.3084 - X0.7550 Z-0.3027 - X0.7600 Z-0.3026 - X0.7850 Z-0.2908 - X0.8150 Z-0.2731 - X0.9150 Z-0.2025 - X0.9700 Z-0.1680 - X1.0200 Z-0.1425 - X1.0650 Z-0.1250 - X1.0850 Z-0.1193 -G3 X1.1300 Y0.9950 Z-0.1118 I0.0975 K-0.4441 -G1 X1.1800 -G3 X1.2200 Y0.9950 Z-0.1204 I-0.0174 K-0.1770 -G3 X1.2600 Y0.9950 Z-0.1376 I-0.0593 K-0.1929 -G1 X1.2950 Z-0.1651 -G3 X1.3300 Y0.9950 Z-0.2104 I-0.0875 K-0.1038 -G3 X1.3550 Y0.9950 Z-0.2819 I-0.1732 K-0.1007 -G1 X1.3750 Z-0.4858 - X1.3800 Z-0.5000 - X1.4300 - Y0.9550 - X1.3950 - X1.3900 Z-0.4858 - X1.3700 Z-0.2799 -G2 X1.3500 Y0.9550 Z-0.2163 I-0.2104 K-0.0312 -G2 X1.2850 Y0.9550 Z-0.1416 I-0.1889 K-0.0987 -G2 X1.2000 Y0.9550 Z-0.1118 I-0.1046 K-0.1621 -G1 X1.1500 - X1.1150 Z-0.1185 -G2 X0.9950 Y0.9550 Z-0.1767 I0.0925 K-0.3435 -G2 X0.8900 Y0.9550 Z-0.2712 I0.3613 K-0.5070 -G1 X0.8600 Z-0.3065 - X0.8150 Z-0.3662 - X0.7600 Z-0.4564 -G3 X0.7350 Y0.9550 Z-0.4829 I-0.1639 K0.1292 -G1 X0.7200 Z-0.4850 - X0.7050 Z-0.4849 - X0.6950 Z-0.4829 - X0.6800 Z-0.4681 - X0.6700 Z-0.4564 - X0.6150 Z-0.3662 - X0.5700 Z-0.3065 - X0.5400 Z-0.2712 -G2 X0.4250 Y0.9550 Z-0.1692 I-0.4900 K-0.4365 -G2 X0.3150 Y0.9550 Z-0.1185 I-0.2060 K-0.3023 -G1 X0.2800 Z-0.1118 - X0.2300 -G2 X0.1900 Y0.9550 Z-0.1204 I0.0174 K-0.1770 -G2 X0.1300 Y0.9550 Z-0.1533 I0.0480 K-0.1587 -G2 X0.0800 Y0.9550 Z-0.2163 I0.1386 K-0.1614 -G2 X0.0600 Y0.9550 Z-0.2799 I0.1904 K-0.0948 -G1 X0.0400 Z-0.4858 - X0.0350 Z-0.5000 - X0.0000 - Y0.9150 - X0.0250 Z-0.4995 - X0.0300 Z-0.4603 -G3 X0.0450 Y0.9150 Z-0.2956 I3.9179 K-0.2738 -G3 X0.0700 Y0.9150 Z-0.2104 I0.2183 K-0.0178 -G3 X0.2100 Y0.9150 Z-0.1118 I0.1623 K-0.0817 -G1 X0.2650 Z-0.1126 - X0.2950 Z-0.1185 - X0.3450 Z-0.1378 -G3 X0.4400 Y0.9150 Z-0.2061 I-0.1692 K-0.3357 -G3 X0.5250 Y0.9150 Z-0.3163 I-0.3670 K-0.3710 -G3 X0.5500 Y0.9150 Z-0.3692 I-0.3664 K-0.2054 -G1 X0.5600 Z-0.3967 - X0.5850 Z-0.4838 - X0.5900 Z-0.4976 - X0.5950 Z-0.5000 - X0.8350 - X0.8400 Z-0.4976 - X0.8700 Z-0.3967 -G3 X0.9050 Y0.9150 Z-0.3163 I0.4274 K-0.1382 -G3 X1.0000 Y0.9150 Z-0.1966 I0.4475 K-0.2577 -G3 X1.0800 Y0.9150 Z-0.1406 I0.2460 K-0.2660 -G1 X1.1350 Z-0.1185 - X1.1700 Z-0.1118 - X1.2200 - X1.2450 Z-0.1165 -G3 X1.3650 Y0.9150 Z-0.2202 I-0.0489 K-0.1778 -G3 X1.3850 Y0.9150 Z-0.2956 I-0.2019 K-0.0939 -G3 X1.4000 Y0.9150 Z-0.4603 I-3.9029 K-0.4385 -G1 X1.4050 Z-0.4995 - X1.4300 Z-0.5000 - Y0.8750 - X1.4150 Z-0.4995 - X1.4100 Z-0.4623 -G2 X1.3950 Y0.8750 Z-0.2956 I-3.7204 K-0.2508 -G2 X1.3700 Y0.8750 Z-0.2084 I-0.2156 K-0.0147 -G2 X1.2300 Y0.8750 Z-0.1118 I-0.1591 K-0.0807 -G1 X1.1850 Z-0.1126 - X1.1450 Z-0.1204 - X1.1200 Z-0.1300 -G2 X1.0700 Y0.8750 Z-0.1582 I0.1224 K-0.2749 -G2 X1.0100 Y0.8750 Z-0.2107 I0.2257 K-0.3186 -G2 X0.9350 Y0.8750 Z-0.3241 I0.3300 K-0.2997 -G2 X0.9150 Y0.8750 Z-0.3790 I0.2834 K-0.1343 -G2 X0.9050 Y0.8750 Z-0.4182 I0.4006 K-0.1230 -G3 X0.8900 Y0.8750 Z-0.4888 I-1.3362 K0.2471 -G1 X0.8850 Z-0.5000 - X0.5450 - X0.5400 Z-0.4888 -G2 X0.5150 Y0.8750 Z-0.3790 I-1.3743 K-0.2552 -G2 X0.4950 Y0.8750 Z-0.3241 I-0.3034 K-0.0794 -G2 X0.4150 Y0.8750 Z-0.2055 I-0.4009 K-0.1840 -G2 X0.3500 Y0.8750 Z-0.1514 I-0.2769 K-0.2665 -G2 X0.2900 Y0.8750 Z-0.1221 I-0.1625 K-0.2573 -G1 X0.2400 Z-0.1118 - X0.2000 -G2 X0.1650 Y0.8750 Z-0.1193 I0.0264 K-0.2075 -G2 X0.1050 Y0.8750 Z-0.1526 I0.0413 K-0.1450 -G1 X0.0850 Z-0.1715 -G2 X0.0550 Y0.8750 Z-0.2182 I0.2353 K-0.1840 -G2 X0.0350 Y0.8750 Z-0.2956 I0.1985 K-0.0926 -G2 X0.0200 Y0.8750 Z-0.4623 I3.7054 K-0.4175 -G1 X0.0150 Z-0.4995 - X0.0000 Z-0.5000 - Y0.8350 - X0.0050 - X0.0100 Z-0.4869 -G2 X0.0250 Y0.8350 Z-0.3172 I-3.4555 K0.3909 -G1 X0.0300 Z-0.2771 -G3 X0.0450 Y0.8350 Z-0.2222 I0.2111 K-0.0282 -G3 X0.1100 Y0.8350 Z-0.1416 I0.1793 K-0.0781 -G1 X0.1250 Z-0.1319 - X0.1600 Z-0.1182 - X0.1900 Z-0.1118 - X0.2350 Z-0.1126 -G3 X0.3050 Y0.8350 Z-0.1339 I-0.0254 K-0.2092 -G3 X0.3650 Y0.8350 Z-0.1739 I-0.1149 K-0.2373 -G3 X0.4150 Y0.8350 Z-0.2264 I-0.1956 K-0.2364 -G3 X0.4750 Y0.8350 Z-0.3328 I-0.3089 K-0.2442 -G3 X0.4950 Y0.8350 Z-0.3976 I-0.3605 K-0.1469 -G1 X0.5150 Z-0.4995 - X0.5200 Z-0.5000 - X0.9150 Z-0.4995 - X0.9350 Z-0.3976 -G3 X0.9550 Y0.8350 Z-0.3328 I0.3805 K-0.0822 -G3 X1.0350 Y0.8350 Z-0.2029 I0.3839 K-0.1466 -G3 X1.1600 Y0.8350 Z-0.1202 I0.2122 K-0.1849 -G1 X1.2000 Z-0.1118 - X1.2400 - X1.2650 Z-0.1165 - X1.3050 Z-0.1319 -G3 X1.3300 Y0.8350 Z-0.1494 I-0.0546 K-0.1047 -G3 X1.3850 Y0.8350 Z-0.2222 I-0.1226 K-0.1498 -G3 X1.4000 Y0.8350 Z-0.2771 I-0.1961 K-0.0831 -G1 X1.4050 Z-0.3172 -G2 X1.4200 Y0.8350 Z-0.4869 I3.4705 K0.2212 -G1 X1.4250 Z-0.5000 - X1.4300 - Y0.7950 - X1.4250 Z-0.4849 -G3 X1.4100 Y0.7950 Z-0.3143 I3.3931 K0.3843 -G1 X1.4050 Z-0.2751 -G2 X1.3850 Y0.7950 Z-0.2105 I-0.1949 K-0.0250 -G2 X1.3250 Y0.7950 Z-0.1406 I-0.1682 K-0.0835 -G2 X1.2900 Y0.7950 Z-0.1221 I-0.1056 K-0.1579 -G2 X1.2450 Y0.7950 Z-0.1118 I-0.0674 K-0.1899 -G1 X1.2100 - X1.1800 Z-0.1173 - X1.1300 Z-0.1359 -G2 X1.0800 Y0.7950 Z-0.1703 I0.0919 K-0.1870 -G2 X1.0350 Y0.7950 Z-0.2166 I0.1992 K-0.2387 -G2 X0.9750 Y0.7950 Z-0.3202 I0.3124 K-0.2501 -G2 X0.9550 Y0.7950 Z-0.3838 I0.3932 K-0.1586 -G1 X0.9350 Z-0.4936 - X0.9300 Z-0.5000 - X0.5000 - X0.4950 Z-0.4936 -G2 X0.4750 Y0.7950 Z-0.3838 I-2.1792 K-0.3402 -G2 X0.4550 Y0.7950 Z-0.3202 I-0.4132 K-0.0949 -G2 X0.3800 Y0.7950 Z-0.1989 I-0.3721 K-0.1463 -G2 X0.3250 Y0.7950 Z-0.1504 I-0.2976 K-0.2816 -G1 X0.3000 Z-0.1359 - X0.2550 Z-0.1185 - X0.2200 Z-0.1118 - X0.1850 -G2 X0.1450 Y0.7950 Z-0.1204 I0.0432 K-0.2961 -G2 X0.0800 Y0.7950 Z-0.1617 I0.0467 K-0.1454 -G2 X0.0350 Y0.7950 Z-0.2339 I0.1408 K-0.1379 -G2 X0.0250 Y0.7950 Z-0.2751 I0.1734 K-0.0639 -G1 X0.0200 Z-0.3143 -G3 X0.0050 Y0.7950 Z-0.4849 I-3.4081 K0.2137 -G1 X0.0000 Z-0.5000 - Y0.7550 - X0.0050 Z-0.4740 - X0.0200 Z-0.2995 - X0.0250 Z-0.2615 -G3 X0.0400 Y0.7550 Z-0.2145 I0.1674 K-0.0275 -G3 X0.1800 Y0.7550 Z-0.1118 I0.1600 K-0.0713 -G1 X0.2150 - X0.2500 Z-0.1185 - X0.2900 Z-0.1339 -G3 X0.3850 Y0.7550 Z-0.2127 I-0.1319 K-0.2557 -G3 X0.4500 Y0.7550 Z-0.3300 I-0.2722 K-0.2274 -G3 X0.4650 Y0.7550 Z-0.3829 I-0.3195 K-0.1191 -G1 X0.4850 Z-0.4947 - X0.4900 Z-0.5000 - X0.9400 - X0.9450 Z-0.4947 -G2 X0.9600 Y0.7550 Z-0.4065 I-1.7386 K0.3409 -G1 X0.9700 Z-0.3614 -G3 X0.9850 Y0.7550 Z-0.3164 I0.5175 K-0.1477 -G3 X1.0550 Y0.7550 Z-0.2009 I0.3484 K-0.1322 -G3 X1.1750 Y0.7550 Z-0.1202 I0.2011 K-0.1694 -G1 X1.2150 Z-0.1118 - X1.2500 - X1.2750 Z-0.1165 -G3 X1.3500 Y0.7550 Z-0.1585 I-0.0439 K-0.1662 -G3 X1.3950 Y0.7550 Z-0.2261 I-0.1500 K-0.1487 -G3 X1.4100 Y0.7550 Z-0.2995 I-0.1945 K-0.0780 -G1 X1.4250 Z-0.4740 - X1.4300 Z-0.5000 - Y0.7150 Z-0.4870 - X1.4100 Z-0.2811 -G2 X1.3900 Y0.7150 Z-0.2125 I-0.1947 K-0.0195 -G2 X1.3550 Y0.7150 Z-0.1624 I-0.1813 K-0.0896 -G1 X1.3200 Z-0.1347 - X1.2750 Z-0.1165 - X1.2500 Z-0.1118 - X1.2100 Z-0.1126 -G2 X1.1450 Y0.7150 Z-0.1319 I0.0226 K-0.1949 -G2 X1.0950 Y0.7150 Z-0.1641 I0.1568 K-0.2986 -G1 X1.0650 Z-0.1919 - X1.0450 Z-0.2154 -G2 X0.9850 Y0.7150 Z-0.3223 I0.2909 K-0.2336 -G2 X0.9700 Y0.7150 Z-0.3713 I0.6768 K-0.2339 -G2 X0.9500 Y0.7150 Z-0.4850 I1.2644 K-0.2810 -G1 X0.9450 Z-0.5000 - X0.4850 - X0.4800 Z-0.4850 -G2 X0.4600 Y0.7150 Z-0.3713 I-1.2844 K-0.1673 -G2 X0.4450 Y0.7150 Z-0.3223 I-0.6918 K-0.1849 -G2 X0.3650 Y0.7150 Z-0.1919 I-0.3584 K-0.1301 -G1 X0.3350 Z-0.1641 - X0.3100 Z-0.1465 -G2 X0.2550 Y0.7150 Z-0.1202 I-0.1129 K-0.1656 -G1 X0.2150 Z-0.1118 - X0.1800 - X0.1550 Z-0.1165 - X0.1100 Z-0.1347 - X0.0750 Z-0.1624 -G2 X0.0300 Y0.7150 Z-0.2360 I0.1483 K-0.1412 -G2 X0.0150 Y0.7150 Z-0.3223 I0.3189 K-0.0999 -G3 X0.0000 Y0.7150 Z-0.4870 I-3.0562 K0.1953 -G1 Y0.6750 Z-0.5000 - X0.0050 Z-0.4811 - X0.0200 Z-0.3045 - X0.0250 Z-0.2653 -G3 X0.0350 Y0.6750 Z-0.2262 I0.1963 K-0.0294 -G3 X0.0850 Y0.6750 Z-0.1543 I0.1853 K-0.0755 -G3 X0.1200 Y0.6750 Z-0.1300 I0.1024 K-0.1100 -G3 X0.1800 Y0.6750 Z-0.1118 I0.0800 K-0.1557 -G1 X0.2200 - X0.2450 Z-0.1165 - X0.2950 Z-0.1359 -G3 X0.3500 Y0.6750 Z-0.1742 I-0.1039 K-0.2075 -G3 X0.3900 Y0.6750 Z-0.2174 I-0.1513 K-0.1804 -G3 X0.4550 Y0.6750 Z-0.3398 I-0.3071 K-0.2415 -G3 X0.4700 Y0.6750 Z-0.3976 I-0.3760 K-0.1285 -G1 X0.4850 Z-0.4858 - X0.4900 Z-0.5000 - X0.9400 - X0.9450 Z-0.4858 -G3 X0.9650 Y0.6750 Z-0.3751 I1.6036 K-0.2326 -G3 X0.9850 Y0.6750 Z-0.3124 I0.3783 K-0.0860 -G3 X1.0650 Y0.6750 Z-0.1879 I0.3690 K-0.1493 -G3 X1.1100 Y0.6750 Z-0.1504 I0.2916 K-0.3036 -G1 X1.1350 Z-0.1359 - X1.1850 Z-0.1165 - X1.2100 Z-0.1118 - X1.2500 - X1.2750 Z-0.1165 -G3 X1.3500 Y0.6750 Z-0.1585 I-0.0439 K-0.1662 -G3 X1.3950 Y0.6750 Z-0.2262 I-0.1490 K-0.1478 -G3 X1.4050 Y0.6750 Z-0.2653 I-0.1863 K-0.0685 -G1 X1.4100 Z-0.3045 - X1.4250 Z-0.4811 - X1.4300 Z-0.5000 - Y0.6350 - X1.4250 Z-0.4908 - X1.4050 Z-0.2810 -G2 X1.3850 Y0.6350 Z-0.2125 I-0.1922 K-0.0190 -G2 X1.3300 Y0.6350 Z-0.1455 I-0.1715 K-0.0847 -G2 X1.2450 Y0.6350 Z-0.1118 I-0.1057 K-0.1423 -G1 X1.2050 - X1.1800 Z-0.1165 -G2 X1.1300 Y0.6350 Z-0.1348 I0.0430 K-0.1952 -G2 X1.0700 Y0.6350 Z-0.1769 I0.1240 K-0.2407 -G2 X1.0350 Y0.6350 Z-0.2134 I0.1752 K-0.2027 -G2 X0.9750 Y0.6350 Z-0.3124 I0.3211 K-0.2624 -G2 X0.9550 Y0.6350 Z-0.3701 I0.5256 K-0.2144 -G1 X0.9500 Z-0.3897 - X0.9300 Z-0.4976 - X0.9250 Z-0.5000 - X0.5050 - X0.5000 Z-0.4976 - X0.4800 Z-0.3897 -G2 X0.4550 Y0.6350 Z-0.3124 I-0.4733 K-0.1103 -G2 X0.3850 Y0.6350 Z-0.2017 I-0.3774 K-0.1612 -G2 X0.3250 Y0.6350 Z-0.1494 I-0.2374 K-0.2120 -G2 X0.2700 Y0.6350 Z-0.1221 I-0.1317 K-0.1964 -G2 X0.2250 Y0.6350 Z-0.1118 I-0.0674 K-0.1899 -G1 X0.1850 - X0.1600 Z-0.1165 -G2 X0.0800 Y0.6350 Z-0.1624 I0.0471 K-0.1746 -G2 X0.0350 Y0.6350 Z-0.2359 I0.1553 K-0.1457 -G2 X0.0250 Y0.6350 Z-0.2810 I0.2293 K-0.0745 -G1 X0.0050 Z-0.4908 - X0.0000 Z-0.5000 - Y0.5950 - X0.0050 - X0.0100 Z-0.4947 - X0.0300 Z-0.2858 -G3 X0.0500 Y0.5950 Z-0.2163 I0.2392 K-0.0312 -G3 X0.1900 Y0.5950 Z-0.1118 I0.1629 K-0.0722 -G1 X0.2400 Z-0.1126 -G3 X0.3050 Y0.5950 Z-0.1319 I-0.0334 K-0.2310 -G3 X0.3700 Y0.5950 Z-0.1749 I-0.1239 K-0.2580 -G3 X0.4100 Y0.5950 Z-0.2153 I-0.2535 K-0.2911 -G3 X0.4500 Y0.5950 Z-0.2722 I-0.2209 K-0.1979 -G3 X0.4800 Y0.5950 Z-0.3328 I-0.4453 K-0.2579 -G3 X0.5000 Y0.5950 Z-0.3956 I-0.4175 K-0.1677 -G1 X0.5200 Z-0.4976 - X0.5250 Z-0.5000 - X0.9050 - X0.9100 Z-0.4976 - X0.9300 Z-0.3956 -G3 X0.9500 Y0.5950 Z-0.3328 I0.3783 K-0.0860 -G3 X1.0350 Y0.5950 Z-0.1989 I0.3998 K-0.1598 -G3 X1.1500 Y0.5950 Z-0.1221 I0.2105 K-0.1907 -G3 X1.1950 Y0.5950 Z-0.1118 I0.0674 K-0.1899 -G1 X1.2400 - X1.2650 Z-0.1165 -G3 X1.3800 Y0.5950 Z-0.2163 I-0.0490 K-0.1726 -G3 X1.4000 Y0.5950 Z-0.2858 I-0.2192 K-0.1007 -G1 X1.4200 Z-0.4947 - X1.4250 Z-0.5000 - X1.4300 - Y0.5550 - X1.4150 - X1.4100 Z-0.4838 -G3 X1.3950 Y0.5550 Z-0.3152 I4.6155 K0.4955 -G1 X1.3900 Z-0.2760 -G2 X1.3650 Y0.5550 Z-0.2045 I-0.1874 K-0.0254 -G2 X1.2300 Y0.5550 Z-0.1118 I-0.1569 K-0.0837 -G1 X1.1850 - X1.1750 Z-0.1134 -G2 X1.1100 Y0.5550 Z-0.1328 I0.0224 K-0.1936 -G2 X1.0700 Y0.5550 Z-0.1553 I0.1540 K-0.3210 -G2 X1.0000 Y0.5550 Z-0.2153 I0.2382 K-0.3487 -G2 X0.9250 Y0.5550 Z-0.3300 I0.3378 K-0.3027 -G2 X0.9100 Y0.5550 Z-0.3673 I0.3468 K-0.1613 -G2 X0.8850 Y0.5550 Z-0.4731 I0.7799 K-0.2400 -G3 X0.8750 Y0.5550 Z-0.5000 I-0.0647 K0.0088 -G1 X0.5550 -G3 X0.5450 Y0.5550 Z-0.4731 I0.0547 K0.0357 -G2 X0.5200 Y0.5550 Z-0.3673 I-0.8049 K-0.1342 -G2 X0.5050 Y0.5550 Z-0.3300 I-0.3618 K-0.1240 -G2 X0.4700 Y0.5550 Z-0.2673 I-0.2977 K-0.1249 -G1 X0.4450 Z-0.2329 - X0.4300 Z-0.2153 -G2 X0.3600 Y0.5550 Z-0.1553 I-0.3082 K-0.2887 -G2 X0.2900 Y0.5550 Z-0.1211 I-0.1582 K-0.2347 -G1 X0.2450 Z-0.1118 - X0.2000 - X0.1750 Z-0.1165 -G2 X0.0550 Y0.5550 Z-0.2241 I0.0508 K-0.1774 -G2 X0.0400 Y0.5550 Z-0.2760 I0.1558 K-0.0732 -G1 X0.0350 Z-0.3152 -G3 X0.0200 Y0.5550 Z-0.4838 I-4.6305 K0.3269 -G1 X0.0150 Z-0.5000 - X0.0000 - Y0.5150 - X0.0250 - X0.0300 Z-0.4878 -G2 X0.0450 Y0.5150 Z-0.3211 I-3.7054 K0.4175 -G1 X0.0500 Z-0.2819 -G3 X0.0750 Y0.5150 Z-0.2065 I0.2091 K-0.0275 -G3 X0.2150 Y0.5150 Z-0.1118 I0.1614 K-0.0878 -G1 X0.2600 - X0.2800 Z-0.1145 - X0.3000 Z-0.1185 - X0.3500 Z-0.1378 -G3 X0.4000 Y0.5150 Z-0.1671 I-0.1210 K-0.2642 -G3 X0.4650 Y0.5150 Z-0.2237 I-0.2889 K-0.3970 -G3 X0.5400 Y0.5150 Z-0.3222 I-0.3935 K-0.3776 -G3 X0.5700 Y0.5150 Z-0.3858 I-0.5285 K-0.2881 -G1 X0.6000 Z-0.4779 - X0.6100 Z-0.5000 - X0.8200 - X0.8300 Z-0.4779 - X0.8600 Z-0.3858 -G3 X0.8900 Y0.5150 Z-0.3222 I0.5585 K-0.2245 -G3 X1.0000 Y0.5150 Z-0.1907 I0.4774 K-0.2878 -G3 X1.0700 Y0.5150 Z-0.1426 I0.2348 K-0.2666 -G1 X1.1300 Z-0.1185 -G3 X1.1700 Y0.5150 Z-0.1118 I0.0801 K-0.3555 -G1 X1.2150 -G3 X1.2550 Y0.5150 Z-0.1213 I-0.0194 K-0.1706 -G3 X1.3150 Y0.5150 Z-0.1566 I-0.0526 K-0.1581 -G3 X1.3650 Y0.5150 Z-0.2261 I-0.1272 K-0.1442 -G3 X1.3800 Y0.5150 Z-0.2819 I-0.2142 K-0.0875 -G1 X1.3850 Z-0.3211 -G2 X1.4000 Y0.5150 Z-0.4878 I3.7204 K0.2508 -G1 X1.4050 Z-0.5000 - X1.4300 - Y0.4750 - X1.3900 - X1.3850 Z-0.4740 - X1.3700 Z-0.3074 - X1.3650 Z-0.2721 -G2 X1.3350 Y0.4750 Z-0.1976 I-0.1862 K-0.0318 -G2 X1.3150 Y0.4750 Z-0.1715 I-0.1196 K-0.0707 -G2 X1.2800 Y0.4750 Z-0.1416 I-0.1699 K-0.1632 -G2 X1.1950 Y0.4750 Z-0.1118 I-0.1042 K-0.1611 -G1 X1.1450 -G2 X1.1000 Y0.4750 Z-0.1213 I0.0434 K-0.3169 -G2 X1.0000 Y0.4750 Z-0.1672 I0.1053 K-0.3611 -G1 X0.9750 Z-0.1845 - X0.9300 Z-0.2198 - X0.9050 Z-0.2427 -G2 X0.8500 Y0.4750 Z-0.2986 I0.8446 K-0.8861 -G1 X0.7800 Z-0.3760 - X0.7400 Z-0.4163 - X0.7300 Z-0.4222 - X0.7150 Z-0.4243 - X0.7000 Z-0.4222 - X0.6900 Z-0.4163 - X0.6500 Z-0.3760 - X0.5800 Z-0.2986 -G2 X0.5250 Y0.4750 Z-0.2427 I-0.8996 K-0.8302 -G2 X0.4250 Y0.4750 Z-0.1641 I-0.4292 K-0.4429 -G2 X0.3150 Y0.4750 Z-0.1173 I-0.1969 K-0.3105 -G1 X0.2850 Z-0.1118 - X0.2350 - X0.2000 Z-0.1185 -G2 X0.0850 Y0.4750 Z-0.2143 I0.0531 K-0.1806 -G2 X0.0650 Y0.4750 Z-0.2721 I0.1598 K-0.0877 -G1 X0.0600 Z-0.3074 - X0.0450 Z-0.4740 - X0.0400 Z-0.5000 - X0.0000 - Y0.4350 - X0.0550 - X0.0600 Z-0.4760 - X0.0750 Z-0.3172 - X0.0800 Z-0.2799 -G3 X0.1200 Y0.4350 Z-0.1859 I0.2031 K-0.0309 -G1 X0.1350 Z-0.1692 - X0.1700 Z-0.1416 - X0.1850 Z-0.1327 -G3 X0.2550 Y0.4350 Z-0.1118 I0.0915 K-0.1781 -G1 X0.3100 -G3 X0.3850 Y0.4350 Z-0.1289 I-0.0410 K-0.3522 -G1 X0.4500 Z-0.1563 - X0.4650 Z-0.1641 -G3 X0.6050 Y0.4350 Z-0.2506 I-1.2849 K-2.2366 -G1 X0.6350 Z-0.2653 - X0.6400 Z-0.2692 - X0.6700 Z-0.2810 - X0.6750 Z-0.2811 - X0.6850 Z-0.2849 - X0.7050 Z-0.2870 - X0.7300 Z-0.2869 - X0.7350 Z-0.2850 - X0.7450 Z-0.2849 - X0.7550 Z-0.2811 - X0.7600 Z-0.2810 - X0.7900 Z-0.2692 - X0.7950 Z-0.2653 - X0.8250 Z-0.2506 - X0.9100 Z-0.1966 - X0.9650 Z-0.1641 -G3 X1.0100 Y0.4350 Z-0.1445 I0.1129 K-0.1976 -G1 X1.0200 Z-0.1387 - X1.0500 Z-0.1271 -G3 X1.1200 Y0.4350 Z-0.1118 I0.1129 K-0.3471 -G1 X1.1750 - X1.2050 Z-0.1173 -G3 X1.2600 Y0.4350 Z-0.1416 I-0.0499 K-0.1879 -G1 X1.3000 Z-0.1741 -G3 X1.3250 Y0.4350 Z-0.2094 I-0.1155 K-0.1083 -G3 X1.3500 Y0.4350 Z-0.2799 I-0.1638 K-0.0978 -G1 X1.3550 Z-0.3172 - X1.3700 Z-0.4760 - X1.3750 Z-0.5000 - X1.4300 - Y0.3950 - X1.3550 - X1.3500 Z-0.4740 - X1.3350 Z-0.3132 - X1.3300 Z-0.2779 -G2 X1.2950 Y0.3950 Z-0.1937 I-0.1999 K-0.0337 -G2 X1.2250 Y0.3950 Z-0.1347 I-0.1574 K-0.1157 -G2 X1.1450 Y0.3950 Z-0.1118 I-0.0974 K-0.1890 -G1 X1.0900 -G2 X1.0250 Y0.3950 Z-0.1232 I0.0666 K-0.5680 -G1 X0.9600 Z-0.1445 - X0.8950 Z-0.1711 - X0.8850 Z-0.1769 - X0.8050 Z-0.2068 -G3 X0.7200 Y0.3950 Z-0.2223 I-0.1085 K0.3538 -G1 X0.6800 Z-0.2193 -G3 X0.6250 Y0.3950 Z-0.2068 I0.0615 K0.3967 -G1 X0.5450 Z-0.1769 - X0.5350 Z-0.1711 - X0.4700 Z-0.1445 - X0.4050 Z-0.1232 -G2 X0.3400 Y0.3950 Z-0.1118 I-0.1470 K-0.6436 -G1 X0.2850 -G2 X0.2400 Y0.3950 Z-0.1204 I0.0239 K-0.2452 -G2 X0.1150 Y0.3950 Z-0.2289 I0.0541 K-0.1886 -G2 X0.1000 Y0.3950 Z-0.2779 I0.1669 K-0.0779 -G1 X0.0950 Z-0.3132 - X0.0800 Z-0.4740 - X0.0750 Z-0.5000 - X0.0000 - Y0.3550 - X0.0950 - X0.1000 Z-0.4897 -G2 X0.1150 Y0.3550 Z-0.3387 I-2.5067 K0.3253 -G3 X0.1250 Y0.3550 Z-0.2681 I0.5491 K-0.0418 -G3 X0.1500 Y0.3550 Z-0.2074 I0.1750 K-0.0366 -G3 X0.1800 Y0.3550 Z-0.1711 I0.1329 K-0.0792 -G3 X0.2250 Y0.3550 Z-0.1386 I0.1413 K-0.1482 -G3 X0.2600 Y0.3550 Z-0.1232 I0.0857 K-0.1473 -G3 X0.3150 Y0.3550 Z-0.1118 I0.0965 K-0.3255 -G1 X0.3800 - X0.4200 Z-0.1173 - X0.5000 Z-0.1348 - X0.5650 Z-0.1533 - X0.6200 Z-0.1671 -G2 X0.7000 Y0.3550 Z-0.1778 I0.1137 K0.5417 -G1 X0.7300 - X0.7950 Z-0.1700 - X0.8350 Z-0.1612 - X0.9150 Z-0.1387 - X0.9700 Z-0.1252 -G3 X1.0500 Y0.3550 Z-0.1118 I0.1616 K-0.7183 -G1 X1.1150 -G3 X1.1700 Y0.3550 Z-0.1232 I-0.0548 K-0.4009 -G3 X1.2500 Y0.3550 Z-0.1711 I-0.0640 K-0.1977 -G3 X1.2950 Y0.3550 Z-0.2359 I-0.1246 K-0.1346 -G3 X1.3100 Y0.3550 Z-0.2956 I-0.2391 K-0.0918 -G1 X1.3300 Z-0.4897 - X1.3350 Z-0.5000 - X1.4300 - Y0.3150 - X1.3100 - X1.3050 Z-0.4917 - X1.2850 Z-0.3093 - X1.2800 Z-0.2779 -G2 X1.2600 Y0.3150 Z-0.2202 I-0.1831 K-0.0311 -G2 X1.1850 Y0.3150 Z-0.1445 I-0.1803 K-0.1036 -G3 X1.1650 Y0.3150 Z-0.1347 I0.0208 K0.0677 -G1 X1.1350 Z-0.1232 - X1.0750 Z-0.1118 - X1.0000 - X0.9800 Z-0.1137 -G3 X0.9400 Y0.3150 Z-0.1185 I-0.0543 K0.2873 -G2 X0.8900 Y0.3150 Z-0.1280 I0.1653 K-1.0018 -G1 X0.8250 Z-0.1376 - X0.7350 Z-0.1465 - X0.6750 Z-0.1455 - X0.6350 Z-0.1416 - X0.5400 Z-0.1280 -G2 X0.4900 Y0.3150 Z-0.1185 I-0.2158 K-0.9947 -G1 X0.4300 Z-0.1118 - X0.3550 - X0.2900 Z-0.1250 - X0.2650 Z-0.1347 -G3 X0.2450 Y0.3150 Z-0.1445 I-0.0408 K0.0579 -G1 X0.2100 Z-0.1708 -G2 X0.1650 Y0.3150 Z-0.2300 I0.1264 K-0.1428 -G2 X0.1500 Y0.3150 Z-0.2779 I0.1695 K-0.0793 -G1 X0.1450 Z-0.3093 - X0.1250 Z-0.4917 - X0.1200 Z-0.5000 - X0.0000 - Y0.2750 - X0.1500 - X0.1550 Z-0.4947 - X0.1600 Z-0.4642 - X0.1750 Z-0.3191 - X0.1800 Z-0.2858 -G3 X0.2100 Y0.2750 Z-0.2084 I0.1676 K-0.0205 -G3 X0.2900 Y0.2750 Z-0.1406 I0.1759 K-0.1263 -G1 X0.3450 Z-0.1213 - X0.4050 Z-0.1118 - X0.4950 - X0.6050 Z-0.1202 - X0.6150 Z-0.1221 - X0.6300 - X0.6400 Z-0.1241 - X0.6600 - X0.6700 Z-0.1260 - X0.7600 - X0.7700 Z-0.1241 - X0.7900 - X0.8000 Z-0.1221 - X0.8150 - X0.8250 Z-0.1202 - X0.9350 Z-0.1118 - X1.0250 - X1.0850 Z-0.1213 -G2 X1.1200 Y0.2750 Z-0.1327 I0.0718 K0.1601 -G3 X1.1600 Y0.2750 Z-0.1523 I-0.0415 K-0.1353 -G3 X1.1900 Y0.2750 Z-0.1747 I-0.0939 K-0.1571 -G3 X1.2350 Y0.2750 Z-0.2339 I-0.1273 K-0.1434 -G3 X1.2500 Y0.2750 Z-0.2858 I-0.1558 K-0.0732 -G1 X1.2550 Z-0.3191 - X1.2700 Z-0.4642 - X1.2750 Z-0.4947 - X1.2800 Z-0.5000 - X1.4300 - Y0.2350 - X1.2450 - X1.2400 Z-0.4928 - X1.2350 Z-0.4653 -G3 X1.2200 Y0.2350 Z-0.3339 I3.4995 K0.4661 -G2 X1.2100 Y0.2350 Z-0.2692 I-0.6965 K-0.0745 -G2 X1.1850 Y0.2350 Z-0.2124 I-0.1561 K-0.0347 -G2 X1.1500 Y0.2350 Z-0.1761 I-0.1487 K-0.1084 -G2 X1.1050 Y0.2350 Z-0.1465 I-0.1268 K-0.1436 -G1 X1.0600 Z-0.1289 - X1.0450 Z-0.1250 -G2 X0.9550 Y0.2350 Z-0.1118 I-0.1216 K-0.5155 -G1 X0.8200 - X0.7550 Z-0.1145 - X0.6750 - X0.6100 Z-0.1118 - X0.4750 -G2 X0.3850 Y0.2350 Z-0.1250 I0.0316 K-0.5287 -G1 X0.3700 Z-0.1289 - X0.3250 Z-0.1465 -G2 X0.3000 Y0.2350 Z-0.1610 I0.0636 K-0.1382 -G2 X0.2600 Y0.2350 Z-0.1943 I0.1578 K-0.2300 -G2 X0.2250 Y0.2350 Z-0.2516 I0.1092 K-0.1061 -G2 X0.2150 Y0.2350 Z-0.2967 I0.2293 K-0.0745 -G1 X0.1950 Z-0.4653 - X0.1900 Z-0.4928 - X0.1850 Z-0.5000 - X0.0000 - Y0.1950 - X0.2250 - X0.2300 Z-0.4928 - X0.2350 Z-0.4653 -G2 X0.2500 Y0.1950 Z-0.3378 I-2.7633 K0.3898 -G3 X0.2600 Y0.1950 Z-0.2771 I0.5783 K-0.0639 -G3 X0.2950 Y0.1950 Z-0.2065 I0.1507 K-0.0308 -G3 X0.3250 Y0.1950 Z-0.1786 I0.2080 K-0.1941 -G1 X0.3650 Z-0.1535 -G3 X0.4250 Y0.1950 Z-0.1309 I0.2024 K-0.4460 -G3 X0.4950 Y0.1950 Z-0.1182 I0.1565 K-0.6657 -G1 X0.5900 Z-0.1118 - X0.8400 - X0.9200 Z-0.1165 -G3 X1.0050 Y0.1950 Z-0.1309 I-0.0591 K-0.6085 -G3 X1.0650 Y0.1950 Z-0.1535 I-0.1424 K-0.4686 -G1 X1.1000 Z-0.1750 - X1.1250 Z-0.1963 -G3 X1.1600 Y0.1950 Z-0.2457 I-0.0982 K-0.1067 -G3 X1.1750 Y0.1950 Z-0.3026 I-0.2253 K-0.0898 -G1 X1.1950 Z-0.4653 - X1.2000 Z-0.4928 - X1.2050 Z-0.5000 - X1.4300 - Y0.1550 - X1.1600 - X1.1550 Z-0.4967 - X1.1500 Z-0.4740 - X1.1300 Z-0.3309 -G2 X1.1200 Y0.1550 Z-0.2779 I-0.6503 K-0.0954 -G2 X1.0850 Y0.1550 Z-0.2124 I-0.1433 K-0.0344 -G2 X1.0250 Y0.1550 Z-0.1672 I-0.1527 K-0.1404 -G1 X0.9600 Z-0.1416 - X0.8850 Z-0.1260 - X0.8450 Z-0.1221 - X0.8300 - X0.8200 Z-0.1202 - X0.7950 - X0.7800 Z-0.1182 - X0.6500 - X0.6350 Z-0.1202 - X0.6100 - X0.6000 Z-0.1221 - X0.5850 - X0.5450 Z-0.1260 -G2 X0.4700 Y0.1550 Z-0.1416 I0.0820 K-0.5854 -G1 X0.4050 Z-0.1672 -G2 X0.3450 Y0.1550 Z-0.2124 I0.0927 K-0.1855 -G2 X0.3150 Y0.1550 Z-0.2623 I0.0958 K-0.0916 -G2 X0.3050 Y0.1550 Z-0.3015 I0.1920 K-0.0699 -G1 X0.2800 Z-0.4740 - X0.2750 Z-0.4967 - X0.2700 Z-0.5000 - X0.0000 - Y0.1150 - X0.3300 Z-0.4995 -G2 X0.3400 Y0.1150 Z-0.4564 I-0.5300 K0.1456 -G1 X0.3600 Z-0.3309 -G3 X0.3700 Y0.1150 Z-0.2838 I0.3430 K-0.0483 -G3 X0.4050 Y0.1150 Z-0.2230 I0.1178 K-0.0274 -G3 X0.4600 Y0.1150 Z-0.1820 I0.1504 K-0.1439 -G1 X0.5200 Z-0.1578 - X0.5450 Z-0.1514 - X0.6050 Z-0.1406 - X0.6450 Z-0.1367 - X0.7000 Z-0.1347 - X0.7850 Z-0.1367 - X0.8250 Z-0.1406 - X0.8850 Z-0.1514 -G3 X0.9650 Y0.1150 Z-0.1793 I-0.0727 K-0.3365 -G3 X1.0150 Y0.1150 Z-0.2133 I-0.0933 K-0.1909 -G3 X1.0600 Y0.1150 Z-0.2838 I-0.0890 K-0.1064 -G3 X1.0700 Y0.1150 Z-0.3309 I-0.3330 K-0.0953 -G1 X1.0900 Z-0.4564 -G2 X1.1000 Y0.1150 Z-0.4995 I0.5400 K0.1025 -G1 X1.4300 Z-0.5000 - Y0.0750 - X1.0300 -G3 X1.0200 Y0.0750 Z-0.4740 I0.0429 K0.0314 -G1 X0.9900 Z-0.3230 -G2 X0.9600 Y0.0750 Z-0.2505 I-0.2142 K-0.0461 -G2 X0.9250 Y0.0750 Z-0.2182 I-0.1063 K-0.0802 -G1 X0.9050 Z-0.2084 - X0.9000 Z-0.2045 - X0.8600 Z-0.1888 - X0.8300 Z-0.1801 - X0.7900 Z-0.1723 - X0.7500 Z-0.1683 - X0.6800 - X0.6700 Z-0.1703 - X0.6550 -G2 X0.5950 Y0.0750 Z-0.1813 I0.0505 K-0.4447 -G1 X0.5800 Z-0.1852 - X0.5300 Z-0.2045 - X0.5250 Z-0.2084 - X0.5050 Z-0.2182 - X0.4850 Z-0.2339 -G2 X0.4500 Y0.0750 Z-0.2897 I0.0820 K-0.0903 -G2 X0.4350 Y0.0750 Z-0.3446 I0.4231 K-0.1451 -G3 X0.4100 Y0.0750 Z-0.4740 I-3.8695 K0.6804 -G3 X0.4000 Y0.0750 Z-0.5000 I-0.0529 K0.0055 -G1 X0.0000 - Y0.0350 - X0.5000 -G2 X0.5100 Y0.0350 Z-0.4878 I-0.0032 K0.0128 -G3 X0.5300 Y0.0350 Z-0.4348 I0.5135 K-0.1637 -G3 X0.5650 Y0.0350 Z-0.3289 I1.3000 K-0.3710 -G2 X0.5900 Y0.0350 Z-0.2878 I-0.3832 K0.2608 -G1 X0.6000 Z-0.2771 -G3 X0.6350 Y0.0350 Z-0.2516 I0.1125 K-0.1177 -G1 X0.6650 Z-0.2419 - X0.6850 Z-0.2398 - X0.7000 Z-0.2360 - X0.7300 - X0.7800 Z-0.2458 - X0.7950 Z-0.2516 - X0.8050 Z-0.2575 - X0.8350 Z-0.2819 - X0.8450 Z-0.2956 -G2 X0.8650 Y0.0350 Z-0.3289 I0.1832 K0.0873 -G3 X0.9000 Y0.0350 Z-0.4348 I-1.2650 K-0.4769 -G1 X0.9250 Z-0.4976 - X0.9300 Z-0.5000 - X1.4300 - Y0.0050 - X0.7450 - X0.7300 Z-0.4888 - X0.7250 Z-0.4870 - X0.7050 - X0.7000 Z-0.4888 - X0.6850 Z-0.5000 - X0.0000 -G19 -G0 Z0.0120 - X1.4000 Y1.0650 -G1 Z-0.3750 -G2 Y0.940000 Z-0.500000 R0.125000 -G1 Y0.9350 - Y0.9300 Z-0.4976 - Y0.9250 Z-0.4878 - Y0.9050 Z-0.4348 -G3 X1.4000 Y0.8700 Z-0.3289 J-1.3000 K-0.3710 -G1 Y0.8450 Z-0.2858 - Y0.8100 Z-0.2575 -G0 Z0.0120 - Y0.6301 -G1 Z-0.2562 -G2 Y0.625000 Z-0.261378 R0.005148 -G1 Y0.5950 Z-0.2858 - Y0.5700 Z-0.3289 -G3 X1.4000 Y0.5350 Z-0.4348 J1.2650 K-0.4769 -G1 Y0.5150 Z-0.4878 - Y0.5100 Z-0.4976 - Y0.5050 Z-0.5000 - Y0.4950 -G0 Z0.0120 - X1.3600 Y0.2850 -G1 Z-0.3667 -G3 Y0.410000 Z-0.491673 R0.125000 -G3 X1.3600 Y0.4200 Z-0.4505 J-0.3032 K0.0954 -G1 Y0.4400 Z-0.3446 -G2 X1.3600 Y0.4550 Z-0.2897 J0.4381 K-0.0902 -G2 X1.3600 Y0.4950 Z-0.2300 J0.1159 K-0.0344 -G0 Z0.0120 - Y0.9450 -G1 Z-0.2298 -G3 Y0.950000 Z-0.234810 R0.005004 -G2 X1.3600 Y0.9850 Z-0.2897 J-0.0740 K-0.0858 -G2 X1.3600 Y1.0000 Z-0.3446 J-0.4231 K-0.1451 -G1 Y1.0250 Z-0.4740 -G3 X1.3600 Y1.0350 Z-0.5000 J0.0529 K0.0055 -G0 Z0.0120 - X1.3200 Y1.2450 -G1 Z-0.3750 -G2 Y1.120000 Z-0.500000 R0.125000 -G1 Y1.1050 Z-0.4995 - Y1.1000 Z-0.4819 -G2 X1.3200 Y1.0750 Z-0.3309 J3.8276 K0.7114 -G3 X1.3200 Y1.0650 Z-0.2838 J-0.3430 K-0.0483 -G3 X1.3200 Y1.0350 Z-0.2289 J-0.1104 K-0.0247 -G0 Z0.0120 - Y0.4100 -G1 Z-0.2239 -G2 Y0.400000 Z-0.233927 R0.010000 -G3 X1.3200 Y0.3700 Z-0.3034 J0.1057 K-0.0868 -G1 Y0.3450 Z-0.4544 -G2 X1.3200 Y0.3350 Z-0.4995 J-0.2393 K0.0294 -G1 Y0.3150 Z-0.5000 -G0 Z0.0120 - X1.2800 Y0.1500 -G1 Z-0.3750 -G3 Y0.275000 Z-0.500000 R0.125000 -G1 Y0.2800 Z-0.4967 - Y0.2850 Z-0.4740 - Y0.3050 Z-0.3309 -G2 X1.2800 Y0.3150 Z-0.2779 J0.6503 K-0.0954 -G2 X1.2800 Y0.3600 Z-0.2026 J0.1430 K-0.0342 -G0 Z0.0120 - Y1.0800 -G1 Z-0.2024 -G3 Y1.085000 Z-0.207445 R0.005001 -G1 Y1.0950 Z-0.2182 - Y1.1100 Z-0.2398 -G2 X1.2800 Y1.1250 Z-0.2779 J-0.1358 K-0.0754 -G2 X1.2800 Y1.1350 Z-0.3309 J-0.6403 K-0.1484 -G1 Y1.1550 Z-0.4740 - Y1.1600 Z-0.4967 - Y1.1650 Z-0.5000 - Y1.1700 -G0 Z0.0120 - X1.2400 Y1.3350 -G1 Z-0.3750 -G2 Y1.210000 Z-0.500000 R0.125000 -G1 Y1.2050 Z-0.4928 - Y1.2000 Z-0.4673 -G2 X1.2400 Y1.1850 Z-0.3378 J3.9383 K0.5221 -G3 X1.2400 Y1.1750 Z-0.2751 J-0.8001 K-0.0953 -G3 X1.2400 Y1.1250 Z-0.1918 J-0.1549 K-0.0363 -G0 Z0.0120 - Y0.3150 -G1 Z-0.1913 -G2 Y0.310000 Z-0.196283 R0.005025 -G3 X1.2400 Y0.2650 Z-0.2751 J0.1028 K-0.1109 -G3 X1.2400 Y0.2550 Z-0.3378 J0.7901 K-0.1581 -G2 X1.2400 Y0.2400 Z-0.4673 J-3.9533 K0.3926 -G1 Y0.2350 Z-0.4928 - Y0.2300 Z-0.5000 - Y0.2250 -G0 Z0.0120 - X1.2000 Y0.0600 -G1 Z-0.3750 -G3 Y0.185000 Z-0.500000 R0.125000 -G1 Y0.1900 - Y0.1950 Z-0.4928 - Y0.2000 Z-0.4653 -G3 X1.2000 Y0.2150 Z-0.3339 J-3.4995 K0.4661 -G2 X1.2000 Y0.2250 Z-0.2692 J0.6965 K-0.0745 -G2 X1.2000 Y0.2700 Z-0.1898 J0.1505 K-0.0328 -G0 Z0.0120 - Y1.1700 -G1 Z-0.1893 -G3 Y1.175000 Z-0.194322 R0.005025 -G2 X1.2000 Y1.2150 Z-0.2692 J-0.1105 K-0.1071 -G2 X1.2000 Y1.2250 Z-0.3339 J-0.6865 K-0.1392 -G3 X1.2000 Y1.2400 Z-0.4653 J3.5145 K0.3347 -G1 Y1.2450 Z-0.4928 - Y1.2500 Z-0.5000 - Y1.2600 -G0 Z0.0120 - X1.1600 Y1.4250 -G1 Z-0.3750 -G2 Y1.300000 Z-0.500000 R0.125000 -G1 Y1.2850 - Y1.2800 Z-0.4928 - Y1.2750 Z-0.4642 - Y1.2600 Z-0.3191 -G3 X1.1600 Y1.2500 Z-0.2614 J-0.5479 K-0.0652 -G3 X1.1600 Y1.2150 Z-0.1963 J-0.1504 K-0.0389 -G0 Z0.0120 - Y0.2250 -G1 Z-0.1972 -G2 Y0.220000 Z-0.202165 R0.005000 -G3 X1.1600 Y0.1900 Z-0.2614 J0.1104 K-0.0931 -G3 X1.1600 Y0.1800 Z-0.3191 J0.5379 K-0.1229 -G1 Y0.1650 Z-0.4642 - Y0.1600 Z-0.4928 - Y0.1550 Z-0.5000 - Y0.1350 -G0 Z0.0120 - X1.1200 Y-0.0300 -G1 Z-0.3750 -G3 Y0.095000 Z-0.500000 R0.125000 -G1 Y0.1250 - Y0.1300 Z-0.4917 - Y0.1500 Z-0.3093 - Y0.1550 Z-0.2779 -G2 X1.1200 Y0.1800 Z-0.2124 J0.1727 K-0.0283 -G0 Z0.0120 - Y1.2600 -G1 Z-0.2152 -G3 Y1.265000 Z-0.220201 R0.005000 -G2 X1.1200 Y1.2850 Z-0.2779 J-0.1598 K-0.0877 -G1 Y1.2900 Z-0.3093 - Y1.3100 Z-0.4917 - Y1.3150 Z-0.5000 - Y1.3500 -G0 Z0.0120 - X1.0800 Y1.4700 -G1 Z-0.3750 -G2 Y1.345000 Z-0.500000 R0.125000 -G1 Y1.3400 - Y1.3350 Z-0.4897 -G2 X1.0800 Y1.3200 Z-0.3387 J2.5067 K0.3253 -G3 X1.0800 Y1.3100 Z-0.2681 J-0.5491 K-0.0418 -G3 X1.0800 Y1.2600 Z-0.1761 J-0.1894 K-0.0433 -G0 Z0.0120 - Y0.1800 -G1 Z-0.1770 -G2 Y0.175000 Z-0.181955 R0.005000 -G3 X1.0800 Y0.1500 Z-0.2163 J0.0910 K-0.0925 -G3 X1.0800 Y0.1300 Z-0.2681 J0.1780 K-0.0984 -G3 X1.0800 Y0.1200 Z-0.3387 J0.5391 K-0.1124 -G2 X1.0800 Y0.1050 Z-0.4897 J-2.5217 K0.1743 -G1 Y0.1000 Z-0.5000 - Y0.0900 -G0 Z0.0120 - X1.0400 Y-0.0750 -G1 Z-0.3750 -G3 Y0.050000 Z-0.500000 R0.125000 -G1 Y0.0800 - Y0.0850 Z-0.4740 - Y0.1000 Z-0.3132 - Y0.1050 Z-0.2779 -G2 X1.0400 Y0.1350 Z-0.2016 J0.1940 K-0.0321 -G0 Z0.0120 - Y1.3050 -G1 Z-0.2054 -G3 Y1.310000 Z-0.210398 R0.005000 -G2 X1.0400 Y1.3350 Z-0.2779 J-0.1559 K-0.0961 -G1 Y1.3400 Z-0.3132 - Y1.3550 Z-0.4740 - Y1.3600 Z-0.5000 - Y1.3950 -G0 Z0.0120 - X1.0000 Y1.5150 -G1 Z-0.3750 -G2 Y1.390000 Z-0.500000 R0.125000 -G1 Y1.3800 - Y1.3750 Z-0.4779 - Y1.3600 Z-0.3172 -G3 X1.0000 Y1.3500 Z-0.2564 J-0.4043 K-0.0353 -G3 X1.0000 Y1.3050 Z-0.1741 J-0.1963 K-0.0539 -G0 Z0.0120 - Y0.1350 -G1 Z-0.1750 -G2 Y0.130000 Z-0.179994 R0.005000 -G3 X1.0000 Y0.0900 Z-0.2564 J0.1737 K-0.1396 -G3 X1.0000 Y0.0800 Z-0.3172 J0.3943 K-0.0961 -G1 Y0.0650 Z-0.4779 - Y0.0600 Z-0.5000 - Y0.0450 -G0 Z0.0120 - X0.9600 Y-0.1200 -G1 Z-0.3750 -G3 Y0.005000 Z-0.500000 R0.125000 -G1 Y0.0450 - Y0.0500 Z-0.4740 - Y0.0650 Z-0.3074 - Y0.0700 Z-0.2721 -G2 X0.9600 Y0.0900 Z-0.2143 J0.1831 K-0.0311 -G0 Z0.0120 - Y1.3450 -G1 Z-0.2093 -G3 Y1.350000 Z-0.214319 R0.005000 -G2 X0.9600 Y1.3700 Z-0.2721 J-0.1598 K-0.0877 -G1 Y1.3750 Z-0.3074 - Y1.3900 Z-0.4740 - Y1.3950 Z-0.5000 - Y1.4350 -G0 Z0.0120 - X0.9200 Y1.5600 -G1 Z-0.3750 -G2 Y1.435000 Z-0.500000 R0.125000 -G1 Y1.4100 - Y1.4050 Z-0.4858 -G2 X0.9200 Y1.3900 Z-0.3211 J3.0412 K0.3600 -G1 Y1.3850 Z-0.2819 -G3 X0.9200 Y1.3500 Z-0.1908 J-0.1937 K-0.0222 -G0 Z0.0120 - Y0.9000 -G1 Z-0.3172 -G2 Y0.895000 Z-0.322162 R0.005000 -G3 X0.9200 Y0.8650 Z-0.3858 J0.5285 K-0.2881 -G1 Y0.8300 Z-0.4917 - Y0.8250 Z-0.5000 - Y0.8100 -G0 Z0.0120 - Y0.7500 -G1 Z-0.3750 -G2 Y0.625000 Z-0.500000 R0.125000 -G1 Y0.6150 - Y0.6100 Z-0.4917 - Y0.5750 Z-0.3858 -G3 X0.9200 Y0.5400 Z-0.3143 J-0.4659 K-0.1838 -G0 Z0.0120 - Y0.0850 -G1 Z-0.2015 -G2 Y0.080000 Z-0.206476 R0.005000 -G3 X0.9200 Y0.0550 Z-0.2819 J0.1653 K-0.0966 -G1 Y0.0500 Z-0.3211 -G2 X0.9200 Y0.0350 Z-0.4858 J-3.0562 K0.1953 -G1 Y0.0300 Z-0.5000 - Y0.0050 -G0 Z0.0120 - X0.8800 Y-0.1200 -G1 Z-0.3750 -G3 Y0.005000 Z-0.500000 R0.125000 -G1 Y0.0200 - Y0.0250 Z-0.4838 -G3 X0.8800 Y0.0400 Z-0.3152 J-4.6155 K0.4955 -G2 X0.8800 Y0.0500 Z-0.2516 J0.4273 K-0.0345 -G2 X0.8800 Y0.0900 Z-0.1754 J0.1935 K-0.0530 -G0 Z0.0120 - Y0.4500 -G1 Z-0.2358 -G3 Y0.455000 Z-0.240779 R0.005000 -G2 X0.8800 Y0.4900 Z-0.2908 J-0.1806 K-0.1637 -G2 X0.8800 Y0.5150 Z-0.3418 J-0.2462 K-0.1523 -G2 X0.8800 Y0.5300 Z-0.3849 J-0.5212 K-0.2054 -G1 Y0.5550 Z-0.4908 - Y0.5600 Z-0.5000 - Y0.5850 -G0 Z0.0120 - Y0.7350 -G1 Z-0.3750 -G3 Y0.860000 Z-0.500000 R0.125000 -G1 Y0.8800 - Y0.8850 Z-0.4908 - Y0.9100 Z-0.3849 -G2 X0.8800 Y0.9250 Z-0.3418 J0.5362 K-0.1623 -G2 X0.8800 Y0.9500 Z-0.2908 J0.2712 K-0.1014 -G2 X0.8800 Y0.9900 Z-0.2329 J0.4097 K-0.2405 -G0 Z0.0120 - Y1.3650 -G1 Z-0.1995 -G3 Y1.370000 Z-0.204515 R0.005000 -G2 X0.8800 Y1.3950 Z-0.2760 J-0.1624 K-0.0969 -G1 Y1.4000 Z-0.3152 -G3 X0.8800 Y1.4150 Z-0.4838 J4.6305 K0.3269 -G1 Y1.4200 Z-0.5000 - Y1.4350 -G0 Z0.0120 - X0.8400 Y1.5600 -G1 Z-0.3750 -G2 Y1.435000 Z-0.500000 R0.125000 -G1 Y1.4300 - Y1.4250 Z-0.4936 -G2 X0.8400 Y1.4100 Z-0.3309 J2.0906 K0.2748 -G3 X0.8400 Y1.4000 Z-0.2594 J-0.6072 K-0.0485 -G3 X0.8400 Y1.3500 Z-0.1656 J-0.1791 K-0.0352 -G0 Z0.0120 - Y0.9900 -G1 Z-0.2672 -G2 Y0.985000 Z-0.272151 R0.005000 -G3 X0.8400 Y0.9400 Z-0.3760 J0.3975 K-0.2339 -G3 X0.8400 Y0.9300 Z-0.4191 J0.5300 K-0.1456 -G2 X0.8400 Y0.9150 Z-0.4976 J-1.3493 K0.2174 -G1 Y0.9100 Z-0.5000 - Y0.9000 -G0 Z0.0120 - Y0.6600 -G1 Z-0.3750 -G2 Y0.535000 Z-0.500000 R0.125000 -G1 Y0.5300 - Y0.5250 Z-0.4976 - Y0.5050 Z-0.3956 -G3 X0.8400 Y0.4950 Z-0.3603 J-0.3043 K-0.0672 -G3 X0.8400 Y0.4500 Z-0.2633 J-0.4346 K-0.1428 -G0 Z0.0120 - Y0.0650 -G1 Z-0.2015 -G2 Y0.060000 Z-0.206476 R0.005000 -G3 X0.8400 Y0.0400 Z-0.2594 J0.1413 K-0.0836 -G3 X0.8400 Y0.0300 Z-0.3309 J0.5972 K-0.1200 -G2 X0.8400 Y0.0150 Z-0.4936 J-2.1056 K0.1120 -G1 Y0.0100 Z-0.5000 - Y0.0050 -G0 Z0.0120 - X0.8000 Y-0.1200 -G1 Z-0.3750 -G3 Y0.005000 Z-0.500000 R0.125000 -G1 Y0.0100 Z-0.4908 -G3 X0.8000 Y0.0250 Z-0.3230 J-2.6266 K0.3194 -G2 X0.8000 Y0.0350 Z-0.2555 J0.5120 K-0.0413 -G2 X0.8000 Y0.0900 Z-0.1582 J0.1850 K-0.0404 -G0 Z0.0120 - Y0.4050 -G1 Z-0.2214 -G3 Y0.410000 Z-0.226387 R0.005000 -G2 X0.8000 Y0.4700 Z-0.3387 J-0.3148 K-0.2403 -G2 X0.8000 Y0.4850 Z-0.3897 J-0.4248 K-0.1527 -G1 Y0.5050 Z-0.4976 - Y0.5100 Z-0.5000 - Y0.5400 -G0 Z0.0120 - Y0.7800 -G1 Z-0.3750 -G3 Y0.905000 Z-0.500000 R0.125000 -G1 Y0.9300 - Y0.9350 Z-0.4976 - Y0.9550 Z-0.3897 -G2 X0.8000 Y0.9700 Z-0.3387 J0.4398 K-0.1017 -G2 X0.8000 Y1.0350 Z-0.2203 J0.3683 K-0.1252 -G0 Z0.0120 - Y1.3800 -G1 Z-0.1995 -G3 Y1.385000 Z-0.204515 R0.005000 -G2 X0.8000 Y1.4000 Z-0.2378 J-0.0821 K-0.0570 -G2 X0.8000 Y1.4100 Z-0.2819 J-0.2424 K-0.0782 -G1 Y1.4150 Z-0.3230 -G3 X0.8000 Y1.4300 Z-0.4908 J2.6416 K0.1517 -G1 Y1.4350 Z-0.5000 -G0 Z0.0120 - X0.7600 Y1.5600 -G1 Z-0.3750 -G2 Y1.435000 Z-0.500000 R0.125000 -G1 Y1.4300 Z-0.4811 - Y1.4150 Z-0.3045 -G3 X0.7600 Y1.4050 Z-0.2418 J-0.3841 K-0.0290 -G3 X0.7600 Y1.3850 Z-0.1968 J-0.1489 K-0.0393 -G3 X0.7600 Y1.3500 Z-0.1543 J-0.1622 K-0.0979 -G0 Z0.0120 - Y1.0350 -G1 Z-0.2330 -G2 Y1.030000 Z-0.237987 R0.005000 -G3 X0.7600 Y0.9800 Z-0.3398 J0.3107 K-0.2158 -G3 X0.7600 Y0.9650 Z-0.3967 J0.3961 K-0.1349 -G1 Y0.9500 Z-0.4858 - Y0.9450 Z-0.5000 -G0 Z0.0120 - Y0.6150 -G1 Z-0.3608 -G2 Y0.490000 Z-0.485791 R0.125000 -G1 Y0.4750 Z-0.3967 -G3 X0.7600 Y0.4600 Z-0.3398 J-0.4111 K-0.0780 -G3 X0.7600 Y0.4050 Z-0.2311 J-0.3725 K-0.1202 -G0 Z0.0120 - Y0.0600 -G1 Z-0.1918 -G2 Y0.055000 Z-0.196811 R0.005000 -G3 X0.7600 Y0.0300 Z-0.2653 J0.1399 K-0.0899 -G1 Y0.0250 Z-0.3045 - Y0.0100 Z-0.4811 - Y0.0050 Z-0.5000 -G0 Z0.0120 - X0.7200 Y-0.1200 -G1 Z-0.3620 -G3 Y0.005000 Z-0.487007 R0.125000 -G3 X0.7200 Y0.0200 Z-0.3223 J-3.0412 K0.3600 -G2 X0.7200 Y0.0300 Z-0.2537 J0.4423 K-0.0294 -G2 X0.7200 Y0.0900 Z-0.1543 J0.1818 K-0.0420 -G0 Z0.0120 - Y0.4050 -G1 Z-0.2389 -G3 Y0.410000 Z-0.243869 R0.005000 -G2 X0.7200 Y0.4550 Z-0.3378 J-0.2598 K-0.1821 -G2 X0.7200 Y0.4700 Z-0.3948 J-0.3439 K-0.1210 -G1 Y0.4850 Z-0.4850 - Y0.4900 Z-0.5000 - Y0.4950 -G0 Z0.0120 - Y0.8250 -G1 Z-0.3750 -G3 Y0.950000 Z-0.500000 R0.125000 -G1 Y0.9550 Z-0.4850 - Y0.9750 Z-0.3713 -G2 X0.7200 Y0.9850 Z-0.3378 J0.4897 K-0.1281 -G2 X0.7200 Y1.0350 Z-0.2360 J0.3464 K-0.1069 -G0 Z0.0120 - Y1.3800 -G1 Z-0.1898 -G3 Y1.385000 Z-0.194850 R0.005000 -G2 X0.7200 Y1.4150 Z-0.2792 J-0.1697 K-0.1079 -G1 Y1.4350 Z-0.4870 -G0 Z0.0120 - X0.6800 Y1.5600 -G1 Z-0.3750 -G2 Y1.435000 Z-0.500000 R0.125000 -G1 Y1.4300 Z-0.4740 - Y1.4150 Z-0.2995 -G3 X0.6800 Y1.4050 Z-0.2399 J-0.3112 K-0.0216 -G3 X0.6800 Y1.3500 Z-0.1543 J-0.1826 K-0.0568 -G0 Z0.0120 - Y1.0350 -G1 Z-0.2349 -G2 Y1.030000 Z-0.239948 R0.005000 -G3 X0.6800 Y0.9850 Z-0.3300 J0.2882 K-0.2003 -G3 X0.6800 Y0.9700 Z-0.3810 J0.4248 K-0.1527 -G1 Y0.9500 Z-0.4947 - Y0.9450 Z-0.5000 -G0 Z0.0120 - Y0.6150 -G1 Z-0.3697 -G2 Y0.490000 Z-0.494711 R0.125000 -G1 Y0.4700 Z-0.3810 -G3 X0.6800 Y0.4550 Z-0.3300 J-0.4398 K-0.1017 -G3 X0.6800 Y0.4050 Z-0.2321 J-0.3765 K-0.1306 -G0 Z0.0120 - Y0.0550 -G1 Z-0.1997 -G2 Y0.050000 Z-0.204654 R0.005000 -G3 X0.6800 Y0.0300 Z-0.2615 J0.1517 K-0.0853 -G1 Y0.0250 Z-0.2995 - Y0.0100 Z-0.4740 - Y0.0050 Z-0.5000 -G0 Z0.0120 - X0.6400 Y-0.1200 -G1 Z-0.3750 -G3 Y0.005000 Z-0.500000 R0.125000 -G1 Y0.0100 Z-0.4849 -G3 X0.6400 Y0.0250 Z-0.3143 J-3.3931 K0.3843 -G2 X0.6400 Y0.0350 Z-0.2496 J0.4880 K-0.0423 -G2 X0.6400 Y0.0900 Z-0.1566 J0.1922 K-0.0509 -G0 Z0.0120 - Y0.4050 -G1 Z-0.2251 -G3 Y0.410000 Z-0.230144 R0.005000 -G2 X0.6400 Y0.4700 Z-0.3485 J-0.3119 K-0.2324 -G2 X0.6400 Y0.4850 Z-0.4074 J-0.3558 K-0.1221 -G1 Y0.5000 Z-0.4936 - Y0.5050 Z-0.5000 - Y0.5400 -G0 Z0.0120 - Y0.7800 -G1 Z-0.3750 -G3 Y0.905000 Z-0.500000 R0.125000 -G1 Y0.9350 - Y0.9400 Z-0.4936 -G2 X0.6400 Y0.9600 Z-0.3838 J2.1792 K-0.3402 -G2 X0.6400 Y0.9750 Z-0.3339 J0.3678 K-0.0833 -G2 X0.6400 Y1.0350 Z-0.2232 J0.3591 K-0.1231 -G0 Z0.0120 - Y1.3800 -G1 Z-0.1976 -G3 Y1.385000 Z-0.202554 R0.005000 -G2 X0.6400 Y1.4000 Z-0.2339 J-0.0925 K-0.0635 -G2 X0.6400 Y1.4100 Z-0.2751 J-0.1734 K-0.0639 -G1 Y1.4150 Z-0.3143 -G3 X0.6400 Y1.4300 Z-0.4849 J3.4081 K0.2137 -G1 Y1.4350 Z-0.5000 -G0 Z0.0120 - X0.6000 Y1.5600 -G1 Z-0.3750 -G2 Y1.435000 Z-0.500000 R0.125000 -G1 Y1.4300 - Y1.4250 Z-0.4869 -G2 X0.6000 Y1.4100 Z-0.3172 J3.4555 K0.3909 -G3 X0.6000 Y1.4000 Z-0.2516 J-0.4751 K-0.0389 -G3 X0.6000 Y1.3500 Z-0.1636 J-0.1827 K-0.0457 -G0 Z0.0120 - Y1.0350 -G1 Z-0.2096 -G2 Y1.030000 Z-0.214622 R0.005000 -G3 X0.6000 Y0.9950 Z-0.2623 J0.2278 K-0.2037 -G3 X0.6000 Y0.9600 Z-0.3328 J0.3779 K-0.2315 -G3 X0.6000 Y0.9400 Z-0.3976 J0.3605 K-0.1469 -G1 Y0.9200 Z-0.4995 - Y0.9000 Z-0.5000 -G0 Z0.0120 - Y0.6600 -G1 Z-0.3750 -G2 Y0.535000 Z-0.500000 R0.125000 -G1 Y0.5200 Z-0.4995 - Y0.5000 Z-0.3976 - Y0.4900 Z-0.3603 -G3 X0.6000 Y0.4500 Z-0.2712 J-0.4493 K-0.1481 -G0 Z0.0120 - Y0.0650 -G1 Z-0.1977 -G2 Y0.060000 Z-0.202693 R0.005000 -G3 X0.6000 Y0.0350 Z-0.2771 J0.1639 K-0.0965 -G1 Y0.0300 Z-0.3172 -G2 X0.6000 Y0.0150 Z-0.4869 J-3.4705 K0.2212 -G1 Y0.0100 Z-0.5000 - Y0.0050 -G0 Z0.0120 - X0.5600 Y-0.1200 -G1 Z-0.3750 -G3 Y0.005000 Z-0.500000 R0.125000 -G1 Y0.0200 Z-0.4995 - Y0.0250 Z-0.4623 -G2 X0.5600 Y0.0400 Z-0.2936 J4.6305 K-0.3269 -G2 X0.5600 Y0.0500 Z-0.2437 J0.3561 K-0.0454 -G2 X0.5600 Y0.0900 Z-0.1715 J0.2014 K-0.0643 -G0 Z0.0120 - Y0.4500 -G1 Z-0.2436 -G3 Y0.455000 Z-0.248622 R0.005000 -G1 Y0.4850 Z-0.2947 -G2 X0.5600 Y0.5100 Z-0.3477 J-0.3664 K-0.2054 -G2 X0.5600 Y0.5250 Z-0.3967 J-0.7470 K-0.2554 -G1 Y0.5450 Z-0.4888 - Y0.5500 Z-0.5000 - Y0.5850 -G0 Z0.0120 - Y0.7350 -G1 Z-0.3750 -G3 Y0.860000 Z-0.500000 R0.125000 -G1 Y0.8900 - Y0.8950 Z-0.4888 - Y0.9150 Z-0.3967 -G2 X0.5600 Y0.9300 Z-0.3477 J0.7620 K-0.2064 -G2 X0.5600 Y0.9900 Z-0.2408 J0.4134 K-0.1618 -G0 Z0.0120 - Y1.3700 -G1 Z-0.2034 -G3 Y1.375000 Z-0.208437 R0.005000 -G2 X0.5600 Y1.4000 Z-0.2936 J-0.1851 K-0.1006 -G2 X0.5600 Y1.4150 Z-0.4623 J-4.6155 K-0.4955 -G1 Y1.4200 Z-0.4995 - Y1.4350 Z-0.5000 -G0 Z0.0120 - X0.5200 Y1.5600 -G1 Z-0.3750 -G2 Y1.435000 Z-0.500000 R0.125000 -G1 Y1.4100 Z-0.4995 -G2 X0.5200 Y1.3950 Z-0.3427 J1.8579 K0.2568 -G3 X0.5200 Y1.3850 Z-0.2642 J-0.7557 K-0.0565 -G3 X0.5200 Y1.3500 Z-0.1869 J-0.1831 K-0.0362 -G0 Z0.0120 - Y0.9450 -G1 Z-0.2658 -G2 Y0.940000 Z-0.270792 R0.005000 -G1 Y0.9100 Z-0.3163 -G3 X0.5200 Y0.8850 Z-0.3673 J0.6673 K-0.3589 -G1 Y0.8450 Z-0.4976 - Y0.8400 Z-0.5000 - Y0.8100 -G0 Z0.0120 - Y0.7500 -G1 Z-0.3750 -G2 Y0.625000 Z-0.500000 R0.125000 -G1 Y0.6000 - Y0.5950 Z-0.4976 - Y0.5550 Z-0.3673 -G3 X0.5200 Y0.4950 Z-0.2633 J-0.4330 K-0.1807 -G0 Z0.0120 - Y0.0800 -G1 Z-0.2054 -G2 Y0.075000 Z-0.210398 R0.005000 -G3 X0.5200 Y0.0500 Z-0.2956 J0.1785 K-0.0986 -G3 X0.5200 Y0.0350 Z-0.4603 J3.9029 K-0.4385 -G1 Y0.0300 Z-0.4995 - Y0.0050 Z-0.5000 -G0 Z0.0120 - X0.4800 Y-0.1200 -G1 Z-0.3750 -G3 Y0.005000 Z-0.500000 R0.125000 -G1 Y0.0400 - Y0.0450 Z-0.4858 - Y0.0650 Z-0.2799 -G2 X0.4800 Y0.0900 Z-0.2084 J0.1712 K-0.0198 -G0 Z0.0120 - Y0.6300 -G1 Z-0.3847 -G3 Y0.635000 Z-0.389712 R0.005000 -G3 X0.4800 Y0.6750 Z-0.4564 J0.6606 K0.3510 -G0 Z0.0120 - Y0.6450 -G1 Z-0.3235 -G3 Y0.770000 Z-0.448536 R0.125000 -G1 Y0.8100 Z-0.3819 -G0 Z0.0120 - Y1.3500 -G1 Z-0.2113 -G3 Y1.355000 Z-0.216280 R0.005000 -G2 X0.4800 Y1.3750 Z-0.2799 J-0.1638 K-0.0864 -G1 Y1.3950 Z-0.4858 - Y1.4000 Z-0.5000 - Y1.4350 -G0 Z0.0120 - X0.4400 Y1.5150 -G1 Z-0.3750 -G2 Y1.390000 Z-0.500000 R0.125000 -G1 Y1.3850 - Y1.3800 Z-0.4858 -G2 X0.4400 Y1.3650 Z-0.3230 J3.1676 K0.3740 -G3 X0.4400 Y1.3550 Z-0.2583 J-0.4880 K-0.0423 -G3 X0.4400 Y1.3050 Z-0.1695 J-0.2026 K-0.0556 -G0 Z0.0120 - Y0.1350 -G1 Z-0.1704 -G2 Y0.130000 Z-0.175407 R0.005000 -G3 X0.4400 Y0.0850 Z-0.2583 J0.1388 K-0.1290 -G3 X0.4400 Y0.0750 Z-0.3230 J0.4780 K-0.1070 -G2 X0.4400 Y0.0600 Z-0.4858 J-3.1826 K0.2113 -G1 Y0.0550 Z-0.5000 - Y0.0450 -G0 Z0.0120 - X0.4000 Y-0.0750 -G1 Z-0.3750 -G3 Y0.050000 Z-0.500000 R0.125000 -G1 Y0.0750 - Y0.0800 Z-0.4740 - Y0.0950 Z-0.3132 - Y0.1000 Z-0.2779 -G2 X0.4000 Y0.1350 Z-0.1937 J0.1827 K-0.0265 -G0 Z0.0120 - Y1.3050 -G1 Z-0.1966 -G3 Y1.310000 Z-0.201563 R0.005000 -G2 X0.4000 Y1.3400 Z-0.2779 J-0.1531 K-0.1042 -G1 Y1.3450 Z-0.3132 - Y1.3600 Z-0.4740 - Y1.3650 Z-0.5000 - Y1.3950 -G0 Z0.0120 - X0.3600 Y1.4700 -G1 Z-0.3750 -G2 Y1.345000 Z-0.500000 R0.125000 -G1 Y1.3400 Z-0.4799 -G2 X0.3600 Y1.3250 Z-0.3309 J3.5018 K0.4277 -G3 X0.3600 Y1.3150 Z-0.2642 J-0.4617 K-0.0352 -G3 X0.3600 Y1.2600 Z-0.1692 J-0.1826 K-0.0422 -G0 Z0.0120 - Y0.1800 -G1 Z-0.1691 -G2 Y0.175000 Z-0.174112 R0.005000 -G3 X0.3600 Y0.1250 Z-0.2642 J0.1310 K-0.1316 -G3 X0.3600 Y0.1150 Z-0.3309 J0.4517 K-0.1018 -G2 X0.3600 Y0.1000 Z-0.4799 J-3.5168 K0.2787 -G1 Y0.0950 Z-0.5000 - Y0.0900 -G0 Z0.0120 - X0.3200 Y-0.0300 -G1 Z-0.3750 -G3 Y0.095000 Z-0.500000 R0.125000 -G1 Y0.1200 - Y0.1250 Z-0.4799 -G3 X0.3200 Y0.1400 Z-0.3328 J-2.7487 K0.3547 -G2 X0.3200 Y0.1500 Z-0.2681 J0.4880 K-0.0423 -G2 X0.3200 Y0.1800 Z-0.2006 J0.1697 K-0.0349 -G0 Z0.0120 - Y1.2600 -G1 Z-0.2034 -G3 Y1.265000 Z-0.208437 R0.005000 -G2 X0.3200 Y1.2950 Z-0.2936 J-0.1485 K-0.1002 -G2 X0.3200 Y1.3100 Z-0.4387 J-2.2137 K-0.3022 -G3 X0.3200 Y1.3200 Z-0.5000 J0.2527 K0.0098 -G1 Y1.3500 -G0 Z0.0120 - X0.2800 Y1.4250 -G1 Z-0.3750 -G2 Y1.300000 Z-0.500000 R0.125000 -G1 Y1.2900 - Y1.2850 Z-0.4740 - Y1.2650 Z-0.2986 -G3 X0.2800 Y1.2550 Z-0.2516 J-0.3430 K-0.0483 -G3 X0.2800 Y1.2150 Z-0.1859 J-0.1523 K-0.0477 -G0 Z0.0120 - Y0.2250 -G1 Z-0.1868 -G2 Y0.220000 Z-0.191759 R0.005000 -G3 X0.2800 Y0.1850 Z-0.2516 J0.1129 K-0.1062 -G3 X0.2800 Y0.1750 Z-0.2986 J0.3330 K-0.0953 -G1 Y0.1550 Z-0.4740 - Y0.1500 Z-0.5000 - Y0.1350 -G0 Z0.0120 - X0.2400 Y0.0600 -G1 Z-0.3678 -G3 Y0.185000 Z-0.492750 R0.125000 -G3 X0.2400 Y0.1950 Z-0.4280 J-0.6865 K0.1392 -G2 X0.2400 Y0.2100 Z-0.2967 J3.5145 K-0.3347 -G2 X0.2400 Y0.2200 Z-0.2516 J0.4074 K-0.0667 -G2 X0.2400 Y0.2400 Z-0.2124 J0.2090 K-0.0819 -G2 X0.2400 Y0.2700 Z-0.1786 J0.1849 K-0.1343 -G0 Z0.0120 - Y1.1700 -G1 Z-0.1795 -G3 Y1.175000 Z-0.184518 R0.005000 -G1 Y1.1800 Z-0.1884 - Y1.2000 Z-0.2124 -G2 X0.2400 Y1.2200 Z-0.2516 J-0.1505 K-0.1015 -G2 X0.2400 Y1.2300 Z-0.2967 J-0.3974 K-0.1118 -G1 Y1.2500 Z-0.4653 - Y1.2550 Z-0.4928 - Y1.2600 Z-0.5000 -G0 Z0.0120 - X0.2000 Y1.3800 -G1 Z-0.3750 -G2 Y1.255000 Z-0.500000 R0.125000 -G1 Y1.2200 - Y1.2150 Z-0.4928 - Y1.2100 Z-0.4653 - Y1.1900 Z-0.2986 -G3 X0.2000 Y1.1700 Z-0.2320 J-0.1995 K-0.0235 -G0 Z0.0120 - Y0.3151 -G1 Z-0.1794 -G2 Y0.310000 Z-0.184518 R0.005148 -G3 X0.2000 Y0.2550 Z-0.2731 J0.0935 K-0.1194 -G1 Y0.2500 Z-0.2986 - Y0.2300 Z-0.4653 - Y0.2250 Z-0.4928 -G0 Z0.0120 - X0.1600 Y0.1050 -G1 Z-0.3750 -G3 Y0.230000 Z-0.500000 R0.125000 -G1 Y0.2650 - Y0.2700 Z-0.4897 - Y0.2750 Z-0.4642 - Y0.2950 Z-0.3093 -G2 X0.1600 Y0.3150 Z-0.2418 J0.2180 K-0.0278 -G0 Z0.0120 - Y1.0799 -G1 Z-0.1886 -G3 Y1.085000 Z-0.193720 R0.005148 -G2 X0.1600 Y1.1350 Z-0.2662 J-0.0815 K-0.1097 -G2 X0.1600 Y1.1450 Z-0.3093 J-0.2562 K-0.0821 -G1 Y1.1650 Z-0.4642 - Y1.1700 Z-0.4897 -G0 Z0.0120 - X0.1200 Y1.2450 -G1 Z-0.3745 -G2 Y1.120000 Z-0.499516 R0.125000 -G2 X0.1200 Y1.1100 Z-0.4544 J0.3974 K0.1118 -G3 X0.1200 Y1.0900 Z-0.3191 J-5.3775 K-0.7258 -G3 X0.1200 Y1.0800 Z-0.2779 J-0.3132 K-0.0543 -G3 X0.1200 Y1.0350 Z-0.2094 J-0.1299 K-0.0363 -G0 Z0.0120 - Y0.4050 -G1 Z-0.2093 -G2 Y0.400000 Z-0.214319 R0.005001 -G3 X0.1200 Y0.3600 Z-0.2779 J0.0856 K-0.0982 -G3 X0.1200 Y0.3500 Z-0.3191 J0.3032 K-0.0954 -G1 Y0.3300 Z-0.4544 - Y0.3250 Z-0.4799 - Y0.3200 Z-0.4995 - Y0.3150 Z-0.5000 -G0 Z0.0120 - X0.0800 Y0.2400 -G1 Z-0.3750 -G3 Y0.365000 Z-0.500000 R0.125000 -G1 Y0.3850 - Y0.3900 Z-0.4917 -G3 X0.0800 Y0.4000 Z-0.4505 J-0.3032 K0.0954 -G2 X0.0800 Y0.4250 Z-0.3152 J2.3368 K-0.3618 -G2 X0.0800 Y0.4500 Z-0.2505 J0.1759 K-0.0308 -G0 Z0.0120 - Y0.9900 -G1 Z-0.2533 -G3 Y0.995000 Z-0.258340 R0.005000 -G2 X0.0800 Y1.0150 Z-0.3152 J-0.1628 K-0.0892 -G2 X0.0800 Y1.0400 Z-0.4505 J-2.0803 K-0.4544 -G3 X0.0800 Y1.0500 Z-0.4917 J0.3132 K0.0542 -G1 Y1.0550 Z-0.5000 - Y1.0800 -G0 Z0.0120 - X0.0400 Y1.1100 -G1 Z-0.3750 -G2 Y0.985000 Z-0.500000 R0.125000 -G1 Y0.9650 - Y0.9600 Z-0.4976 -G2 X0.0400 Y0.9450 Z-0.4564 J0.2697 K0.1216 -G3 X0.0400 Y0.9100 Z-0.3270 J-1.7704 K-0.4094 -G3 X0.0400 Y0.8850 Z-0.2760 J-0.2712 K-0.1014 -G3 X0.0400 Y0.8550 Z-0.2457 J-0.1055 K-0.0745 -G0 Z0.0120 - Y0.5851 -G1 Z-0.2445 -G2 Y0.580000 Z-0.249613 R0.005148 -G3 X0.0400 Y0.5550 Z-0.2760 J0.0608 K-0.0827 -G3 X0.0400 Y0.5300 Z-0.3270 J0.1400 K-0.1003 -G1 Y0.4950 Z-0.4564 -G2 X0.0400 Y0.4800 Z-0.4976 J-0.3224 K0.0941 -G1 Y0.4750 Z-0.5000 - Y0.4500 -G0 Z0.0120 -M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/line-arc-burnin.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/line-arc-burnin.ngc index eafe2f8a565..01dd5fff6ab 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/line-arc-burnin.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/line-arc-burnin.ngc @@ -1,5 +1,4 @@ -G90 G20 -G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-convex.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-convex.ngc index 8910b6202fc..460a43cd46c 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-convex.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-convex.ngc @@ -1,6 +1,4 @@ -G20 G17 G40 -G90 G91.1 -G64 (Q0.001) +o call F999.0000 G0 X0 Y0 Z0 G1 X-1 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-end-overrun.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-end-overrun.ngc index 59c09770daa..f03391768b8 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-end-overrun.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-end-overrun.ngc @@ -1,8 +1,5 @@ -G20 +o call G0 Z0.0120 -G17 G40 -G80 G90 G91.1 -G64 (Q0.001) F999.0000 G18 G0 X3.3390 Y4.8580 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-stellabee.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-stellabee.ngc index 3b1d03a084a..ca78e8acf5a 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-stellabee.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-stellabee.ngc @@ -1,8 +1,5 @@ -G20 +o call G0 Z0.0120 -G17 G40 -G80 G90 G91.1 -G64 (Q0.001) F999.0000 G18 G0 X3.3390 Y4.8580 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc.ngc index ee3a4a3d3f2..9845a9948a8 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc.ngc @@ -1,4 +1,4 @@ -G20 G90 +o call G64 P0.0 Q0.0 F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/tort-sam-mach3.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/tort-sam-mach3.ngc index c368be598fb..0e5a5b4214d 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/tort-sam-mach3.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/tort-sam-mach3.ngc @@ -1,9 +1,8 @@ -g21 -G64 P0 (Q0.004) +o call G0 X0 Y0 Z20 G0 X5.394910 Y13.495489 Z20.332291 G19 G2 F12700 (225 29) J5.656854 K5.656854 X10.394910 Y26.080547 Z29.989145 G19 G2 (75 14) J-0.258819 K-0.965926 X6.894910 Y26.787653 Z29.282038 G17 G2 (329 285) I-4.330127 J2.500000 X3.858878 Y24.458024 Z31.282038 G0 X0 Y0 Z20 -m30 +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/auto-test/XYtests b/tests/trajectory-planner/circular-arcs/nc_files/auto-test/XYtests deleted file mode 120000 index 47c072572fb..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/auto-test/XYtests +++ /dev/null @@ -1 +0,0 @@ -../XYtests \ No newline at end of file diff --git a/tests/trajectory-planner/circular-arcs/nc_files/auto-test/performance b/tests/trajectory-planner/circular-arcs/nc_files/auto-test/performance deleted file mode 120000 index 6b52c1e72d4..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/auto-test/performance +++ /dev/null @@ -1 +0,0 @@ -../performance/ \ No newline at end of file diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/abc_tangent.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/abc_tangent.ngc index f22259bfbdd..f6111b240ca 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/abc_tangent.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/abc_tangent.ngc @@ -1,4 +1,4 @@ -G20 G90 G17 +o call G0 Z1 G0 X0 Y0 A0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/adjustment.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/adjustment.ngc index 995988e53dc..810c56ca5ed 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/adjustment.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/adjustment.ngc @@ -1,5 +1,5 @@ -(Adjustment) -G20 G90 +(Test blend behavior due to alternating long and short moves) +o call G64 P0 Q0.005 F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/arc-with-spiral.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/arc-with-spiral.ngc index 021162ebb8f..4aacedf571d 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/arc-with-spiral.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/arc-with-spiral.ngc @@ -1,4 +1,4 @@ -G90 G20 G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_1.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_1.ngc index 913f3a558b1..fdbbeb73b21 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_1.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_1.ngc @@ -1,7 +1,4 @@ -G90 G20 G40 - -G64 P0 Q0 -/G61.1 +o call G0 Z1 G0 X0 Y0 Z0 @@ -19,4 +16,3 @@ G1 X1 Y-1 M2 - diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_comp.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_comp.ngc index ff7a636e189..f72de568fc1 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_comp.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_comp.ngc @@ -1,7 +1,4 @@ -G90 G20 G40 - -G64 P0 Q0 -/G61.1 +o call G10 L1 P1 Z1 R.5 T1 M6 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_lathe.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_lathe.ngc index c60f1d54329..e100da7a0af 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_lathe.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_lathe.ngc @@ -1,7 +1,5 @@ -G90 G20 G40 +o call G18 -G64 P0 Q0 -/G61.1 G0 X0 Z0 G1 Z0 F100 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/default_tolerance_check.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/default_tolerance_check.ngc index 24afba1a7cd..66bdebeb923 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/default_tolerance_check.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/default_tolerance_check.ngc @@ -19,7 +19,7 @@ X0.0 Y1 X0 Y0 o100 endsub -G20 G90 G17 +o call ("Default" blending with no G64 words) diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/feed-tracking-test.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/feed-tracking-test.ngc index d08b9674a65..08ed4475168 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/feed-tracking-test.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/feed-tracking-test.ngc @@ -1,4 +1,4 @@ -G90 G20 +o call G64 P0.005 Q0.0 @@ -10,4 +10,3 @@ G1 X5 F1000 G1 X6 F1000 G1 X7 F1000 M2 - diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/helix_slowdown.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/helix_slowdown.ngc index 6e53b4604a3..bfd5fa0c737 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/helix_slowdown.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/helix_slowdown.ngc @@ -1,4 +1,4 @@ -G90 G20 +o call G0 Z.01 G0 X0 Y0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/modal_state_test.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/modal_state_test.ngc index cf9ade9710f..2c863b638c4 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/modal_state_test.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/modal_state_test.ngc @@ -1,5 +1,4 @@ -G20 G90 - +o call F5 G0 Z1 G0 X0 Y0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/parabolic_split1.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/parabolic_split1.ngc index ba58533b729..593ca153d82 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/parabolic_split1.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/parabolic_split1.ngc @@ -1,5 +1,4 @@ -G90 G20 -G64 P0 Q0 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/ripple1.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/ripple1.ngc index 58aac4a4b80..9e085e3c65c 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/ripple1.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/ripple1.ngc @@ -1,5 +1,5 @@ -G20 G90 -G64 P0.0 Q0.0 +(Demonstrate low acceleration in last segment due to G64) +o call F1000 G0 X0 Y0 Z0 G1 X1 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/simple_blend.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/simple_blend.ngc index 92b431c2975..4016ba686e9 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/simple_blend.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/simple_blend.ngc @@ -1,4 +1,4 @@ -G90 G20 G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/spiral_aggressive.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/spiral_aggressive.ngc index dc2391bb444..003b28dd193 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/spiral_aggressive.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/spiral_aggressive.ngc @@ -1,7 +1,4 @@ -G90 G20 G40 - -G64 P0 Q0 -/G61.1 +o call G0 Z1 G0 X0 Y0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/tangent_check.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/tangent_check.ngc index b9d70e9dce2..98a1812c329 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/tangent_check.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/tangent_check.ngc @@ -1,4 +1,5 @@ -G61 G90 +o call +G61 G0 X-.1 Y0 Z0 F20 G1 X0 Y0 Z0 G64 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/test-active-states.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/test-active-states.ngc index 12e4ae53c49..fd99f8fcb2e 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/test-active-states.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/test-active-states.ngc @@ -1,4 +1,5 @@ -G90 G20 G17 G54 +o call + G0 Z1 X0 Y0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/find_dupes.sh b/tests/trajectory-planner/circular-arcs/nc_files/find_dupes.sh new file mode 100644 index 00000000000..bebfe60b0d7 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/nc_files/find_dupes.sh @@ -0,0 +1,9 @@ +#!/bin/sh + +# Finds duplicates based on this SO suggestion: https://stackoverflow.com/a/16278407 +dirname=${1:-'./'} +find $dirname -type f | sed 's_.*/__' | sort| uniq -d| +while read fileName +do +find $dirname -type f | grep "$fileName" +done diff --git a/tests/trajectory-planner/circular-arcs/nc_files/speed-tests/sam-tail-slowdown.ngc b/tests/trajectory-planner/circular-arcs/nc_files/performance/sam-tail-slowdown.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/speed-tests/sam-tail-slowdown.ngc rename to tests/trajectory-planner/circular-arcs/nc_files/performance/sam-tail-slowdown.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-concave-convex.ngc b/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-concave-convex.ngc deleted file mode 100644 index eca82217ad6..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-concave-convex.ngc +++ /dev/null @@ -1,12 +0,0 @@ -G90 G20 G64 -F1000 -G0 X0 Y0 -Z0 -G1 X.1 -G2 X.2 R.1 -G3 X.3 R.2 -G1 X.8 -G1 Y.2 -X0 -Y0 -M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/dodecagon_0.1in.ngc b/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/dodecagon_0.1in.ngc index 5e94a6e1612..44ecb67417d 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/dodecagon_0.1in.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/dodecagon_0.1in.ngc @@ -1,7 +1,6 @@ -G61 G90 G20 - G0 X-.1 Y0 Z0 F20 - G1 X0 Y0 Z0 -G64 +o call +G0 X-.1 Y0 Z0 F20 +G1 X0 Y0 Z0 G1 X0.086603 Y-0.050000 Z0.000000 F10.000000 G1 X0.136603 Y-0.136603 Z0.000000 F10.000000 G1 X0.136603 Y-0.236603 Z0.000000 F10.000000 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-stellabee.ngc b/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-stellabee.ngc deleted file mode 100644 index 3b1d03a084a..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-stellabee.ngc +++ /dev/null @@ -1,132 +0,0 @@ -G20 -G0 Z0.0120 -G17 G40 -G80 G90 G91.1 -G64 (Q0.001) -F999.0000 -G18 -G0 X3.3390 Y4.8580 -G1 Z-0.0319 -G3 X3.214000 Z-0.156892 R0.125000 -G1 X3.2100 Z-0.1530 - X3.2040 Z-0.1539 - X3.2000 Z-0.1569 - X3.1940 Z-0.1539 - X3.1840 Z-0.1588 - X3.1680 Z-0.1549 - X3.1620 Z-0.1565 - X3.1380 Z-0.1539 - X3.1280 Z-0.1589 - X3.1260 Z-0.1575 - X3.1240 Z-0.1520 - X3.1200 Z-0.1510 - X3.1160 Z-0.1549 - X3.1060 Z-0.1588 -G3 X3.0940 Y4.8580 Z-0.1540 I0.0013 K0.0205 -G1 X3.0860 Z-0.1556 - X3.0800 Z-0.1539 - X3.0760 Z-0.1569 - X3.0700 Z-0.1539 - X3.0660 Z-0.1549 - X3.0620 Z-0.1529 - X3.0560 Z-0.1539 - X3.0520 Z-0.1520 - X3.0440 Z-0.1520 - X3.0400 Z-0.1549 - X3.0360 Z-0.1556 - X3.0280 Z-0.1549 - X3.0240 Z-0.1529 - X3.0040 Z-0.1549 - X2.9900 Z-0.1490 - X2.9860 Z-0.1529 - X2.9800 Z-0.1510 - X2.9740 Z-0.1546 - X2.9380 Z-0.1510 - X2.9300 Z-0.1526 - X2.9220 Z-0.1520 - X2.9180 Z-0.1559 - X2.9100 Z-0.1556 - X2.9080 Z-0.1536 - X2.9040 Z-0.1530 - X2.9000 Z-0.1559 - X2.8940 Z-0.1529 - X2.8860 Z-0.1520 - X2.8780 Z-0.1546 - X2.8760 Z-0.1569 - X2.8640 -G2 X2.8540 Y4.8580 Z-0.1510 I-0.0110 K-0.0073 -G1 X2.8500 Z-0.1516 - X2.8480 Z-0.1546 - X2.8320 Z-0.1539 - X2.8220 Z-0.1556 - X2.8200 Z-0.1536 - X2.8160 Z-0.1530 - X2.8140 Z-0.1536 - X2.8100 Z-0.1598 - X2.8060 Z-0.1526 - X2.8040 Z-0.1520 - X2.8000 Z-0.1526 - X2.7980 Z-0.1556 - X2.7960 Z-0.1559 - X2.7920 Z-0.1529 - X2.7740 Z-0.1559 - X2.7700 Z-0.1529 - X2.7600 Z-0.1549 - X2.7560 Z-0.1490 - X2.7520 Z-0.1480 - X2.7500 Z-0.1490 - X2.7480 Z-0.1539 - X2.7420 Z-0.1559 - X2.7200 Z-0.1539 - X2.7040 Z-0.1575 - X2.6980 Z-0.1529 - X2.6920 Z-0.1549 - X2.6820 Z-0.1530 - X2.6740 Z-0.1559 - X2.6600 Z-0.1529 - X2.6560 Z-0.1579 - X2.6540 Z-0.1556 - X2.6480 Z-0.1546 - X2.6460 Z-0.1487 - X2.6440 Z-0.1481 - X2.6400 Z-0.1487 - X2.6360 Z-0.1540 - X2.6300 Z-0.1529 - X2.6220 Z-0.1559 - X2.6140 Z-0.1539 - X2.6120 Z-0.1497 - X2.6080 Z-0.1490 - X2.6040 Z-0.1530 - X2.6000 Z-0.1520 - X2.5960 Z-0.1559 - X2.5920 Z-0.1559 - X2.5900 Z-0.1500 - X2.5860 Z-0.1471 - X2.5820 Z-0.1481 - X2.5800 Z-0.1530 - X2.5760 Z-0.1539 - X2.5720 Z-0.1530 - X2.5700 Z-0.1471 - X2.5680 Z-0.1461 - X2.5640 Z-0.1471 - X2.5600 Z-0.1539 - X2.5540 Z-0.1540 - X2.5460 Z-0.1510 - X2.5420 Z-0.1546 - X2.5360 Z-0.1530 - X2.5320 Z-0.1549 - X2.5260 Z-0.1520 - X2.5140 Z-0.1529 - X2.5080 Z-0.1510 - X2.5040 Z-0.1526 - X2.4920 Z-0.1490 - X2.4760 Z-0.1510 - X2.4640 Z-0.1480 - X2.4580 Z-0.1510 - X2.4480 Z-0.1510 - X2.4420 Z-0.1536 - X2.4380 Z-0.1487 - X2.4340 Z-0.1480 - X2.4240 Z-0.1529 - X2.4180 Z-0.1490 -M2 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 index e454a777e2d..40847f81382 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_blend.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_blend.ngc @@ -1,5 +1,6 @@ (From TP Issue #68) -G20 G64 G8 G18 +G20 G8 G18 +G64 P0.01 Q0.0 M3 S400 G0 X2.0 Z0 G0 X1.1 Z0.1 @@ -8,8 +9,8 @@ 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 -/G64 P0.002 Q0.001 (Arc blend here during position sync) G33 K0.1 X0.800 Z-1.5 (Arc blend here during position sync) 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..48add847aaf 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,9 +1,8 @@ -G90 G20 G64 (absolute distance mode) +o call 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 M5 M2 (end program) diff --git a/tests/trajectory-planner/circular-arcs/nc_files/spindle/rigidtap_test.ngc b/tests/trajectory-planner/circular-arcs/nc_files/spindle/rigidtap_test.ngc index 62f3cd27615..53f9dbb3ad7 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/spindle/rigidtap_test.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/spindle/rigidtap_test.ngc @@ -1,4 +1,4 @@ -G20 G90 G40 +o call M3 S200 G0 X1.000 Y1.000 Z0.100 G33.1 Z-0.750 K0.05 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/spindle/spindle-wait.ngc b/tests/trajectory-planner/circular-arcs/nc_files/spindle/spindle-wait.ngc index 7991e4ac16c..e0fd5af447c 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/spindle/spindle-wait.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/spindle/spindle-wait.ngc @@ -1,6 +1,6 @@ +o call (Test of atspeed) (Setup code) -G20 G90 G64 G17 (Move to standard initial position) G0 x0 y0 z1 (Tangent segments allow speed above "triangle" velocity) diff --git a/tests/trajectory-planner/circular-arcs/nc_files/spindle/spiral_spindleon.ngc b/tests/trajectory-planner/circular-arcs/nc_files/spindle/spiral_spindleon.ngc index c2ccba8a909..f764b689d03 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/spindle/spiral_spindleon.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/spindle/spiral_spindleon.ngc @@ -1,5 +1,4 @@ -G20 -G64 +o call G0 X1 Y0 Z0 G0 X0 G1 X1.724638 Y-1.012731 F999 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/spindle/straight_spindleon.ngc b/tests/trajectory-planner/circular-arcs/nc_files/spindle/straight_spindleon.ngc index aa31044674a..7e39396622a 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/spindle/straight_spindleon.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/spindle/straight_spindleon.ngc @@ -1,6 +1,6 @@ (Simple straight line case to test "perfect" tangent case) -(Setup code) -G20 G90 G64 G17 +o call + (Test code begins here) O100 sub diff --git a/tests/trajectory-planner/circular-arcs/nc_files/spindle/tapping.ngc b/tests/trajectory-planner/circular-arcs/nc_files/spindle/tapping.ngc index ff0b511aae4..c7ec7344c2b 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/spindle/tapping.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/spindle/tapping.ngc @@ -1,4 +1,4 @@ -G20 G90 G40 +o call M3 S200 G0 X1.000 Y1.000 Z0.100 G33.1 Z-0.750 K0.05 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_inch.ngc b/tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_inch.ngc new file mode 100644 index 00000000000..245a4940714 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_inch.ngc @@ -0,0 +1,4 @@ +o sub +o call +G20 +o endsub diff --git a/tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_mm.ngc b/tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_mm.ngc new file mode 100644 index 00000000000..740977777ae --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_mm.ngc @@ -0,0 +1,4 @@ +o sub +o call +G21 +o endsub diff --git a/tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_modal_state.ngc b/tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_modal_state.ngc new file mode 100644 index 00000000000..ff5f147f21f --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_modal_state.ngc @@ -0,0 +1,19 @@ +o sub +G10 L2 P0 R0 (Clear any coordinate system rotation) +G17 +G20 +G40 (clear cutter compensation) +G49 (cancel tool length compensation) +G54 (Start in G54 work offset) +G64 P0 Q0 (Blend mode with no tolerance applied) +G90 (Absolute positioning) +G92.1 (Reset G92 offsets) +G94 (Feed-per-minute mode) +G97 (RPM spindle mode) +G98 (Return to the initial value) + +M5 (Turn off spindle) +M9 (Turn off coolant) +S0 +F0 +o endsub diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/spreadsheeterrors.ngc b/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/spreadsheeterrors.ngc deleted file mode 100644 index ec8ea42362d..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/spreadsheeterrors.ngc +++ /dev/null @@ -1,66 +0,0 @@ -g21 - -#8=0 -#9=0 -#10=0 -#11=0 -# = 5 -o140 do -(DEBUG, Deg [#8]) -g10l2p1x#9y#10z#11r#8f100000 -(# = .7) -g64p.01q.01 - -G1 X[#*41.394] Y[#*88.961] -G2 X[#*42.768] Y[#*88.508] I[#*-5.181] J[#*-18.046] -X[#*44.707] Y[#*87.733] I[#*-10.076] J[#*-27.999] -X[#*49.036] Y[#*85.098] I[#*-9.45] J[#*-20.404] -X[#*51.404] Y[#*82.809] I[#*-10.941] J[#*-13.681] -X[#*53.138] Y[#*80.405] I[#*-14.853] J[#*-12.548] -X[#*54.031] Y[#*78.718] I[#*-13.122] J[#*-8.02] -X[#*55.009] Y[#*75.787] I[#*-14.431] J[#*-6.445] -X[#*55.332] Y[#*73.785] I[#*-14.376] J[#*-3.348] -X[#*55.384] Y[#*71.711] I[#*-18.936] J[#*-1.513] -X[#*54.714] Y[#*67.79] I[#*-16.786] J[#*0.852] -X[#*53.315] Y[#*64.487] I[#*-14.195] J[#*4.067] -X[#*50.7] Y[#*61.041] I[#*-13.786] J[#*7.745] -X[#*48.588] Y[#*59.2] I[#*-15.748] J[#*15.936] -X[#*46.275] Y[#*57.738] I[#*-11.693] J[#*15.933] -X[#*45.966] Y[#*57.603] I[#*-0.656] J[#*1.083] -G3 X[#*45.882] Y[#*57.673] I[#*-0.131] J[#*-0.074] -X[#*45.836] Y[#*57.491] I[#*-0.023] J[#*-0.091] -X[#*45.959] Y[#*57.554] I[#*0.02] J[#*0.111] -G1 X[#*45.967] Y[#*57.564] -G2 X[#*46.417] Y[#*57.372] I[#*-0.597] J[#*-2.031] -X[#*48.505] Y[#*56.011] I[#*-5.52] J[#*-10.749] -X[#*50.414] Y[#*54.369] I[#*-22.264] J[#*-27.826] -X[#*51.883] Y[#*52.934] I[#*-37.762] J[#*-40.113] -X[#*54.037] Y[#*50.531] I[#*-34.816] J[#*-33.364] -X[#*57.418] Y[#*46.006] I[#*-41.891] J[#*-34.84] -X[#*58.904] Y[#*43.164] I[#*-15.621] J[#*-9.974] -X[#*61.158] Y[#*35.798] I[#*-41.283] J[#*-16.66] -X[#*61.822] Y[#*31.33] I[#*-35.828] J[#*-7.606] -X[#*61.951] Y[#*28.768] I[#*-33.99] J[#*-2.994] -X[#*61.941] Y[#*26.703] I[#*-41.425] J[#*-0.847] -X[#*61.583] Y[#*22.416] I[#*-41.847] J[#*1.342] -X[#*60.815] Y[#*18.286] I[#*-40.791] J[#*5.443] -X[#*59.747] Y[#*14.756] I[#*-33.542] J[#*8.223] -X[#*58.73] Y[#*12.322] I[#*-29.525] J[#*10.903] -X[#*57.307] Y[#*9.681] I[#*-25.104] J[#*11.828] -X[#*56.009] Y[#*7.837] I[#*-12.665] J[#*7.534] -X[#*54.075] Y[#*5.821] I[#*-11.615] J[#*9.211] -X[#*52.734] Y[#*4.711] I[#*-14.655] J[#*16.335] -G1 X[#*51.442] Y[#*3.758] -G2 X[#*48.906] Y[#*2.121] I[#*-28.308] J[#*41.068] -G1 X[#*47.145] Y[#*1.135] - -G0 Z[#*5.0] -G0 Z[#*30.0] -#8=[#8+20.2] -#9=[#9+5] -#10=[#10+5] -#11=[#11+20] -#=[#*.9] -o140 while [#8 LT 22] -g10l2p1r0 -m30 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/130207L.ngc b/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/130207L.ngc index e151d129431..3da76282f89 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/130207L.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/130207L.ngc @@ -1,7 +1,5 @@ -G92.1 G54 G49 -M111 +o call G53 G0 Z0 W0 -G90 G64 P0.005 Q0.005 F80 M3 S18000 @@ -670,7 +668,6 @@ G1 Z0. G1 X9.6003 Y2.0173 Z-0.015 G1 X9.6014 Y2.0385 Z0. G1 X9.6003 Y2.0173 Z-0.015 -G1 X9.7859 Y1.8111 G1 X9.8071 Y1.81 Z0. G1 X9.7859 Y1.8111 Z-0.015 G1 X9.7848 Y1.7898 Z0. @@ -3937,5 +3934,4 @@ G53 G0 Z0 W0 G53 G0 X0 Y65 G49 G92.1 M5 -M30 -% +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/130207LZW.ngc b/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/130207LZW.ngc index 99ef5f8452e..6f8c0c75cff 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/130207LZW.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/130207LZW.ngc @@ -1,7 +1,5 @@ -G20 -G92.1 G54 G49 +o call G53 G0 Z0 W0 -G90 G64 P0.005 F80 M3 S18000 @@ -3857,5 +3855,4 @@ G0 Z0.25 W0.25 G53 G0 Z0 W0 G49 G92.1 M5 -M30 -% +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/xw.ngc b/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/xw.ngc index 78d17268534..4da66c4f5f1 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/xw.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/xw.ngc @@ -1,6 +1,6 @@ -G20 g64 -g0x0y0w1 -g1w-.1f3000 -x1 -w1 -m30 +o call +G0 X0 Y0 W1 +G1 W-.1 F3000 +X1 +W1 +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/xz.ngc b/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/xz.ngc index ba9fe55251f..5270694059c 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/xz.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/xz.ngc @@ -1,6 +1,6 @@ -g64 -g0x0y0z1 -g1z-.1f3000 -x1 -z1 -m30 +o call +G0 X0 Y0 Z1 +G1 Z-.1 F3000 +X1 +Z1 +M30 diff --git a/tests/trajectory-planner/circular-arcs/octave/create_3d_line.m b/tests/trajectory-planner/circular-arcs/octave/create_3d_line.m deleted file mode 100644 index 15c4c476d28..00000000000 --- a/tests/trajectory-planner/circular-arcs/octave/create_3d_line.m +++ /dev/null @@ -1,62 +0,0 @@ -function move=create_3d_line(v_range,dp,theta,prev_move,limits) - %% Create a random trajectory segment based on the input - - if ~isstruct(prev_move) - %wlog assume +X as initial direction and origin as start - prev_move.P1=[-1,0,0]; - prev_move.P2=[0,0,0]; - end - - move.P1=prev_move.P2; - - %Hard-coded settings for G-code dump, make this more general later if need be - if length(v_range)==2 - move.v_req=ezrand(v_range(1),v_range(2)); - else - move.v_req=v_range; - end - - %dummy value - move.a_max=1; - - move.tolerance=10; - - dp_old=prev_move.P2-prev_move.P1; - u_old=dp_old/norm(dp_old); - - if length(theta)==1 - theta_vec=[-theta,theta,-theta,theta,-theta,theta]; - elseif length(theta)==2 - theta_vec=[theta,theta,theta]; - theta_vec=theta_vec(:); - else - theta_vec=theta; - end - - inlimits = false; - while ~inlimits - %Rotate about Z - theta_z=rand(1)*(theta_vec(2)-theta_vec(1)) + theta_vec(1); - u_raw=u_old*[cos(theta_z),-sin(theta_z),0;sin(theta_z),cos(theta_z),0;0,0,1]; - - %rotate about Y - theta_y=rand(1)*(theta_vec(4)-theta_vec(3)) + theta_vec(3); - u_raw=u_raw*[cos(theta_y),0,-sin(theta_y);0,1,0;sin(theta_y),0,cos(theta_y)]; - - theta_x=rand(1)*(theta_vec(6)-theta_vec(5)) + theta_vec(5); - u_raw=u_raw*[1, 0, 0;0, cos(theta_x),sin(theta_x);0,-sin(theta_x),cos(theta_x)]; - - %Hack to make a minimum length move - if length(dp) == 1 - move.P2=move.P1+u_raw*dp; - else - move.P2=move.P1+u_raw*(rand(1)*(dp(2)-dp(1))+dp(1)); - end - - if move.P2(1)limits(1) && move.P2(2)limits(3) && move.P2(3)limits(5) - inlimits = true; - end - end - -end - diff --git a/tests/trajectory-planner/circular-arcs/octave/create_3d_random_walk.m b/tests/trajectory-planner/circular-arcs/octave/create_3d_random_walk.m index 725699892a6..c3c9328fd9e 100644 --- a/tests/trajectory-planner/circular-arcs/octave/create_3d_random_walk.m +++ b/tests/trajectory-planner/circular-arcs/octave/create_3d_random_walk.m @@ -1,16 +1,40 @@ -function create_3d_random_walk(num_moves, v_range, dp, theta, prefix,limits) - %% Create a random path for G code testing - % This version of the code produces random lengths, and random 3d bends. Desired velocity is held at - if ~exist('limits','var') +%% Create a random path for G code testing. +% Produces random lengths, and random 3d bends, constrained by the provided settings. Writes a unique file name to the nc_files directory +% +% num_moves: number of moves in the generated file +% v_range: interval [min, max] of desired feeds (each move gets a randomly selected feed in this range) +% dp: length of moves (can be a min / max interval instead of a single value) +% limits: specify a bounding box for the program in a 6-element array [-X,+X,-Y,+Y,-Z,+Z] (default +/- 10.0 in XY, 5.0 in Z) +% tol: blend tolerance in user units (default 0.005) +% ncd_tol: naivecam tolerance (default 0) +function create_3d_random_walk(num_moves, v_range, dp, theta, limits, tol, ncd_tol, file_path, file_prefix) + if ~exist('file_path','var') + file_path = '../nc_files/generated' + end + mkdir(file_path); + if ~exist('file_prefix','var') + file_prefix = 'random_walk_linear' + end + + if ~exist('limits','var') || min(size(limits)) < 1 limits=[-10,10,-10,10,-5,5] end - + if ~exist('tol','var') + tol = 0.005 + end + if ~exist('ncd_tol','var') + ncd_tol = 0.0 + end tag = datestr(now(),"YYYY-MM-DD_HH-MM"); fsuf=sprintf('%dsteps_%gin-%gin_%gips_%gdeg_max_angle_%s.ngc',num_moves,dp(1),dp(end),v_range(end),max(theta)*180/pi,tag); - fid=fopen(sprintf('../nc_files/cv_random_walk_%s_%s',prefix,fsuf),'w'); + fid=fopen(sprintf('%s/%s_%s', file_path, file_prefix, fsuf),'w'); + if fid < 1 + error('Got invalid file handle') + end + %Initial state - header= 'G61 G90\n G0 X-.1 Y0 Z0 F20\n G1 X0 Y0 Z0\nG64\n'; + header= sprintf('G61 G90\n G0 X-.1 Y0 Z0\n G1 X0 Y0 Z0 F20\nG64 P%f Q%f\n', tol, ncd_tol); fprintf(fid, header); move=create_3d_line(v_range, dp, theta,[],limits); for k=1:num_moves @@ -22,3 +46,70 @@ function create_3d_random_walk(num_moves, v_range, dp, theta, prefix,limits) fclose(fid); end + +function move=create_3d_line(v_range,dp,theta,prev_move,limits) + %% Create a random trajectory segment based on the input + + if ~isstruct(prev_move) + %wlog assume +X as initial direction and origin as start + prev_move.P1=[-1,0,0]; + prev_move.P2=[0,0,0]; + end + + move.P1=prev_move.P2; + + %Hard-coded settings for G-code dump, make this more general later if need be + if length(v_range)==2 + move.v_req=ezrand(v_range(1),v_range(2)); + else + move.v_req=v_range; + end + + %dummy value + move.a_max=1; + + move.tolerance=10; + + dp_old=prev_move.P2-prev_move.P1; + u_old=dp_old/norm(dp_old); + + if length(theta)==1 + theta_vec=[-theta,theta,-theta,theta,-theta,theta]; + elseif length(theta)==2 + theta_vec=[theta,theta,theta]; + theta_vec=theta_vec(:); + else + theta_vec=theta; + end + + inlimits = false; + iter = 0; + while ~inlimits + %Rotate about Z + theta_z=rand(1)*(theta_vec(2)-theta_vec(1)) + theta_vec(1); + u_raw=u_old*[cos(theta_z),-sin(theta_z),0;sin(theta_z),cos(theta_z),0;0,0,1]; + + %rotate about Y + theta_y=rand(1)*(theta_vec(4)-theta_vec(3)) + theta_vec(3); + u_raw=u_raw*[cos(theta_y),0,-sin(theta_y);0,1,0;sin(theta_y),0,cos(theta_y)]; + + theta_x=rand(1)*(theta_vec(6)-theta_vec(5)) + theta_vec(5); + u_raw=u_raw*[1, 0, 0;0, cos(theta_x),sin(theta_x);0,-sin(theta_x),cos(theta_x)]; + + %Hack to make a minimum length move + if length(dp) == 1 + move.P2=move.P1+u_raw*dp; + else + move.P2=move.P1+u_raw*(rand(1)*(dp(2)-dp(1))+dp(1)); + end + + if move.P2(1)limits(1) && move.P2(2)limits(3) && move.P2(3)limits(5) + inlimits = true; + end + iter = iter + 1; + if iter > 100 + error('Exceeded max iterations trying to fit random move within limits, try a smaller length'); + end + end +end + diff --git a/tests/trajectory-planner/circular-arcs/octave/estimate_depth.m b/tests/trajectory-planner/circular-arcs/octave/estimate_depth.m index 48161a5a04d..7b67d17b032 100644 --- a/tests/trajectory-planner/circular-arcs/octave/estimate_depth.m +++ b/tests/trajectory-planner/circular-arcs/octave/estimate_depth.m @@ -1,5 +1,8 @@ +%% -- estimate_depth(a_limit, v_limit, L_min, dt) +% +% For the given a_limit (max acceleration), v_limit (max velocity), estimate the queue depth +% to reach peak velocity, for a series of worst-case moves of length L_min function depth = estimate_depth(a_limit,v_limit,L_min,dt) - v_sample= L_min/(2.0*dt); v_max = min(v_sample,v_limit); v = 0; @@ -8,5 +11,4 @@ v = sqrt(v^2 + 2*a_limit*L_min); depth+=1; end - end diff --git a/tests/trajectory-planner/circular-arcs/octave/ezrand.m b/tests/trajectory-planner/circular-arcs/octave/ezrand.m new file mode 100644 index 00000000000..6492a78688c --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/octave/ezrand.m @@ -0,0 +1,4 @@ +function out = ezrand(rmin,rmax) + out = rand(size(rmin))*(rmax-rmin)+rmin; +end + diff --git a/tests/trajectory-planner/circular-arcs/octave/make_peak_img.m b/tests/trajectory-planner/circular-arcs/octave/make_peak_img.m deleted file mode 100644 index 58c7d4d3480..00000000000 --- a/tests/trajectory-planner/circular-arcs/octave/make_peak_img.m +++ /dev/null @@ -1,29 +0,0 @@ -## Copyright (C) 2014 Robert W. Ellenberg -## -## This program is free software; you can redistribute it and/or modify -## it under the terms of the GNU General Public License as published by -## the Free Software Foundation; either version 3 of the License, or -## (at your option) any later version. -## -## 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 Octave; see the file COPYING. If not, see -## . - -## make_peak_img - -## Author: Robert W. Ellenberg rob@carbidelabs.com -## Created: 2014-02-18 - -peaks(200) -colormap(gray(1024)) -shading interp -view(0,90) -grid off -print 'peaks.png' '-S - - diff --git a/tests/trajectory-planner/circular-arcs/octave/optimal_circular_arc_accel.m b/tests/trajectory-planner/circular-arcs/octave/optimal_circular_arc_accel.m new file mode 100644 index 00000000000..284d1d22542 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/octave/optimal_circular_arc_accel.m @@ -0,0 +1,34 @@ +% Find optimal maximum velocity and acceleration ratio for circular motions. +% This shows the transition from velocity-limited to acceleration-limited motions +% Circles with large radii relative to their max velocity can use more of their +% acceleration budget for tangential acceleration, since normal accelerations are lower. +% +% NOTE: this calculation currently does not account for helical motion (which increases the effective radius) +a_max = 10.0 +v_max = 1.0 + +a_t_max_nominal = 0.5 * a_max; +a_n_max_nominal = sqrt(1.0 - a_t_max_nominal^2); + +r_eff = logspace(-2,.1,1000); + +v_max_cutoff = sqrt(a_max*sqrt(3)/2*r_eff); +v_max_req = 0*r_eff + v_max; + +figure(1) +plot(r_eff,v_max_cutoff,r_eff, v_max_req) +grid on +title('Circle maximum velocity vs radius') +xlabel('radius, units') +ylabel('velocity, units / sec') + +a_n_max_cutoff = v_max^2 ./ r_eff; +a_t_max_cutoff = sqrt(a_max^2 - a_n_max_cutoff.^2); + +figure(2) +plot(r_eff, a_t_max_cutoff / a_max,r_eff, r_eff*0+a_t_max_nominal/ a_max) +grid on +title('Circle maximum tangential acceleration vs radius') +xlabel('radius, units') +ylabel('velocity, units / sec') + diff --git a/tests/trajectory-planner/circular-arcs/octave/parallel_test.m b/tests/trajectory-planner/circular-arcs/octave/parallel_test.m new file mode 100644 index 00000000000..05c78ce3943 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/octave/parallel_test.m @@ -0,0 +1,10 @@ +% Demonstrate dot product of unit vectors in various directions as a sanity check +theta=0:pi/36:pi; +u0=[1;0;0]; +u1=[cos(theta);sin(theta);0*theta]; + +udot = u0'*u1; +plot(udot) +title(sprintf('Dot product of unit vectors as angle varies from %0.3f to %0.3f', theta(1), theta(end))) +xlabel('Angle, rad') +grid on diff --git a/tests/trajectory-planner/circular-arcs/octave/plot_displacement.m b/tests/trajectory-planner/circular-arcs/octave/plot_displacement.m new file mode 100644 index 00000000000..b0aa558e7ba --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/octave/plot_displacement.m @@ -0,0 +1,10 @@ +d=csvread('displacement_data.txt'); + +t = d(:,1); +xyz=d(:,2:4); +figure(1) +plot(t, xyz,'Linewidth',2); + +grid on +legend('X','Y','Z') + diff --git a/tests/trajectory-planner/circular-arcs/octave/plot_movement.m b/tests/trajectory-planner/circular-arcs/octave/plot_movement.m deleted file mode 100755 index 435f5d2ccee..00000000000 --- a/tests/trajectory-planner/circular-arcs/octave/plot_movement.m +++ /dev/null @@ -1,10 +0,0 @@ -#! /usr/bin/octave --persist -load movement.log -%Assume 1kHz -t=movement(:,1); -dt=diff(movement(:,1)); -v_nominal=movement(:,3); -dx = [0;movement(2:end,2)./dt]; -dv = [0;diff(dx)./dt]; -da = [0;diff(dv)./dt]; -plot(t,dv) diff --git a/tests/trajectory-planner/circular-arcs/octave/plot_spindledata.m b/tests/trajectory-planner/circular-arcs/octave/plot_spindledata.m new file mode 100644 index 00000000000..7c89377f6dd --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/octave/plot_spindledata.m @@ -0,0 +1,15 @@ +d=csvread('spindle_data.txt'); + +d(:,5)=d(:,5)*1000; +% Spindle data +figure(1) +plot(d(:,1:5),'Linewidth',2); + +grid on +legend('spindle displacement','raw pos', 'pos desired', 'pos progress', 'pos_error') + +% Position error +figure(2) +plot(d(:,5),'Linewidth',2) +grid on +legend('pos error') \ No newline at end of file diff --git a/tests/trajectory-planner/circular-arcs/octave/plot_tcdata.m b/tests/trajectory-planner/circular-arcs/octave/plot_tcdata.m new file mode 100644 index 00000000000..447423f5d53 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/octave/plot_tcdata.m @@ -0,0 +1,7 @@ +d=csvread('tc_state_data.txt'); + +figure(1) +plot(d(:,[1:5,8]),'Linewidth',2); + +grid on +legend('vr', 'vf','vmax','v_current','acc') diff --git a/tests/trajectory-planner/circular-arcs/octave/process_log.m b/tests/trajectory-planner/circular-arcs/octave/process_log.m deleted file mode 100644 index cb6fff5e220..00000000000 --- a/tests/trajectory-planner/circular-arcs/octave/process_log.m +++ /dev/null @@ -1,7 +0,0 @@ -load constraints.log -plot(constraints(:,1:6)); -legend('Xacc','Yacc','Zacc','Xvel','Yvel','Zvel','Xpos','Ypos','Zpos'); - -indX=find(abs(constraints(:,1))>30); -indY=find(abs(constraints(:,2))>30); -indZ=find(abs(constraints(:,3))>30); diff --git a/tests/trajectory-planner/circular-arcs/octave/test_kink_accel.m b/tests/trajectory-planner/circular-arcs/octave/test_kink_accel.m new file mode 100644 index 00000000000..f4d5ba1daab --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/octave/test_kink_accel.m @@ -0,0 +1,29 @@ +% Test behavior of acceleration as TP hits a sharp corner. +% this demonstrates the momentary acceleration spike encountered due to a not-quite-tangent intersection +% The sharp corner, or "kink" requires a potentially large reduction in velocity to avoid violating overall limits. + +a_bound=[30;30;30]; +dt = 0.001; +v_max = 1.05; + +u0=[0.900637; -0.434573; 0.000000] +u1=[-0.766314; 0.642466; 0.000000] + +a_kink = v_max / dt * (u1-u0) + +a_kink_violation = a_kink ./ a_bound; + +accel_loss_ratio = max(abs(a_kink_violation(:))) + +% How much segment acceleration to budget for the end point +allowed_acceleration_loss = 0.1; + +if (accel_violation_factor > allowed_acceleration_loss) + disp('Too much acceleration loss') + kink_vel = v_max * allowed_acceleration_loss / accel_loss_ratio +else + kink_vel = v_max * allowed_acceleration_loss / accel_loss_ratio +end + + + diff --git a/tests/trajectory-planner/circular-arcs/octave/test_split_cycle.m b/tests/trajectory-planner/circular-arcs/octave/test_split_cycle.m new file mode 100644 index 00000000000..8f1d86ab89a --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/octave/test_split_cycle.m @@ -0,0 +1,63 @@ +%% Estimate time remaining in a motion segment given some assumptions. +% From time 0 to dt_1, the TP tries to reach the final velocity, accelerating at max acceleration to do so. +% From time dt_1 onwards, the TP accelerates at max acceleration, then +% decelerates back to final velocity, such that it reaches the end position +% just when it also reaches the final velocity. This handles the case where the +% TP reaches vf with some distance to spare in region 1. +% TODO: handle case where final velocity is lower than current velocity by accelerating to peak first +vc = 1.0; +vf = 0.5; +a_sign = sign(vf-vc); +dx = logspace(-3,-1,200); + + +a_max = 10.0; + +t_step = 0.001; + +dt_cutoff = abs(vf-vc) / a_max; + +dx_1 = abs(vc.^2 - vf.^2) ./ (2.0 * a_max); + +dx_2 = dx-dx_1; + +for k = 1:length(dx_2) + if dx_2(k) > 0 + % Case where we can accelerate to meet vf and still have distance left. + % Assumes that the TP accelerates to peak velocity, then back down to final velocity + A=a_max/4; + B=vf; + C=-dx_2(k); + + disc = sqrt(B^2-4*A*C); + dt_2_a=(-B+disc)/(2*A); + dt_2_b=(-B-disc)/(2*A); + dt_2(k) = max(dt_2_a, dt_2_b); + + dt_1(k) = dt_cutoff; + else + dt_2(k) = 0; + A=a_sign*a_max/2; + B=vc; + C=-dx(k); + + disc = sqrt(B^2-4*A*C); + + dt_1_a(k)=(-B+disc)/(2*A); + dt_1_b(k)=(-B-disc)/(2*A); + if dt_1_a(k) < 0 + dt_1(k) = dt_1_b(k); + elseif dt_1_b < 0 + dt_1(k) = dt_1_a(k); + else + dt_1(k) = min(dt_1_a(k), dt_1_b(k)); + end + end +end + +dt_2flat = max(dx_2,0) ./ vf; +plot(dx,dt_1,dx,dt_2,dx,dt_1+dt_2,'--') +grid on +legend('dt velocity match','dt position match','total time','no reaccel') +xlabel('net distance') +ylabel('time, seconds') diff --git a/tests/trajectory-planner/circular-arcs/parse_tp_logs.py b/tests/trajectory-planner/circular-arcs/parse_tp_logs.py new file mode 100755 index 00000000000..d49158b7d00 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/parse_tp_logs.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 +import pyjson5 + +import sys +import re +from matplotlib import pyplot as plt +import numpy as np + +axes={ + 'X': 0, + 'Y': 1, + 'Z': 2, + 'A': 3, + 'B': 4, + 'C': 5, + 'U': 6, + 'V': 7, + 'W': 8, +} + +if __name__ == '__main__': + if len(sys.argv) < 2: + print(sys.stderr, 'No logfile specified') + exit(1) + + logs = [] + with open(sys.argv[1], 'r') as log_file: + for l in log_file.readlines(): + # Try to filter out obviously non-JSON lines + if re.search('{', l) is None or re.search('}', l) is None: + continue + try: + logs.append(pyjson5.decode(l)) + except Exception as e: + print('Ignoring exception ', e) + + axis_pos = np.array([e.get('axis_pos', None) for e in logs if e.get('log_entry', None) == 'tpRunCycle']) + axis_vel = np.array([e.get('axis_vel', None) for e in logs if e.get('log_entry', None) == 'tpRunCycle']) + axis_accel = np.array([e.get('axis_accel', None) for e in logs if e.get('log_entry', None) == 'tpRunCycle']) + times = np.array([e.get('time', None) for e in logs if e.get('log_entry', None) == 'tpRunCycle']) + + np.savetxt('axis_pos.csv', axis_pos, delimiter=',') + np.savetxt('axis_vel.csv', axis_vel, delimiter=',') + np.savetxt('axis_accel.csv', axis_accel, delimiter=',') + + plt_axis = 'X' + idx = axes[plt_axis] + + plt.figure(1) + plt.plot(times * 1000.0, axis_pos[:, idx]) + plt.grid(True) + plt.title('{} Axis position vs time, ms'.format(plt_axis)) + + plt.figure(2) + plt.plot(times * 1000.0, axis_vel[:, idx]) + plt.grid(True) + plt.title('{} velocity vs time, ms'.format(plt_axis)) + + plt.figure(3) + plt.plot(times * 1000.0, axis_accel[:, idx]) + plt.grid(True) + plt.title('{} Axis acceleration vs time, ms'.format(plt_axis)) + + plt.figure(4) + plt.plot(times * 1000.0, np.sqrt(np.sum(np.square(axis_vel[:, 0:2]), 1))) + plt.grid(True) + plt.title('XYZ velocity vs time, ms') + + plt.show() + + + diff --git a/tests/trajectory-planner/circular-arcs/run_all_tests.py b/tests/trajectory-planner/circular-arcs/run_all_tests.py deleted file mode 100644 index 3757fd9999e..00000000000 --- a/tests/trajectory-planner/circular-arcs/run_all_tests.py +++ /dev/null @@ -1,115 +0,0 @@ -#!/usr/bin/env python -'''Copied from m61-test''' - -import linuxcnc -from linuxcnc_control import LinuxcncControl -import hal -import sys - -from time import sleep - -from os import listdir -from os.path import isfile, join -import re -import Tkinter, os - -def query_yes_no(question, default="yes"): - """Ask a yes/no question via raw_input() and return their answer. - - "question" is a string that is presented to the user. - "default" is the presumed answer if the user just hits . - It must be "yes" (the default), "no" or None (meaning - an answer is required of the user). - - The "answer" return value is one of "yes" or "no". - """ - valid = {"yes":True, "y":True, "ye":True, - "no":False, "n":False} - if default == None: - prompt = " [y/n] " - elif default == "yes": - prompt = " [Y/n] " - elif default == "no": - prompt = " [y/N] " - else: - raise ValueError("invalid default answer: '%s'" % default) - - while True: - sys.stdout.write(question + prompt) - choice = raw_input().lower() - if default is not None and choice == '': - return valid[default] - elif choice in valid: - return valid[choice] - else: - sys.stdout.write("Please respond with 'yes' or 'no' "\ - "(or 'y' or 'n').\n") - -def find_test_nc_files(testpath='nc_files', show=False): - files = [ testpath + '/' + f for f in listdir(testpath) if isfile(join(testpath,f)) and re.search('.ngc$',f) is not None ] - if show: - print "In folder {0}, found {1} files:".format(testpath,len(files)) - for f in files: - print f - return files - -def axis_open_program(t,f): - return t.tk.call("send", "axis", ("remote","open_file_name", os.path.abspath(f))) - -t = Tkinter.Tk() -t.wm_withdraw() - -"""Run the test""" - -raw_input("Press any key when the LinuxCNC GUI has loaded") - -import glob -#KLUDGE this lists all subfolders in the auto-test directory -# this dir should be populated with symlinks to any folders of test files to -# run -test_folders=glob.glob('nc_files/auto-test/*') - -test_files = [] - - -for f in test_folders: - run_quick = query_yes_no("Run test cases in %s?" % f ) - if run_quick: - test_files.extend(find_test_nc_files(f,True)) - - -h = hal.component("python-ui") -h.ready() # mark the component as 'ready' - -#Don't need this with axis? -#os.system("halcmd source ./postgui.hal") - -# -# connect to LinuxCNC -# - -e = LinuxcncControl(1) -e.g_raise_except = False -e.set_mode(linuxcnc.MODE_MANUAL) -e.set_state(linuxcnc.STATE_ESTOP_RESET) -e.set_state(linuxcnc.STATE_ON) -e.do_home(-1) -sleep(1) -e.set_mode(linuxcnc.MODE_AUTO) -sleep(1) -for f in test_files: - if re.search('\.ngc$',f) is not None: - print "Loading program {0}".format(f) - e.set_mode(linuxcnc.MODE_AUTO) - sleep(1) - axis_open_program(t,f) - sleep(1) - e.set_mode(linuxcnc.MODE_AUTO) - while e.run_full_program(): - sleep(.5) - res = e.wait_on_program() - if res == False: - print "Program {0} failed to complete!".format(f) - sys.exit(1) - -print "All tests completed!" diff --git a/tests/trajectory-planner/circular-arcs/tp_testing_venv.cfg b/tests/trajectory-planner/circular-arcs/tp_testing_venv.cfg new file mode 100644 index 00000000000..9ac5d225004 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/tp_testing_venv.cfg @@ -0,0 +1,4 @@ +matplotlib==3.0.2 +# Only do this if the system package is old, takes forever +#numpy==1.16.0 +pyjson5==0.4.5 diff --git a/tests/trajectory-planner/common-config/apply_limits_to_enable.tcl b/tests/trajectory-planner/common-config/apply_limits_to_enable.tcl new file mode 100644 index 00000000000..5409fbf48ad --- /dev/null +++ b/tests/trajectory-planner/common-config/apply_limits_to_enable.tcl @@ -0,0 +1 @@ +net constraints-ok <= match_all.out => motion.enable diff --git a/tests/trajectory-planner/common-config/axis-A.tcl b/tests/trajectory-planner/common-config/axis-A.tcl new file mode 100644 index 00000000000..9a593cc73b9 --- /dev/null +++ b/tests/trajectory-planner/common-config/axis-A.tcl @@ -0,0 +1,35 @@ +set av_id 3 +set aa_id 13 + +# Add axis components to servo thread +addf wcomp.$av_id servo-thread +addf wcomp.$aa_id servo-thread +addf minmax.$av_id servo-thread +addf minmax.$aa_id servo-thread +addf d2dt2.$av_id servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net a_pos axis.$av_id.motor-pos-cmd => axis.$av_id.motor-pos-fb d2dt2.$av_id.in + +# send the position commands thru differentiators to +# generate velocity and accel signals +net a_vel <= d2dt2.$av_id.out1 => wcomp.$av_id.in minmax.$av_id.in +net a_acc <= d2dt2.$av_id.out2 => wcomp.$aa_id.in minmax.$aa_id.in + +#Conservative limits +set acc_limit 1.0001 +set vel_limit 1.01 + +setp wcomp.$aa_id.max $::AXIS_3(MAX_ACCELERATION)*$acc_limit +setp wcomp.$aa_id.min $::AXIS_3(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$av_id.max $::AXIS_3(MAX_VELOCITY)*$vel_limit +setp wcomp.$av_id.min $::AXIS_3(MAX_VELOCITY)*-1.0*$vel_limit + +net a_acc-ok <= wcomp.$av_id.out => match_abc.a0 +net a_vel-ok <= wcomp.$aa_id.out => match_abc.a1 + +# Enable match_all pins for axis +setp match_abc.b0 1 +setp match_abc.b1 1 + diff --git a/tests/trajectory-planner/common-config/axis-X.tcl b/tests/trajectory-planner/common-config/axis-X.tcl new file mode 100644 index 00000000000..0c549683595 --- /dev/null +++ b/tests/trajectory-planner/common-config/axis-X.tcl @@ -0,0 +1,35 @@ +set xv_id 0 +set xa_id 10 + +# Add axis components to servo thread +addf wcomp.$xv_id servo-thread +addf wcomp.$xa_id servo-thread +addf minmax.$xv_id servo-thread +addf minmax.$xa_id servo-thread +addf d2dt2.$xv_id servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net x_pos axis.$xv_id.motor-pos-cmd => axis.$xv_id.motor-pos-fb d2dt2.$xv_id.in + +# send the position commands thru differentiators to +# generate velocity and accel signals +net x_vel <= d2dt2.$xv_id.out1 => vel_xyz.in$xv_id wcomp.$xv_id.in minmax.$xv_id.in +net x_acc <= d2dt2.$xv_id.out2 => wcomp.$xa_id.in minmax.$xa_id.in + +#Conservative limits +set acc_limit 1.0001 +set vel_limit 1.01 + +setp wcomp.$xa_id.max $::AXIS_0(MAX_ACCELERATION)*$acc_limit +setp wcomp.$xa_id.min $::AXIS_0(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$xv_id.max $::AXIS_0(MAX_VELOCITY)*$vel_limit +setp wcomp.$xv_id.min $::AXIS_0(MAX_VELOCITY)*-1.0*$vel_limit + +net x_acc-ok <= wcomp.$xv_id.out => match_xyz.a0 +net x_vel-ok <= wcomp.$xa_id.out => match_xyz.a1 + +# Enable match_all pins for axis +setp match_xyz.b0 1 +setp match_xyz.b1 1 + diff --git a/tests/trajectory-planner/common-config/axis-Y.tcl b/tests/trajectory-planner/common-config/axis-Y.tcl new file mode 100644 index 00000000000..0c393ed57c6 --- /dev/null +++ b/tests/trajectory-planner/common-config/axis-Y.tcl @@ -0,0 +1,35 @@ +set yv_id 1 +set ya_id 11 + +# Add axis components to servo thread +addf wcomp.$yv_id servo-thread +addf wcomp.$ya_id servo-thread +addf minmax.$yv_id servo-thread +addf minmax.$ya_id servo-thread +addf d2dt2.$yv_id servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net y_pos axis.$yv_id.motor-pos-cmd => axis.$yv_id.motor-pos-fb d2dt2.$yv_id.in + +# send the position commands thru differentiators to +# generate velocity and accel signals +net y_vel <= d2dt2.$yv_id.out1 => vel_xyz.in$yv_id wcomp.$yv_id.in minmax.$yv_id.in +net y_acc <= d2dt2.$yv_id.out2 => wcomp.$ya_id.in minmax.$ya_id.in + +#Conservative limits +set acc_limit 1.0001 +set vel_limit 1.01 + +setp wcomp.$ya_id.max $::AXIS_1(MAX_ACCELERATION)*$acc_limit +setp wcomp.$ya_id.min $::AXIS_1(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$yv_id.max $::AXIS_1(MAX_VELOCITY)*$vel_limit +setp wcomp.$yv_id.min $::AXIS_1(MAX_VELOCITY)*-1.0*$vel_limit + +net y_acc-ok <= wcomp.$yv_id.out => match_xyz.a2 +net y_vel-ok <= wcomp.$ya_id.out => match_xyz.a3 + +# Enable match_all pins for axis +setp match_xyz.b2 1 +setp match_xyz.b3 1 + diff --git a/tests/trajectory-planner/common-config/axis-Z.tcl b/tests/trajectory-planner/common-config/axis-Z.tcl new file mode 100644 index 00000000000..87e186ce971 --- /dev/null +++ b/tests/trajectory-planner/common-config/axis-Z.tcl @@ -0,0 +1,34 @@ +set zv_id 2 +set za_id 12 + +# Add axis components to servo thread +addf wcomp.$zv_id servo-thread +addf wcomp.$za_id servo-thread +addf minmax.$zv_id servo-thread +addf minmax.$za_id servo-thread +addf d2dt2.$zv_id servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net z_pos axis.$zv_id.motor-pos-cmd => axis.$zv_id.motor-pos-fb d2dt2.$zv_id.in + +# send the position commands thru differentiators to +# generate velocity and accel signals +net z_vel <= d2dt2.$zv_id.out1 => vel_xyz.in$zv_id wcomp.$zv_id.in minmax.$zv_id.in +net z_acc <= d2dt2.$zv_id.out2 => wcomp.$za_id.in minmax.$za_id.in + +#Conservative limits +set acc_limit 1.0001 +set vel_limit 1.01 + +setp wcomp.$za_id.max $::AXIS_2(MAX_ACCELERATION)*$acc_limit +setp wcomp.$za_id.min $::AXIS_2(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$zv_id.max $::AXIS_2(MAX_VELOCITY)*$vel_limit +setp wcomp.$zv_id.min $::AXIS_2(MAX_VELOCITY)*-1.0*$vel_limit + +net z_acc-ok <= wcomp.$zv_id.out => match_xyz.a4 +net z_vel-ok <= wcomp.$za_id.out => match_xyz.a5 + +# Enable match_all pins for axis +setp match_xyz.b4 1 +setp match_xyz.b5 1 diff --git a/tests/trajectory-planner/common-config/core_sim_components.hal b/tests/trajectory-planner/common-config/core_sim_components.hal new file mode 100644 index 00000000000..7f32d4a7384 --- /dev/null +++ b/tests/trajectory-planner/common-config/core_sim_components.hal @@ -0,0 +1,19 @@ +# core HAL config file for simulation + +# first load all the RT modules that will be needed +# kinematics +loadrt trivkins +# motion controller, get name and thread periods from ini file +loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[TRAJ]AXES + +# add motion controller functions to servo thread +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +# estop loopback +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in + +# create signals for tool loading loopback +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed + diff --git a/tests/trajectory-planner/common-config/linuxcnc_control.py b/tests/trajectory-planner/common-config/linuxcnc_control.py new file mode 100644 index 00000000000..c9316f8c41f --- /dev/null +++ b/tests/trajectory-planner/common-config/linuxcnc_control.py @@ -0,0 +1,291 @@ +#!/usr/bin/env python +"""Copied from m61-test""" + +import linuxcnc +import os +import sys +import re +from time import sleep +import datetime +import string + + +class LinuxcncError(Exception): + def __init__(self, value): + self.value = value + + def __str__(self): + return repr(self.value) + + +class CommandTimeoutError(Exception): + def __init__(self, message): + # Call the base class constructor with the parameters it needs + self.message = message + + def __str__(self): + return self.message + + +class LinuxcncControl: + """ + issue G-Code commands + make sure important modes are saved and restored + mode is saved only once, and can be restored only once + + usage example: + e = emc_control() + e.prepare_for_mdi() + any internal sub using e.g("G0.....") + e.finish_mdi() + + """ + + def __init__(self, timeout=2, max_runtime=24 * 60 * 60, throw_lcnc_exceptions=False): + self.c = None + self.e = None + self.s = None + self.cmd_timeout = timeout + self.raise_on_error = throw_lcnc_exceptions + # Ignore blank errors by default + self.err_ignore_mask = '^$' + self.try_init(ignore_error=True) + # Don't allow a test to run for more than + self.max_runtime = max_runtime + self.has_error_ = False + + def try_init(self, ignore_error=False): + try: + self.c = linuxcnc.command() + self.e = linuxcnc.error_channel() + self.s = linuxcnc.stat() + self.s.poll() + except linuxcnc.error as e: + if ignore_error: + return False + + return True + + def wait_for_backend(self, timeout_sec): + k = 0 + print('Waiting for LinuxCNC...') + while k < 25: + k += 1 + sleep(1) + if self.try_init(ignore_error=True): + return True + + return False + + def await_done(self, timeout=None): + sts = self.c.wait_complete(timeout if timeout is not None else self.cmd_timeout) + if sts == -1: + raise CommandTimeoutError("command exceeded timeout of {}".format(self.cmd_timeout)) + return sts + + def running(self, do_poll=True): + """ + check if interpreter is running. + If so, cant switch to MDI mode. + """ + if do_poll: + self.s.poll() + return (self.s.task_mode == linuxcnc.MODE_AUTO and + self.s.interp_state != linuxcnc.INTERP_IDLE) + + def set_mode(self, m): + """ + set EMC mode if possible, else throw LinuxcncError + """ + self.s.poll() + if self.s.task_mode == m : + return 1 + if self.running(do_poll=False): + raise LinuxcncError("interpreter running - cant change mode") + self.c.mode(m) + return self.await_done() + + + def set_state(self, m): + """ + set EMC mode if possible, else throw LinuxcncError + """ + self.s.poll() + if self.s.task_mode == m : + return 1 + self.c.state(m) + return self.await_done() + + def do_home(self, axismask): + self.s.poll() + self.c.home(axismask) + return self.await_done() + + def ok_for_mdi(self): + """ + check wether ok to run MDI commands. + """ + self.s.poll() + return not self.s.estop and self.s.enabled and self.s.homed + + def prepare_for_mdi(self): + """ + check wether ok to run MDI commands. + throw LinuxcncError if told so. + return current mode + """ + + self.s.poll() + if self.s.estop: + raise LinuxcncError("machine in ESTOP") + + if not self.s.enabled: + raise LinuxcncError("machine not enabled") + + if not self.s.homed: + raise LinuxcncError("machine not homed") + + if self.running(): + raise LinuxcncError("interpreter not idle") + + return self.set_mode(linuxcnc.MODE_MDI) + + g_raise_except = True + + def g(self, code, wait=False): + """ + issue G-Code as MDI command. + wait for completion if reqested + """ + + self.c.mdi(code) + if wait: + try: + while self.c.wait_complete(self.cmd_timeout) == -1: + pass + return True + except KeyboardInterrupt: + self.log('mdi', 'warning', 'interrupted by keyboard in c.wait_complete(self.timeout)') + return False + + self.poll_and_log_error(code) + return False + + def get_current_tool(self): + self.s.poll() + return self.s.tool_in_spindle + + def active_codes(self): + self.s.poll() + return self.s.gcodes + + def get_current_system(self): + g = self.active_codes() + for i in g: + if i >= 540 and i <= 590: + return i/10 - 53 + elif i >= 590 and i <= 593: + return i - 584 + return 1 + + def open_program(self, filename): + """Open an nc file""" + self.s.poll() + self.set_mode(linuxcnc.MODE_AUTO) + full_file = os.path.join(os.getcwd(), filename) + self.c.program_open(full_file) + return self.await_done() + + def run_full_program(self): + """Start a loaded program""" + self.s.poll() + self.c.auto(linuxcnc.AUTO_RUN, 0) + # WARNING: does not await completion of the command (use wait_on_program for this) + + def set_feed_scale(self, scale): + """Assign a feed scale""" + + self.s.poll() + self.c.feedrate(scale) + return self.await_done() + + def wait_on_program(self, f): + iter_count = 0 + update_rate = 50. # Hz + max_iter = int(update_rate * self.max_runtime) + while iter_count < max_iter: + self.s.poll() + self.flush_errors(f) + if self.s.task_state != linuxcnc.STATE_ON: + self.log_error(f, 'motion disabled') + return False + + if self.s.exec_state == linuxcnc.EXEC_ERROR or self.s.state == linuxcnc.RCS_ERROR: + self.log_error(f, 'unhandled linuxcnc error with exec_state = {}, rcs_state = {}'.format( + self.s.exec_state, self.s.state)) + return False + + if self.s.exec_state == linuxcnc.EXEC_DONE and self.s.state == linuxcnc.RCS_DONE: + return True + + sleep(1. / update_rate) + else: + self.log_error(f, 'exceeded max allowed run time of {} seconds'.format(self.max_runtime)) + return False + + def run_until_done(self, f): + self.set_mode(linuxcnc.MODE_AUTO) + self.open_program(f) + self.set_mode(linuxcnc.MODE_AUTO) + self.run_full_program() + return self.wait_on_program(f) + + def check_rcs_error(self): + self.s.poll() + if self.s.state == linuxcnc.RCS_ERROR: + return True + return False + + def set_error_ignore_pattern(self, pattern): + self.err_ignore_mask = pattern + + def log(self, ngc_file, level, msg_text): + full_msg = '{} | {}: {}: {}\n'.format(datetime.datetime.now(), ngc_file, level, msg_text) + sys.stderr.write(full_msg) + if string.lower(level) is 'error': + self.has_error_ = True + return full_msg + + def log_error(self, ngc_file, msg_text): + err_str = self.log(ngc_file, 'error', msg_text) + if self.raise_on_error: + raise LinuxcncError(err_str) + + def poll_and_log_error(self, f): + error = self.e.poll() + if error is None: + return False + + err_type, msg_text = error + err_type_str = 'error' if err_type in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR) else 'info' + self.log(f, err_type_str, msg_text) + return True + + def flush_errors(self, filename=""): + while self.poll_and_log_error(filename): + sleep(0.001) + continue + + def has_error(self): + return self.has_error_ + + def do_startup(self, need_home=False): + self.set_mode(linuxcnc.MODE_MANUAL) + self.set_state(linuxcnc.STATE_ESTOP_RESET) + self.set_state(linuxcnc.STATE_ON) + if need_home: + self.do_home(-1) + sleep(0.1) + self.set_mode(linuxcnc.MODE_AUTO) + + diff --git a/tests/trajectory-planner/common-config/load_constraint_components.tcl b/tests/trajectory-planner/common-config/load_constraint_components.tcl new file mode 100644 index 00000000000..5fa13c938db --- /dev/null +++ b/tests/trajectory-planner/common-config/load_constraint_components.tcl @@ -0,0 +1,57 @@ +# Load all constraint-checking components used in HAL files for various sim configurations +# NOTE: add only the actively used ones to the RT thread in axis TCL files + +loadrt d2dt2 count=10 +# load additional blocks +loadrt hypot names=vel_xy,vel_xyz +addf vel_xy servo-thread +addf vel_xyz servo-thread + +# Compares computed velocities / accelerations to limits +loadrt wcomp count=20 + +# Track the overall min / max value of each signal reached (for VCP display) +loadrt minmax count=20 + +# Ties all limit checks together (used to force a motion enable / disable) +loadrt match8 names=match_xyz,match_abc,match_uvw,match_all + +net XYZ-limits-ok <= match_xyz.out => match_all.a0 +net ABC-limits-ok <= match_abc.out => match_all.a1 +net UVW-limits-ok <= match_uvw.out => match_all.a2 + +setp match_xyz.b0 0 +setp match_xyz.b1 0 +setp match_xyz.b2 0 +setp match_xyz.b3 0 +setp match_xyz.b4 0 +setp match_xyz.b5 0 +setp match_xyz.b6 0 +setp match_xyz.b7 0 + +setp match_abc.b0 0 +setp match_abc.b1 0 +setp match_abc.b2 0 +setp match_abc.b3 0 +setp match_abc.b4 0 +setp match_abc.b5 0 +setp match_abc.b6 0 +setp match_abc.b7 0 + +setp match_uvw.b0 0 +setp match_uvw.b1 0 +setp match_uvw.b2 0 +setp match_uvw.b3 0 +setp match_uvw.b4 0 +setp match_uvw.b5 0 +setp match_uvw.b6 0 +setp match_uvw.b7 0 + +setp match_all.b0 1 +setp match_all.b1 1 +setp match_all.b2 1 +setp match_all.b3 0 +setp match_all.b4 0 +setp match_all.b5 0 +setp match_all.b6 0 +setp match_all.b7 0 diff --git a/tests/trajectory-planner/common-config/mill_XYZA_base.ini b/tests/trajectory-planner/common-config/mill_XYZA_base.ini new file mode 100644 index 00000000000..c9d3d2bdf6f --- /dev/null +++ b/tests/trajectory-planner/common-config/mill_XYZA_base.ini @@ -0,0 +1,177 @@ +# LinuxCNC controller parameters for a simulated 4-axis milling machine + +# General section ------------------------------------------------------------- +[EMC] + +# Version of this INI file +VERSION = $Revision$ + +# Name of machine, for use with display, etc. +MACHINE = LinuxCNC-Auto-Test-Config + +# Debug level, 0 means no messages. See src/emc/nml_int/emcglb.h for others +DEBUG = 0 + +# NOTE can only handle raw G-code files + +# Task controller section ----------------------------------------------------- +[TASK] + +# Name of task controller program, e.g., milltask +TASK = milltask + +# Cycle time, in seconds, that task controller will sleep between polls +CYCLE_TIME = 0.001 + +# Part program interpreter section -------------------------------------------- +[RS274NGC] + +# File containing interpreter variables +PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../common-subs + +# Motion control section ------------------------------------------------------ +[EMCMOT] + +EMCMOT = motmod + +# Timeout for comm to emcmot, in seconds +COMM_TIMEOUT = 1.0 + +# Interval between tries to emcmot, in seconds +COMM_WAIT = 0.010 + +# BASE_PERIOD is unused in this configuration but specified in core_sim.hal +BASE_PERIOD = 0 + +# Servo task period, in nano-seconds +SERVO_PERIOD = 1000000 + +# Hardware Abstraction Layer section ----------------------------------------- +[HAL] +# Sim config with individual axis files to set up velocity / acceleration calcs in HAL +# KLUDGE ini file is called from the test directory, hence the prefix +HALFILE = ../common-config/core_sim_components.hal +HALFILE = ../common-config/load_constraint_components.tcl +HALFILE = ../common-config/axis-X.tcl +HALFILE = ../common-config/axis-Y.tcl +HALFILE = ../common-config/axis-Z.tcl +HALFILE = ../common-config/axis-A.tcl +HALFILE = sim_spindle_encoder.hal + +HALUI = halui + +# Trajectory planner section -------------------------------------------------- +[TRAJ] + +AXES = 4 +COORDINATES = X Y Z A +HOME = 0 0 0 0 +LINEAR_UNITS = inch +ANGULAR_UNITS = degree +CYCLE_TIME = 0.010 +DEFAULT_VELOCITY = 1.2 +POSITION_FILE = position.txt +MAX_LINEAR_VELOCITY = 12 + +ARC_BLEND_ENABLE = 1 +ARC_BLEND_FALLBACK_ENABLE = 0 +ARC_BLEND_GAP_CYCLES = 4 +ARC_BLEND_RAMP_FREQ = 100 + +EXTRA_CONSISTENCY_CHECKS = 1 + +# Axes sections --------------------------------------------------------------- + +# First axis +[AXIS_0] + +TYPE = LINEAR +HOME = 0.000 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.000 +INPUT_SCALE = 2000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +# Second axis +[AXIS_1] + +TYPE = LINEAR +HOME = 0.000 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.000 +INPUT_SCALE = 2000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +# Third axis +[AXIS_2] + +TYPE = LINEAR +HOME = 0.0 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.0 +INPUT_SCALE = 2000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -10.0 +MAX_LIMIT = 10.0001 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +[AXIS_3] +TYPE = ANGULAR +HOME = 0.0 +MAX_VELOCITY = 25 +MAX_ACCELERATION = 800 +SCALE = 500.0 +FERROR = 5.0 +MIN_FERROR = 2.5 +MIN_LIMIT = -9999.0 +MAX_LIMIT = 9999.0 +HOME_OFFSET = 0.000000 +HOME_SEARCH_VEL = 0.00000 +HOME_LATCH_VEL = 0.00000 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +# section for main IO controller parameters ----------------------------------- +[EMCIO] + +# Name of IO controller program, e.g., io +EMCIO = io + +# cycle time, in seconds +CYCLE_TIME = 0.066666666666 + +# tool table file +TOOL_TABLE = sim.tbl +TOOL_CHANGE_POSITION = 0 0 0 +TOOL_CHANGE_QUILL_UP = 1 + +[REDIS] +DISABLE_SERVER=1 diff --git a/tests/trajectory-planner/common-config/sim.tbl b/tests/trajectory-planner/common-config/sim.tbl new file mode 100644 index 00000000000..1947e260753 --- /dev/null +++ b/tests/trajectory-planner/common-config/sim.tbl @@ -0,0 +1,6 @@ +T1 P1 D1.000000 Z+1.000000 ;1/8 end mill +T2 P2 D0.062500 Z+0.100000 ;1/16 end mill +T3 P3 D0.201000 Z+1.273000 ;#7 tap drill +T4 P4 D0.031250 Z+0.511000 ;1/32 end mill +T99999 P99999 Z+0.100000 ;big tool number +T15 P15 D0.062500 Z+1.000000 ;dummy tool diff --git a/tests/trajectory-planner/common-subs/default_inch.ngc b/tests/trajectory-planner/common-subs/default_inch.ngc new file mode 100644 index 00000000000..245a4940714 --- /dev/null +++ b/tests/trajectory-planner/common-subs/default_inch.ngc @@ -0,0 +1,4 @@ +o sub +o call +G20 +o endsub diff --git a/tests/trajectory-planner/common-subs/default_mm.ngc b/tests/trajectory-planner/common-subs/default_mm.ngc new file mode 100644 index 00000000000..740977777ae --- /dev/null +++ b/tests/trajectory-planner/common-subs/default_mm.ngc @@ -0,0 +1,4 @@ +o sub +o call +G21 +o endsub diff --git a/tests/trajectory-planner/common-subs/default_modal_state.ngc b/tests/trajectory-planner/common-subs/default_modal_state.ngc new file mode 100644 index 00000000000..ff5f147f21f --- /dev/null +++ b/tests/trajectory-planner/common-subs/default_modal_state.ngc @@ -0,0 +1,19 @@ +o sub +G10 L2 P0 R0 (Clear any coordinate system rotation) +G17 +G20 +G40 (clear cutter compensation) +G49 (cancel tool length compensation) +G54 (Start in G54 work offset) +G64 P0 Q0 (Blend mode with no tolerance applied) +G90 (Absolute positioning) +G92.1 (Reset G92 offsets) +G94 (Feed-per-minute mode) +G97 (RPM spindle mode) +G98 (Return to the initial value) + +M5 (Turn off spindle) +M9 (Turn off coolant) +S0 +F0 +o endsub diff --git a/unit_tests/greatest.h b/unit_tests/greatest.h index fbab23c9c95..e3e6654d404 100644 --- a/unit_tests/greatest.h +++ b/unit_tests/greatest.h @@ -112,6 +112,10 @@ int main(int argc, char **argv) { #define GREATEST_STDOUT stdout #endif +#ifndef GREATEST_STDERR +#define GREATEST_STDERR stderr +#endif + /* Remove GREATEST_ prefix from most commonly used symbols? */ #ifndef GREATEST_USE_ABBREVS #define GREATEST_USE_ABBREVS 1 @@ -144,7 +148,7 @@ int main(int argc, char **argv) { /* Floating point type, for ASSERT_IN_RANGE. */ #ifndef GREATEST_FLOAT #define GREATEST_FLOAT double -#define GREATEST_FLOAT_FMT "%g" +#define GREATEST_FLOAT_FMT "%0.17g" #endif /* Size of buffer for test name + optional '_' separator and suffix */ @@ -487,11 +491,15 @@ typedef enum greatest_test_res { do { \ greatest_info.assertions++; \ if ((EXP) != (GOT)) { \ - GREATEST_FPRINTF(GREATEST_STDOUT, "\nExpected: "); \ - GREATEST_FPRINTF(GREATEST_STDOUT, FMT, EXP); \ - GREATEST_FPRINTF(GREATEST_STDOUT, "\n Got: "); \ - GREATEST_FPRINTF(GREATEST_STDOUT, FMT, GOT); \ - GREATEST_FPRINTF(GREATEST_STDOUT, "\n"); \ + GREATEST_FPRINTF(GREATEST_STDERR, \ + __FILE__ ":%u: In %s:\n", \ + __LINE__, greatest_info.name_buf); \ + GREATEST_FPRINTF(GREATEST_STDERR, \ + __FILE__ ":%u: expected " FMT \ + ", got " FMT "\n", \ + __LINE__, \ + EXP, \ + GOT); \ GREATEST_FAILm(MSG); \ } \ } while (0) @@ -503,9 +511,13 @@ typedef enum greatest_test_res { int greatest_GOT = (int)(GOT); \ greatest_enum_str_fun *greatest_ENUM_STR = ENUM_STR; \ if (greatest_EXP != greatest_GOT) { \ - GREATEST_FPRINTF(GREATEST_STDOUT, "\nExpected: %s", \ - greatest_ENUM_STR(greatest_EXP)); \ - GREATEST_FPRINTF(GREATEST_STDOUT, "\n Got: %s\n", \ + GREATEST_FPRINTF(GREATEST_STDERR, \ + __FILE__ ":%u: In %s:\n", \ + __LINE__, greatest_info.name_buf); \ + GREATEST_FPRINTF(GREATEST_STDERR, \ + __FILE__ ":%u: expected %s, got %s\n", \ + __LINE__, \ + greatest_ENUM_STR(greatest_EXP), \ greatest_ENUM_STR(greatest_GOT)); \ GREATEST_FAILm(MSG); \ } \ @@ -522,12 +534,19 @@ typedef enum greatest_test_res { greatest_EXP - greatest_GOT > greatest_TOL) || \ (greatest_EXP < greatest_GOT && \ greatest_GOT - greatest_EXP > greatest_TOL)) { \ - GREATEST_FPRINTF(GREATEST_STDOUT, \ - "\nExpected: " GREATEST_FLOAT_FMT \ + GREATEST_FPRINTF(GREATEST_STDERR, \ + __FILE__ ":%u: In %s:\n", \ + __LINE__, greatest_info.name_buf); \ + GREATEST_FPRINTF(GREATEST_STDERR, \ + __FILE__ ":%u: expected " GREATEST_FLOAT_FMT \ " +/- " GREATEST_FLOAT_FMT \ - "\n Got: " GREATEST_FLOAT_FMT \ - "\n", \ - greatest_EXP, greatest_TOL, greatest_GOT); \ + ", got " GREATEST_FLOAT_FMT \ + ", difference " GREATEST_FLOAT_FMT "\n", \ + __LINE__, \ + greatest_EXP, \ + greatest_TOL, \ + greatest_GOT, \ + greatest_EXP-greatest_GOT); \ GREATEST_FAILm(MSG); \ } \ } while (0) @@ -759,24 +778,17 @@ static void greatest_do_pass(void) { \ \ static void greatest_do_fail(void) { \ struct greatest_run_info *g = &greatest_info; \ - if (GREATEST_IS_VERBOSE()) { \ - GREATEST_FPRINTF(GREATEST_STDOUT, \ - "%s:%u: error: test %s failed: %s", \ + GREATEST_FPRINTF(GREATEST_STDERR, \ + "%s:%u: error: %s failed: %s\n", \ g->fail_file, g->fail_line, \ g->name_buf, \ g->msg ? g->msg : ""); \ + if (GREATEST_IS_VERBOSE()) { \ + GREATEST_FPRINTF(GREATEST_STDOUT, "FAIL %s: %s", \ + g->name_buf, g->msg ? g->msg : ""); \ } else { \ GREATEST_FPRINTF(GREATEST_STDOUT, "F"); \ g->col++; /* add linebreak if in line of '.'s */ \ - if (g->col != 0) { \ - GREATEST_FPRINTF(GREATEST_STDOUT, "\n"); \ - g->col = 0; \ - } \ - GREATEST_FPRINTF(GREATEST_STDOUT, \ - "%s:%u: error: test %s failed: %s", \ - g->fail_file, g->fail_line, \ - g->name_buf, \ - g->msg ? g->msg : ""); \ } \ g->suite.failed++; \ } \ @@ -1183,6 +1195,7 @@ greatest_run_info greatest_info #define ASSERT_FALSE GREATEST_ASSERT_FALSE #define ASSERT_EQ GREATEST_ASSERT_EQ #define ASSERT_EQ_FMT GREATEST_ASSERT_EQ_FMT +#define ASSERT_FLOAT_EQ(EXP, GOT) GREATEST_ASSERT_IN_RANGE(EXP, GOT, 1e-17) #define ASSERT_IN_RANGE GREATEST_ASSERT_IN_RANGE #define ASSERT_EQUAL_T GREATEST_ASSERT_EQUAL_T #define ASSERT_STR_EQ GREATEST_ASSERT_STR_EQ @@ -1210,6 +1223,14 @@ greatest_run_info greatest_info #define SHUFFLE_TESTS GREATEST_SHUFFLE_TESTS #define SHUFFLE_SUITES GREATEST_SHUFFLE_SUITES +// Customized for LinuxCNC +#define ASSERT_PMCARTESIAN_IN_RANGE(EXP, GOT, tol) do { \ + ASSERT_IN_RANGE(EXP.x, GOT.x, tol); \ + ASSERT_IN_RANGE(EXP.y, GOT.y, tol); \ + ASSERT_IN_RANGE(EXP.z, GOT.z, tol); \ +} while (0) + + #ifdef GREATEST_VA_ARGS #define RUN_TESTp GREATEST_RUN_TESTp #endif diff --git a/unit_tests/mock/mock_rtapi.inc b/unit_tests/mock/mock_rtapi.inc new file mode 100644 index 00000000000..a630d3fedab --- /dev/null +++ b/unit_tests/mock/mock_rtapi.inc @@ -0,0 +1,22 @@ +static struct emcmot_debug_t lcl = {0}; +struct emcmot_debug_t *emcmotDebug = &lcl; + +// KLUDGE fix link error the ugly way +void rtapi_print_msg(msg_level_t level, const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + vprintf(fmt, args); + va_end(args); +} + +void rtapi_print(const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + vprintf(fmt, args); + va_end(args); +} + diff --git a/unit_tests/tp/gen_posemath_boilerplate.m b/unit_tests/tp/gen_posemath_boilerplate.m new file mode 100644 index 00000000000..a4b23810f68 --- /dev/null +++ b/unit_tests/tp/gen_posemath_boilerplate.m @@ -0,0 +1,23 @@ +%% Generate arithmetic test examples for Posemath library +% Suitable for copy/paste into a unit test +s = (rand(3,1)-0.5)*rand(1)*10; +e = (rand(3,1)-0.5)*rand(1)*10; +k = (rand(1)-0.5)*2.0; + + +fprintf(' static const PmCartesian v1 = {%0.17g, %0.17g, %0.17g};\n', s); +fprintf(' static const PmCartesian v2 = {%0.17g, %0.17g, %0.17g};\n', e); +fprintf(' static const double k = %0.17g;\n', k); + +fprintf(' static const double mag_v1 = %0.17g;\n',norm(s)); +fprintf(' static const double mag_v2 = %0.17g;\n',norm(e)); +fprintf(' static const double mag_diff = %0.17g;\n',sqrt(sum((e-s).^2))); +fprintf(' static const double dot = %0.17g;\n', e'*s); +fprintf(' static const PmCartesian cross = {%0.17g, %0.17g, %0.17g};\n', cross(s,e)); +fprintf(' static const PmCartesian diff = {%0.17g, %0.17g, %0.17g};\n', e-s); +fprintf(' static const PmCartesian sum = {%0.17g, %0.17g, %0.17g};\n', e+s); +fprintf(' static const PmCartesian v1_neg = {%0.17g, %0.17g, %0.17g};\n', -s); +fprintf(' static const PmCartesian v1_mult_k = {%0.17g, %0.17g, %0.17g};\n', s*k); +fprintf(' static const PmCartesian v1_div_k = {%0.17g, %0.17g, %0.17g};\n', s/k); +fprintf(' static const PmCartesian elem_mult = {%0.17g, %0.17g, %0.17g};\n', s.*e); +fprintf(' static const PmCartesian elem_div = {%0.17g, %0.17g, %0.17g};\n', s./e); \ No newline at end of file diff --git a/unit_tests/tp/test_blendmath.c b/unit_tests/tp/test_blendmath.c index 8d9140a3786..ca96b1ecb3d 100644 --- a/unit_tests/tp/test_blendmath.c +++ b/unit_tests/tp/test_blendmath.c @@ -4,19 +4,53 @@ #include "tp_types.h" #include "math.h" #include "rtapi.h" +#include "joint_util.h" +#include "motion_debug.h" +#include "tc.h" /* Expand to all the definitions that need to be in the test runner's main file. */ GREATEST_MAIN_DEFS(); -// KLUDGE fix link error the ugly way -void rtapi_print_msg(msg_level_t level, const char *fmt, ...) + +#include "mock_rtapi.inc" + +TEST test_findMaxTangentAngle() { + double acc_limit = 30.0; + double cycle_time = 0.001; + + // Don't bother testing arithmetic, just make sure that the output is sane + double max_angle_out = findMaxTangentAngle(1.0, acc_limit, cycle_time); + ASSERT(max_angle_out <= acc_limit / 1000); + ASSERT(max_angle_out >= 0.0); + + double max_angle_at_zero = findMaxTangentAngle(0.0, acc_limit, cycle_time); + ASSERT(max_angle_at_zero > 0.0); + + PASS(); +} + +TEST test_findKinkAccel() { - va_list args; + double acc_out = findKinkAccel(M_PI / 8, 2.0, 0.001); + ASSERT(acc_out > 0.0); + double acc_tangent = findKinkAccel(0, 2.0, 0.001); + ASSERT_FLOAT_EQ(0.0, acc_tangent); + PASS(); +} + +TEST test_findAccelScale() +{ + PmCartesian bounds = {5.0, 0.0, 20.0}; + PmCartesian acc = {200.0, 100.0, -1.0}; - va_start(args, fmt); - printf(fmt, args); - va_end(args); + PmCartesian scale_out; + findAccelScale(&acc, &bounds, &scale_out); + + ASSERT_FLOAT_EQ(40, scale_out.x); + ASSERT_FLOAT_EQ(0, scale_out.y); + ASSERT_FLOAT_EQ(1/20.0, scale_out.z); + PASS(); } TEST pmCartCartParallel_numerical() { @@ -63,15 +97,365 @@ TEST pmCartCartAntiParallel_numerical() { PASS(); } +SUITE(blendmath) { + RUN_TEST(test_findMaxTangentAngle); + RUN_TEST(test_findKinkAccel); + RUN_TEST(test_findAccelScale); + RUN_TEST(pmCartCartParallel_numerical); + RUN_TEST(pmCartCartAntiParallel_numerical); +} + + +TEST findMinNonZeroComplete() { + PmCartesian normal_bounds = {3.0,1.0,2.0}; + + double min_bound = findMinNonZero(&normal_bounds); + ASSERT_EQ(min_bound, 1.0); + PASS(); +} + +TEST findMinNonZeroPartial() { + PmCartesian partial_bounds = {3.0,0.0,2.0}; + + double min_bound = findMinNonZero(&partial_bounds); + ASSERT_EQ(min_bound, 2.0); + PASS(); +} + +SUITE(joint_utils) { + RUN_TEST(findMinNonZeroComplete); + RUN_TEST(findMinNonZeroPartial); +} + + +static const double a_max_mock = 10.0; +static const double v_max_mock = 2.0; +static const double cycle_time = 0.001; +static const double target_dist_mock = 2.0; + +static const double SMALL_DISTANCE = 1e-10; +static const double SMALL_VELOCITY = 1e-12; + +TEST checkEndCondition_zero_velocity() +{ + EndCondition ec_zero = checkEndCondition(cycle_time, + 0.0, + target_dist_mock, + 0.0, + 0.0, + a_max_mock + ); + // When both initial and final velocity are zero, assume that at least one cycle is left + ASSERT(ec_zero.dt >= cycle_time); + + EndCondition ec_small = checkEndCondition(cycle_time, + 0.0, + target_dist_mock, + SMALL_VELOCITY, + 0.0, + a_max_mock + ); + // when "average" velocity is very small and distance to go is large, at least one cycle is left + ASSERT(ec_small.dt >= cycle_time); + PASS(); +} +TEST checkEndCondition_small_velocity() +{ + EndCondition ec_small_close = checkEndCondition(cycle_time, + target_dist_mock - SMALL_VELOCITY*cycle_time / 2.0, + target_dist_mock, + SMALL_VELOCITY, + 0.0, + a_max_mock + ); + ASSERT(ec_small_close.dt < cycle_time); + + EndCondition ec_small_far = checkEndCondition(cycle_time, + target_dist_mock - SMALL_VELOCITY*cycle_time * 2.0, + target_dist_mock, + SMALL_VELOCITY, + 0.0, + a_max_mock + ); + // when "average" velocity is very small and distance to go is large, at least one cycle is left + ASSERT(ec_small_far.dt >= cycle_time); + PASS(); +} + + +TEST checkEndCondition_at_velocity() +{ + double v_final = 1.0; + EndCondition ec_close = checkEndCondition(cycle_time, + target_dist_mock - 0.001 + SMALL_DISTANCE, + target_dist_mock, + v_final, + v_final, + a_max_mock + ); + ASSERT(ec_close.dt < cycle_time); + ASSERT_IN_RANGEm("final velocity", ec_close.v_f, v_final - DOUBLE_FUZZ, v_final + DOUBLE_FUZZ); + + EndCondition ec_far = checkEndCondition(cycle_time, + target_dist_mock - 0.001 - SMALL_DISTANCE, + target_dist_mock, + v_final, + v_final, + a_max_mock + ); + ASSERT(ec_far.dt >= cycle_time); + ASSERT_IN_RANGEm("final velocity", ec_close.v_f, v_final - DOUBLE_FUZZ, v_final + DOUBLE_FUZZ); + PASS(); +} + +TEST checkEndCondition_below_vf_close() +{ + const double v_final = v_max_mock; + const double v_current = 1.0; + const double nominal_cycle_dist = v_current * cycle_time; + + EndCondition ec = checkEndCondition(cycle_time, + target_dist_mock - nominal_cycle_dist, + target_dist_mock, + v_current, + v_final, + a_max_mock + ); + ASSERT(ec.dt < cycle_time); + ASSERT_IN_RANGEm("final velocity", ec.v_f, 1.0, v_final); + PASS(); +} + +TEST checkEndCondition_below_vf_far() +{ + const double v_final = v_max_mock; + const double v_current = 1.0; + const double nominal_cycle_dist = v_current * cycle_time; + + EndCondition ec = checkEndCondition(cycle_time, + target_dist_mock - 4*nominal_cycle_dist, + target_dist_mock, + v_current, + v_final, + a_max_mock + ); + ASSERT(ec.dt > cycle_time); + ASSERT_IN_RANGEm("final velocity", ec.v_f, 1.0, v_final); + PASS(); +} + +TEST checkEndCondition_above_vf_far() +{ + const double v_final = 1.0; + const double v_current = v_max_mock; + + EndCondition ec = checkEndCondition(cycle_time, + 0.0, + target_dist_mock, + v_current, + v_final, + a_max_mock + ); + ASSERT(ec.dt > cycle_time); + ASSERT_IN_RANGEm("final velocity", ec.v_f, 1.0, v_final); + PASS(); +} + + +TEST checkEndCondition_above_vf_close() +{ + const double v_final = 1.0; + const double v_current = v_final + a_max_mock * cycle_time; + const double nominal_cycle_dist = (v_current + v_final) / 2.0 * cycle_time; + + EndCondition ec = checkEndCondition(cycle_time, + target_dist_mock - nominal_cycle_dist, + target_dist_mock, + v_current, + v_final, + a_max_mock + ); + ASSERT_IN_RANGE(ec.dt, cycle_time, TP_TIME_EPSILON); + ASSERT_IN_RANGEm("final velocity", ec.v_f, 1.0, v_final); + PASS(); +} + + +SUITE(tc_functions) { + RUN_TEST(checkEndCondition_zero_velocity); + RUN_TEST(checkEndCondition_small_velocity); + RUN_TEST(checkEndCondition_at_velocity); + RUN_TEST(checkEndCondition_below_vf_far); + RUN_TEST(checkEndCondition_below_vf_close); + RUN_TEST(checkEndCondition_above_vf_far); + RUN_TEST(checkEndCondition_above_vf_close); +} + +TEST test_pmCircleTangentVector_unitcircle() +{ + PmCircle c; + PmCartesian start = {1,0, 0}; + PmCartesian end = {0,1,0}; + PmCartesian center = {0,0,0}; + PmCartesian normal = {0,0,1}; + pmCircleInit(&c, &start, &end, ¢er, &normal, 0); + + { + PmCartesian utan_start; + const double angle = 0.0; + ASSERT_FALSE(pmCircleTangentVector(&c, angle, &utan_start)); + PmCartesian const expect_utan_start = {0,1,0}; + ASSERT_PMCARTESIAN_IN_RANGE(utan_start, expect_utan_start, CART_FUZZ); + } + + { + PmCartesian utan_30deg; + const double angle = M_PI / 6.0; + ASSERT_FALSE(pmCircleTangentVector(&c, angle, &utan_30deg)); + PmCartesian const expect_utan_30deg = {-0.5, sqrt(3)/2.0,0}; + ASSERT_PMCARTESIAN_IN_RANGE(utan_30deg, expect_utan_30deg, CART_FUZZ); + } + { + PmCartesian utan_60deg; + const double angle = M_PI / 3.0; + ASSERT_FALSE(pmCircleTangentVector(&c, angle, &utan_60deg)); + PmCartesian const expect_utan_60deg = {-sqrt(3)/2.0, 0.5, 0}; + ASSERT_PMCARTESIAN_IN_RANGE(utan_60deg, expect_utan_60deg, CART_FUZZ); + } + { + PmCartesian utan_end; + const double angle = M_PI / 2.0; + ASSERT_FALSE(pmCircleTangentVector(&c, angle, &utan_end)); + PmCartesian const expect_utan_end = {-1.0, 0, 0}; + ASSERT_PMCARTESIAN_IN_RANGE(utan_end, expect_utan_end, CART_FUZZ); + } + + PASS(); +} + + +/** + * Evaluate the derivative of the ideal XY spiral + * x(theta) = a*cos(theta) - (r0+a*theta)*sin(theta) + * y(theta) = a*sin(theta) + (r0+a*theta)*sin(theta) + * + * Where a is the spiral component, r0 is radius when theta is zero. + * + * Uses the derivative to produce the unit vector at the given theta. + * From https://math.stackexchange.com/questions/1078185/differentiate-archimedess-spiral + */ +static PmCartesian idealXYSpiralUnitVec(double a, double r0, double theta) +{ + PmCartesian out = {0}; + out.x = a*cos(theta) - (r0+a*theta)*sin(theta); + out.y = a*sin(theta) + (r0+a*theta)*cos(theta); + pmCartUnitEq(&out); + return out; +} + +TEST test_pmCircleTangentVector_spiralout() +{ + PmCircle c; + PmCartesian start = {1,0, 0}; + PmCartesian end = {1.0 + M_PI_2,0,0}; + PmCartesian center = {0,0,0}; + PmCartesian normal = {0,0,1}; + pmCircleInit(&c, &start, &end, ¢er, &normal, 0); + + // a = (r1-r0)/theta = (pi/2) / (2pi) = 1/4 + double expect_spiral_a = 0.25; + ASSERT_FLOAT_EQ(c.angle, 2.0 * M_PI); + + { + PmCartesian utan_start; + const double angle = 0.0; + ASSERT_FALSE(pmCircleTangentVector(&c, angle, &utan_start)); + PmCartesian const expect_utan_start = idealXYSpiralUnitVec(expect_spiral_a, 1.0, angle); + ASSERT_PMCARTESIAN_IN_RANGE(utan_start, expect_utan_start, CART_FUZZ); + } + + { + PmCartesian utan_30deg; + const double angle = M_PI / 6.0; + ASSERT_FALSE(pmCircleTangentVector(&c, angle, &utan_30deg)); + PmCartesian const expect_utan_30deg = idealXYSpiralUnitVec(expect_spiral_a, 1.0, angle); + ASSERT_PMCARTESIAN_IN_RANGE(utan_30deg, expect_utan_30deg, CART_FUZZ); + } + + { + PmCartesian utan_end; + const double angle = M_PI / 6.0; + ASSERT_FALSE(pmCircleTangentVector(&c, angle, &utan_end)); + PmCartesian const expect_utan_end = idealXYSpiralUnitVec(expect_spiral_a, 1.0, angle); + ASSERT_PMCARTESIAN_IN_RANGE(utan_end, expect_utan_end, CART_FUZZ); + } + + PASS(); +} + + +TEST test_pmCircleActualMaxVel_cutoff() +{ + double const v_max = 8.0; + double const a_max = 30.0; + double const r_cutoff = pmSq(v_max)/(a_max * BLEND_ACC_RATIO_NORMAL); - SUITE(blendmath) { - RUN_TEST(pmCartCartParallel_numerical); - RUN_TEST(pmCartCartAntiParallel_numerical); + PmCircle c; + PmCartesian start = {r_cutoff,0, 0}; + PmCartesian end = start; + PmCartesian center = {0,0,0}; + PmCartesian normal = {0,0,1}; + pmCircleInit(&c, &start, &end, ¢er, &normal, 2); - } + PmCircleLimits lim = pmCircleActualMaxVel(&c, v_max, a_max); + + ASSERT_IN_RANGE(lim.v_max, v_max, CART_FUZZ); + ASSERT_IN_RANGE(lim.acc_ratio, 0.5, CART_FUZZ); + PASS(); +} + +SUITE(circle_funcs) +{ + RUN_TEST(test_pmCircleTangentVector_unitcircle); + RUN_TEST(test_pmCircleTangentVector_spiralout); + RUN_TEST(test_pmCircleActualMaxVel_cutoff); +} + +TEST test_calculateInscribedRadius() +{ + const PmCartesian xy = {0,0,1}; + const PmCartesian bounds = {3,4,5}; + double d_xy; + calculateInscribedRadius(&xy, &bounds, &d_xy); + ASSERT_FLOAT_EQ(3.0, d_xy); + + double d_yz; + const PmCartesian xz = {1,0,0}; + calculateInscribedRadius(&xz, &bounds, &d_yz); + ASSERT_FLOAT_EQ(4.0, d_yz); + + // Test the case where the normal vector is aligned just so the plane slices the prism along the XY diagonal + double d_xycorner; + PmCartesian corner = {-4.0, 3.0, 0.0}; + pmCartUnitEq(&corner); + + calculateInscribedRadius(&corner, &bounds, &d_xycorner); + ASSERT_FLOAT_EQ(5, d_xycorner); + + PASS(); +} + +SUITE(geom_funcs) +{ + RUN_TEST(test_calculateInscribedRadius); +} int main(int argc, char **argv) { GREATEST_MAIN_BEGIN(); /* command-line arguments, initialization. */ - RUN_SUITE(blendmath); /* run a suite */ + RUN_SUITE(blendmath); + RUN_SUITE(joint_utils); + RUN_SUITE(tc_functions); + RUN_SUITE(circle_funcs); + RUN_SUITE(geom_funcs); GREATEST_MAIN_END(); /* display results */ - } +} diff --git a/unit_tests/tp/test_joint_util.c b/unit_tests/tp/test_joint_util.c new file mode 100644 index 00000000000..35ea22b990c --- /dev/null +++ b/unit_tests/tp/test_joint_util.c @@ -0,0 +1,83 @@ +#include "tp_debug.h" +#include "greatest.h" +#include "math.h" +#include "rtapi.h" +#include "joint_util.h" +#include "motion_debug.h" +#include "mot_priv.h" + +static const struct emcmot_debug_t emcmotDebug_empty = {0}; + +/* Expand to all the definitions that need to be in + the test runner's main file. */ +GREATEST_MAIN_DEFS(); + +#include "mock_rtapi.inc" + +static void emc_globals_setup(void *arg) { + printf("-- in EMC global vars setup callback\n"); + emcmotDebug->joints[0].acc_limit = 10.0; + emcmotDebug->joints[1].acc_limit = 5.0; + emcmotDebug->joints[2].acc_limit = 2.0; + (void)arg; +} + +static void emc_globals_teardown(void *arg) { + printf("-- in EMC global vars teardown callback\n"); + *emcmotDebug = emcmotDebug_empty; + (void)arg; +} + +TEST findAccelViolations_basic() +{ + const EmcPose bad_accel = {{10.01,5.01,2.01},0,0,0,0,0,0}; + const EmcPose good_accel = {{10,5,2},0,0,0,0,0,0}; + unsigned bad_axes = findAccelViolations(bad_accel); + + ASSERT_EQ(bad_axes, 1 | (1<<1) | (1<<2)); + unsigned good_axes = findAccelViolations(good_accel); + + ASSERT_FALSE(good_axes); + PASS(); +} + +TEST findAccelViolations_single() +{ + const EmcPose accel = {{10.0,5.01,2.0},0,0,0,0,0,0}; + + unsigned bad_axes = findAccelViolations(accel); + ASSERT_EQ(bad_axes, (1<<1)); + PASS(); +} + +TEST findMinNonZero_cases() +{ + { + PmCartesian v1 = {1,2,0}; + ASSERT_IN_RANGE(findMinNonZero(&v1), 1.0,DOUBLE_FUZZ); + } + + { + PmCartesian v2 = {3,1,4}; + ASSERT_IN_RANGE(findMinNonZero(&v2), 1.0,DOUBLE_FUZZ); + } + { + PmCartesian v3 = {3,0,0}; + ASSERT_IN_RANGE(findMinNonZero(&v3), 3.0,DOUBLE_FUZZ); + } + PASS(); +} + +SUITE(joint_util) { + GREATEST_SET_SETUP_CB(emc_globals_setup, NULL); + GREATEST_SET_TEARDOWN_CB(emc_globals_teardown, NULL); + RUN_TEST(findAccelViolations_basic); + RUN_TEST(findAccelViolations_single); + RUN_TEST(findMinNonZero_cases); +} + +int main(int argc, char **argv) { + GREATEST_MAIN_BEGIN(); /* command-line arguments, initialization. */ + RUN_SUITE(joint_util); + GREATEST_MAIN_END(); /* display results */ +} diff --git a/unit_tests/tp/test_posemath.c b/unit_tests/tp/test_posemath.c new file mode 100644 index 00000000000..9906c0acbc4 --- /dev/null +++ b/unit_tests/tp/test_posemath.c @@ -0,0 +1,507 @@ +#include "posemath.h" +#include "greatest.h" +#include "rtapi.h" +#include "math.h" +#include "motion_debug.h" + +/* Expand to all the definitions that need to be in + the test runner's main file. */ +GREATEST_MAIN_DEFS(); + +#include "mock_rtapi.inc" + +#define ASSERT_PMCARTESIAN_EQ(EXP, GOT) ASSERT_PMCARTESIAN_IN_RANGE(EXP, GOT, V_FUZZ) + +TEST test_pmCartCartCompare_self() +{ + PmCartesian start = {10.0, 2.0, -3.45}; + + // Always self-compare positive + ASSERT(pmCartCartCompare(&start, &start)); + PASS(); +} + +TEST test_pmCartCartCompare_small() +{ + PmCartesian start = {DOUBLE_FUZZ, DOUBLE_FUZZ, DOUBLE_FUZZ}; + + PmCartesian large_diff=start; + large_diff.z += 1.0; + ASSERT(pmCartCartCompare(&start, &large_diff) == 0); + + // Always self-compare positive + ASSERT(pmCartCartCompare(&start, &start)); + + // NOTE: the initial point is <0,0,0> so that initial magnitude is small, + // and adding the fuzz value does not round away the difference. + PmCartesian too_small=start; + too_small.y += nextafter(CART_FUZZ, 0.0); + ASSERT(pmCartCartCompare(&start, &too_small) == 1); + + PmCartesian big_enough=start; + big_enough.x += nextafter(CART_FUZZ, 1.0); + ASSERT(pmCartCartCompare(&start, &big_enough) == 0); + + PASS(); +} + +TEST test_pmCartCartDot_parallel() +{ + PmCartesian zero = {0.0,0.0,0.0}; + double pmdot; + pmCartCartDot(&zero, &zero, &pmdot); + ASSERT_FLOAT_EQ(pmdot, 0.0); + + PmCartesian one = {1.0,-2.0,0.5}; + pmCartCartDot(&one, &one, &pmdot); + ASSERT_FLOAT_EQ(pmdot, 5.25); + PASS(); +} + +TEST test_pmCartCartDot_perpendicular() +{ + PmCartesian v1 = {cos(M_PI_4), sin(M_PI_4), 0.0}; + PmCartesian v2 = {sin(M_PI_4), -cos(M_PI_4), 0.0}; + double pmdot; + pmCartCartDot(&v1, &v2, &pmdot); + ASSERT_FLOAT_EQ(pmdot, 0.0); + + PASS(); +} + +TEST test_pmCartInfNorm() +{ + PmCartesian v1 ={3.25, -1, 2.0}; + PmCartesian v2 = v1; + double v1_inf; + pmCartInfNorm(&v1, &v1_inf); + ASSERT_FLOAT_EQ(v1_inf, 3.25); + + v2.y = -4; + double v2_inf; + pmCartInfNorm(&v2, &v2_inf); + ASSERT_FLOAT_EQ(v2_inf, 4); + + PASS(); +} + +static const PmCartesian v1 = {0.0011892247147802617, 0.019855106334368509, -0.00050476166977278807}; +static const PmCartesian v2 = {-0.15453530685917072, -0.28437966952533078, -0.15744435569802101}; +static const double k = -0.21253452789880767; +static const double mag_v1 = 0.019897092433684791; +static const double mag_v2 = 0.35991899456205328; +static const double mag_diff = 0.37608372040023119; +static const double dot = -0.0057506939081026628; +static const PmCartesian cross = {-0.0032696183809693856, 0.00026524021852782629, 0.0027301236187225355}; +static const PmCartesian diff = {-0.15572453157395097, -0.30423477585969927, -0.15693959402824822}; +static const PmCartesian sum = {-0.15334608214439047, -0.26452456319096229, -0.1579491173677938}; +static const PmCartesian v1_neg = {-0.0011892247147802617, -0.019855106334368509, 0.00050476166977278807}; +static const PmCartesian v1_mult_k = {-0.0002527513133214171, -0.0042198956511556368, 0.00010727928318657337}; +static const PmCartesian v1_div_k = {-0.0055954424278133177, -0.093420615137988119, 0.002374963140168529}; +static const PmCartesian elem_mult = {-0.00018377720622307753, -0.005646388577758019, 7.9471875878433867e-05}; +static const PmCartesian elem_div = {-0.0076954887459084794, -0.069819007693164015, 0.0032059686581646868}; + +TEST test_PmCartesian_arithmetic() +{ + double pm_mag_v1=0.0; + pmCartMag(&v1, &pm_mag_v1); + ASSERT_IN_RANGE(1e-16, mag_v1, pm_mag_v1); + + double pm_mag_v2=0.0; + pmCartMag(&v2, &pm_mag_v2); + ASSERT_IN_RANGE(mag_v2, 1e-16, pm_mag_v2); + + double pm_mag_sq=0.0; + pmCartMagSq(&v1, &pm_mag_sq); + ASSERT_FLOAT_EQ(pow(mag_v1, 2), pm_mag_sq); + + double pm_disp; + ASSERT_EQ(PM_OK, pmCartCartDisp(&v1, &v2, &pm_disp)); + ASSERT_FLOAT_EQ(mag_diff, pm_disp); + + PmCartesian pmsum; + ASSERT_EQ(PM_OK, pmCartCartAdd(&v1, &v2, &pmsum)); + ASSERT_PMCARTESIAN_EQ(sum, pmsum); + + PmCartesian pm_diff; + ASSERT_EQ(PM_OK, pmCartCartSub(&v2, &v1, &pm_diff)); + ASSERT_PMCARTESIAN_EQ(diff, pm_diff); + + PmCartesian pm_mult_k; + ASSERT_EQ(PM_OK, pmCartScalMult(&v1, k, &pm_mult_k)); + ASSERT_PMCARTESIAN_EQ(v1_mult_k, pm_mult_k); + + PmCartesian pm_div_k; + ASSERT_EQ(PM_OK, pmCartScalDiv(&v1, k, &pm_div_k)); + ASSERT_PMCARTESIAN_EQ(v1_div_k, pm_div_k); + + PmCartesian pm_neg; + ASSERT_EQ(PM_OK, pmCartNeg(&v1, &pm_neg)); + ASSERT_PMCARTESIAN_EQ(v1_neg, pm_neg); + + double pm_dot=0.0; + pmCartCartDot(&v1, &v2, &pm_dot); + ASSERT_FLOAT_EQ(pm_dot, dot); + + PmCartesian pm_cross; + pmCartCartCross(&v1, &v2, &pm_cross); + ASSERT_PMCARTESIAN_EQ(pm_cross, cross); + + PmCartesian pm_elem_mult; + pmCartCartMult(&v1, &v2, &pm_elem_mult); + ASSERT_PMCARTESIAN_EQ(pm_elem_mult, elem_mult); + + PmCartesian pm_elem_div; + pmCartCartDiv(&v1, &v2, &pm_elem_div); + ASSERT_PMCARTESIAN_EQ(pm_elem_div, elem_div); + + PASS(); +} + +TEST test_pmCartUnit() +{ + PmCartesian pm_u1; + ASSERT_FALSE(pmCartUnit(&v1, &pm_u1)); + + double rx = pm_u1.x / v1.x; + double ry = pm_u1.y / v1.y; + double rz = pm_u1.z / v1.z; + + // Verify that unit vector is in the same direction + ASSERT_FLOAT_EQ(rx, ry); + ASSERT_FLOAT_EQ(ry, rz); + ASSERT_FLOAT_EQ(rz, rx); + + double pm_umag; + ASSERT_FALSE(pmCartMag(&pm_u1, &pm_umag)); + + // And that the magnitude is close enough + ASSERT_IN_RANGE(1.0, 1e-16, pm_umag); + + // Show that the self-modifying version is the same output + PmCartesian ueq = v1; + ASSERT_FALSE(pmCartUnitEq(&ueq)); + ASSERT_PMCARTESIAN_EQ(ueq, pm_u1); + PASS(); +} + +TEST test_pmCartAbs() +{ + PmCartesian mixed = {-0.1, 2.0, -5.0}; + + PmCartesian pm_abs; + ASSERT_FALSE(pmCartAbs(&mixed, &pm_abs)); + ASSERT(pm_abs.x >= 0.0); + ASSERT(pm_abs.y >= 0.0); + ASSERT(pm_abs.z >= 0.0); + PASS(); +} + +SUITE(pm_cart_basics) +{ + RUN_TEST(test_pmCartCartCompare_self); + RUN_TEST(test_pmCartCartCompare_small); + RUN_TEST(test_pmCartCartDot_parallel); + RUN_TEST(test_pmCartCartDot_perpendicular); + RUN_TEST(test_pmCartInfNorm); + RUN_TEST(test_PmCartesian_arithmetic); + RUN_TEST(test_pmCartUnit); + RUN_TEST(test_pmCartAbs); +} + +TEST test_pmCircleInit_simple() +{ + PmCircle c; + PmCartesian start = {10.0,0, 0}; + PmCartesian end = start; + PmCartesian center = {0,0,0}; + PmCartesian normal = {0,0,1}; + pmCircleInit(&c, &start, &end, ¢er, &normal, 0); + + ASSERT_FLOAT_EQ(c.angle, 2.0 * M_PI); + ASSERT_FLOAT_EQ(c.radius, 10.0); + + PASS(); +} + +TEST test_pmCircleInit() +{ + PmCircle c; + PmCartesian center = {2.2, 3.3, 4.4}; + + const double nominal_radius = 0.5; + const double theta1 = 0.123; + const double theta2 = 1.456; + + PmCartesian start = {0.0, nominal_radius * cos(theta1), nominal_radius * sin(theta1)}; + pmCartCartAddEq(&start, ¢er); + PmCartesian end = {0.0, nominal_radius * cos(theta2), nominal_radius * sin(theta2)}; + pmCartCartAddEq(&end, ¢er); + + PmCartesian normal = {1,0,0}; + pmCircleInit(&c, &start, &end, ¢er, &normal, 0); + + ASSERT_IN_RANGE(c.angle, theta2-theta1, CART_FUZZ); + ASSERT_IN_RANGE(c.radius, nominal_radius, CART_FUZZ); + + PASS(); +} + +TEST test_pmCircleInit_spiral_short() +{ + PmCircle c; + PmCartesian center = {0.0, 0.0, 0.0}; + PmCartesian start = {1,0, 0}; + PmCartesian end = {0, 1 + 0.3,0}; + + PmCartesian normal = {0,0,1}; + pmCircleInit(&c, &start, &end, ¢er, &normal, 0); + + ASSERT_IN_RANGE(M_PI_2, c.angle, CART_FUZZ); + ASSERT_IN_RANGE(0.3, c.spiral, CART_FUZZ); + + PASS(); +} + +TEST test_pmCircleInit_spiral_long() +{ + PmCircle c; + PmCartesian center = {0.0, 0.0, 0.0}; + PmCartesian start = {1,0, 0}; + PmCartesian end = {-1-0.3, 0,0}; + + PmCartesian normal = {0,0,1}; + pmCircleInit(&c, &start, &end, ¢er, &normal, 3); + + ASSERT_IN_RANGE(M_PI * 7, c.angle, CART_FUZZ); + ASSERT_IN_RANGE(0.3, c.spiral, CART_FUZZ); + + PASS(); +} + +TEST test_pmCircleInit_helix() +{ + PmCircle c; + // Assume center is zero for ease of writing the test + const PmCartesian center = {1.0, 2.0, 3.0}; + + PmCartesian start = {1,0, 0}; + PmCartesian end = {1.0,0,0.4}; + pmCartCartAddEq(&start, ¢er); + pmCartCartAddEq(&end, ¢er); + + PmCartesian normal = {0,0,1}; + pmCircleInit(&c, &start, &end, ¢er, &normal, 0); + + PmCartesian const expect_rHelix = {0,0,0.4}; + + ASSERT_IN_RANGE(2.0 * M_PI, c.angle, CART_FUZZ); + ASSERT_PMCARTESIAN_EQ(expect_rHelix, c.rHelix); + const double expect_radius = 1.0; + + // Radius should be constant for an ideal helix + for (double angle = 0.0; angle <= M_PI * 2.0; angle += M_PI / 3.0) { + PmCartesian p = {0}; + pmCirclePoint(&c, 0.0, &p); + pmCartCartSubEq(&p, ¢er); + // Ignore perpendicular component + p.z=0; + double r; + pmCartMag(&p, &r); + + ASSERT_FLOAT_EQ(expect_radius, r); + } + + PASS(); +} + + +TEST test_pmCircleStretch() +{ + PmCircle c; + PmCartesian center = {0.0, 0.0, 0.0}; + + const double nominal_radius = 0.5; + const double theta1 = M_PI_4; + const double theta2 = 3.0 / 8.0 * M_PI; + + PmCartesian start = {nominal_radius * cos(theta1), nominal_radius * sin(theta1), 0.0}; + PmCartesian end = {nominal_radius * cos(theta2), nominal_radius * sin(theta2), 0.0}; + PmCartesian normal = {0,0,1}; + + pmCircleInit(&c, &start, &end, ¢er, &normal, 0); + + PmCartesian sample_before; + pmCirclePoint(&c, M_PI / 8.0, &sample_before); + + // Stretch to extend past the end point + pmCircleStretch(&c, M_PI, 0); + + PmCartesian sample_end_stretch; + pmCirclePoint(&c, M_PI / 8.0, &sample_end_stretch); + + ASSERT_PMCARTESIAN_EQ(sample_before, sample_end_stretch); + + // Stretch to extend past the starting point + pmCircleStretch(&c, M_PI * 5.0/4.0, 1); + + PmCartesian sample_start_stretch; + pmCirclePoint(&c, M_PI / 8.0 + M_PI_4, &sample_start_stretch); + + ASSERT_PMCARTESIAN_EQ(sample_before, sample_start_stretch); + PASS(); +} + +SUITE(pm_circle) { + RUN_TEST(test_pmCircleInit_simple); + RUN_TEST(test_pmCircleInit); + RUN_TEST(test_pmCircleInit_spiral_short); + RUN_TEST(test_pmCircleInit_spiral_long); + RUN_TEST(test_pmCircleInit_helix); + RUN_TEST(test_pmCircleStretch); +} + +TEST test_pmCartLineInit_simple() +{ + PmCartLine tst; + PmCartesian start = {0,0,0}; + PmCartesian end = {1,2,3}; + + pmCartLineInit(&tst, &start, &end); + + ASSERT_FLOAT_EQ(tst.tmag, sqrt(1 + 4 + 9)); + ASSERT_PMCARTESIAN_EQ(start, tst.start); + ASSERT_PMCARTESIAN_EQ(end, tst.end); + PASS(); +} + +TEST test_pmCartLineInit_ex1() +{ + PmCartLine tst; + const PmCartesian start = { + 0.51320891452372031, + 0.3513817033641376, + 0.30203143034775765, + }; + const PmCartesian end = { + 0.96211392505131188, + 0.20784727108932183, + 0.73266325488481165, + }; + const double mag = 0.63840552161564434; + pmCartLineInit(&tst, &start, &end); + + ASSERT_FLOAT_EQ(tst.tmag, mag); + ASSERT_PMCARTESIAN_EQ(start, tst.start); + ASSERT_PMCARTESIAN_EQ(end, tst.end); + PASS(); +} + +TEST test_pmCartLineInit_ex2() +{ + PmCartLine tst; + const PmCartesian start = { + 1.2196110251384671, + -0.99958732370407333, + 0.41726932038630371, + }; + const PmCartesian end = { + 0.29786615882106027, + 0.91903453415863678, + 0.7079824705329727, + }; + const double mag = 2.1483103983640843; + pmCartLineInit(&tst, &start, &end); + + ASSERT_FLOAT_EQ(tst.tmag, mag); + ASSERT_PMCARTESIAN_EQ(start, tst.start); + ASSERT_PMCARTESIAN_EQ(end, tst.end); + PASS(); +} + +TEST test_pmCartLineInit_small_belowfuzz() +{ + PmCartLine tst; + const PmCartesian start = { + 0,0,0 + }; + const double almost_fuzz = nextafter(CART_FUZZ, 0.0); + const PmCartesian end = { + almost_fuzz, + -almost_fuzz, + almost_fuzz, + }; + pmCartLineInit(&tst, &start, &end); + double mag=0.0; + pmCartMag(&end, &mag); + + // Store the actual magnitude + ASSERT_FLOAT_EQ(tst.tmag, mag); + ASSERT(tst.tmag_zero > 0); + ASSERT_PMCARTESIAN_EQ(start, tst.start); + ASSERT_PMCARTESIAN_EQ(end, tst.end); + PASS(); +} + +TEST test_pmCartLineInit_small_abovefuzz() +{ + PmCartLine tst; + const PmCartesian start = { + 0,0,0 + }; + const double above_fuzz = nextafter(CART_FUZZ, 1.0); + const PmCartesian end = { + above_fuzz, + -above_fuzz, + above_fuzz, + }; + pmCartLineInit(&tst, &start, &end); + + ASSERT(tst.tmag > 0.0); + ASSERT(tst.tmag_zero == 0); + ASSERT_PMCARTESIAN_EQ(start, tst.start); + ASSERT_PMCARTESIAN_EQ(end, tst.end); + PASS(); +} + +TEST test_pmCartLinePoint() +{ + PmCartLine tst; + const PmCartesian start = { + 0.020215567195396768, + -0.01189543243460452, + 0.025741132979321263, + }; + const PmCartesian end = { + 4.6752714631010335, + -2.6867637044820922, + -3.6444762156871819, + }; + + pmCartLineInit(&tst, &start, &end); + PmCartesian point = {0,0,0}; + pmCartLinePoint(&tst, 1.4828560169111997, &point); + + ASSERT(tst.tmag > 0.0); + ASSERT(tst.tmag_zero == 0); + ASSERT_PMCARTESIAN_EQ(start, tst.start); + ASSERT_PMCARTESIAN_EQ(end, tst.end); + PASS(); +} + +SUITE(pm_line) { + RUN_TEST(test_pmCartLineInit_simple); + RUN_TEST(test_pmCartLineInit_ex1); + RUN_TEST(test_pmCartLineInit_ex2); + RUN_TEST(test_pmCartLineInit_small_belowfuzz); + RUN_TEST(test_pmCartLineInit_small_abovefuzz); + RUN_TEST(test_pmCartLinePoint); +} + +int main(int argc, char **argv) { + GREATEST_MAIN_BEGIN(); + RUN_SUITE(pm_cart_basics); + RUN_SUITE(pm_circle); + RUN_SUITE(pm_line); + GREATEST_MAIN_END(); +} diff --git a/unit_tests/tp/test_spherical_arc.c b/unit_tests/tp/test_spherical_arc.c new file mode 100644 index 00000000000..235e23db485 --- /dev/null +++ b/unit_tests/tp/test_spherical_arc.c @@ -0,0 +1,72 @@ +#include "tp_debug.h" +#include "spherical_arc.h" +#include "greatest.h" +#include "blendmath.h" +#include "tp_types.h" +#include "math.h" +#include "rtapi.h" +#include "motion_debug.h" + +/* Expand to all the definitions that need to be in + the test runner's main file. */ +GREATEST_MAIN_DEFS(); + +#include "mock_rtapi.inc" + +TEST arcInitFromPoints_simple() { + SphericalArc arc; + PmCartesian start = {1,2,0}; + PmCartesian end = {2,3,0}; + PmCartesian center = {1,3,0}; + + int res = arcInitFromPoints(&arc, &start, &end, ¢er); + ASSERT_FALSE(res); + + ASSERT_PMCARTESIAN_IN_RANGE(arc.end, end, V_FUZZ); + ASSERT_PMCARTESIAN_IN_RANGE(arc.start, start, V_FUZZ); + ASSERT_PMCARTESIAN_IN_RANGE(arc.center, center, V_FUZZ); + + double mag; + pmCartMag(&arc.rStart, &mag); + ASSERT_FLOAT_EQ(mag, 1.0); + pmCartMag(&arc.rEnd, &mag); + ASSERT_FLOAT_EQ(mag, 1.0); + + ASSERT_FLOAT_EQ(arc.radius, 1.0); + ASSERT_FLOAT_EQ(arc.Sangle, 1.0); + ASSERT_FLOAT_EQ(arc.spiral, 0.0); + ASSERT_FLOAT_EQ(arc.angle, M_PI_2); + + PmCartesian pt={0}; + arcPoint(&arc, 0, &pt); + ASSERT_PMCARTESIAN_IN_RANGE(arc.center, center, V_FUZZ); + + PASS(); +} + +/** + * Tests the orientation of a line-arc intersection + * @return + */ +TEST test_rayIntersectsArc() { + PmCartesian center = {DOUBLE_FUZZ,1,0}; + PmCartesian P = {0,0,0}; + PmCartesian uVec = {1,0,0}; + int reverse_dir = 0; + + ASSERT(checkRayIntersectsArc(¢er, &P, &uVec, reverse_dir)); + center.x*=-1.0;; + ASSERT_FALSE(checkRayIntersectsArc(¢er, &P, &uVec, reverse_dir)); + PASS(); +} + +SUITE(arc_shape) { + RUN_TEST(arcInitFromPoints_simple); + RUN_TEST(test_rayIntersectsArc); +} + +int main(int argc, char **argv) { + GREATEST_MAIN_BEGIN(); + RUN_SUITE(arc_shape); + GREATEST_MAIN_END(); +}