From 450655c822d874a92e181ab9ccdb4597582b8036 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 24 Sep 2026 22:43:48 +1000 Subject: [PATCH 1/2] Honor [AXIS_] TYPE in the interpreter, canon and motion The interpreter and canon took an axis to be a length or an angle by its letter, so an ANGULAR V moved 25.4 times too far under G20 and a LINEAR A did not scale at all. - src/emc/ini/axis_kinds.hh reads TYPE per letter and a new [TRAJ] FEED_AXES (default X Y Z), for both the interpreter and canon. X Y Z must be LINEAR, and the FEED_AXES letters must be LINEAR axes. - G20/G21 converts positions, offsets, stored positions and tool offsets by type. - F, G93 included, is measured along the feed axes that move, else the other linear axes, else the angular axes in degrees. By default that is XYZ, else UVW, else ABC. - Motion still measures a line by letter, so canon scales the rates it sends by motion's length over its own. The ratio is 1 with the default types. - The max velocity slider caps canon's length. Canon sends a per-move scale, 0 for a move measured in degrees, and tcPureRotaryCheck() goes. - WRAPPED_ROTARY, ROTARY_MODULO and LOCKING_INDEXER_JOINT apply to any ANGULAR axis. - B and C moves use the angular minimum displacement, as A does. - rs274 (sai) takes its axes from [TRAJ] COORDINATES. With the default types the canon output is unchanged. --- docs/src/config/ini-config.adoc | 22 +- docs/src/gcode/machining-center.adoc | 30 +- docs/src/gui/axis.adoc | 2 +- src/Makefile | 1 + src/emc/ini/axis_kinds.hh | 114 +++++ src/emc/motion/command.c | 7 +- src/emc/motion/motion.h | 1 + src/emc/nml_intf/emc.cc | 3 + src/emc/nml_intf/emc.hh | 9 +- src/emc/nml_intf/emc_nml.hh | 6 + src/emc/rs274ngc/interp_check.cc | 4 +- src/emc/rs274ngc/interp_convert.cc | 519 ++++++++++--------- src/emc/rs274ngc/interp_find.cc | 213 +++++--- src/emc/rs274ngc/interp_internal.cc | 28 +- src/emc/rs274ngc/interp_internal.hh | 15 +- src/emc/rs274ngc/interp_namedparams.cc | 42 +- src/emc/rs274ngc/interp_queue.cc | 26 +- src/emc/rs274ngc/interp_queue.hh | 2 +- src/emc/rs274ngc/interp_setup.cc | 14 +- src/emc/rs274ngc/interpmodule.cc | 24 +- src/emc/rs274ngc/rs274ngc_pre.cc | 131 +++-- src/emc/rs274ngc/units.h | 4 + src/emc/sai/driver.cc | 8 + src/emc/sai/saicanon.cc | 3 +- src/emc/sai/saicanon.hh | 1 + src/emc/task/emccanon.cc | 666 +++++++++---------------- src/emc/task/emctaskmain.cc | 5 +- src/emc/task/taskintf.cc | 12 +- src/emc/tp/tc.c | 9 - src/emc/tp/tc.h | 1 - src/emc/tp/tc_types.h | 1 + src/emc/tp/tp.c | 16 +- src/emc/tp/tp.h | 6 +- tests/axis-type/README | 5 + tests/axis-type/axis-type.hal | 17 + tests/axis-type/axis-type.ini | 124 +++++ tests/axis-type/checkresult | 3 + tests/axis-type/test-ui.py | 120 +++++ tests/axis-type/test.sh | 3 + tests/interp/axis-type/expected | 51 ++ tests/interp/axis-type/test.ini | 18 + tests/interp/axis-type/test.ngc | 25 + tests/interp/axis-type/test.sh | 3 + 43 files changed, 1405 insertions(+), 909 deletions(-) create mode 100644 src/emc/ini/axis_kinds.hh create mode 100644 tests/axis-type/README create mode 100644 tests/axis-type/axis-type.hal create mode 100644 tests/axis-type/axis-type.ini create mode 100755 tests/axis-type/checkresult create mode 100755 tests/axis-type/test-ui.py create mode 100755 tests/axis-type/test.sh create mode 100644 tests/interp/axis-type/expected create mode 100644 tests/interp/axis-type/test.ini create mode 100644 tests/interp/axis-type/test.ngc create mode 100755 tests/interp/axis-type/test.sh diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index 8822e2953a3..a99a87e94f8 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -935,12 +935,19 @@ Finally, no amount of tweaking will speed up a tool path with lots of small, tig For the common 'trivkins kinematics', joint numbers are assigned in sequence according to the trivkins parameter 'coordinates='. So, for trivkins 'coordinates=xz', joint0 corresponds to X and joint1 corresponds to Z. See the kinematics man page ('$ man kins') for information on trivkins and other kinematics modules. +* `FEED_AXES = X Y Z` - (((FEED AXES))) The axes the programmed feed rate F is measured along. + All of them must be `LINEAR` axes; the default is X Y Z. + A move goes at F along the straight line through the feed axes that move, and every other axis in the move arrives at the same time. + A move in which no feed axis moves goes at F along the other linear axes that move, and a move of angular axes only goes at F in degrees per minute. + With the defaults this is the RS274NGC rule: X Y Z, else U V W, else A B C. + With `FEED_AXES = X Y Z U`, for example, a move of X and U together goes at F along the line through both. + The maximum velocity slider of the GUIs caps the rate along that same line, and does not cap a move of angular axes only. * `LINEAR_UNITS =` _ - (((LINEAR UNITS))) Specifies the 'machine units' for linear axes. Possible choices are mm or inch. This does not affect the linear units in NC code (the G20 and G21 words do this). * `ANGULAR_UNITS =` __ - (((ANGULAR UNITS))) Specifies the 'machine units' for rotational axes. Possible choices are 'deg', 'degree' (360 per circle), 'rad', 'radian' (2*π per circle), 'grad', or 'gon' (400 per circle). - This does not affect the angular units of NC code. In RS274NGC, A-, B- and C- words are always expressed in degrees. + This does not affect the angular units of NC code. In RS274NGC, the words of an `ANGULAR` axis, A, B and C by default, are always expressed in degrees. * `DEFAULT_LINEAR_VELOCITY = 0.0167` - The initial rate for jogs of linear axes, in machine units per second. The value shown in 'AXIS' equals machine units per minute. * `DEFAULT_LINEAR_ACCELERATION = 2.0` - In machines with nontrivial kinematics, the acceleration used for "teleop" (Cartesian space) jogs, in 'machine units' per second per second. @@ -1022,8 +1029,11 @@ The __ specifies one of: X Y Z A B C U V W * `TYPE = LINEAR` - (enum) The type of this axis, either `LINEAR` or `ANGULAR`. Required if this axis is not a default axis type. The default axis types are X,Y,Z,U,V,W = LINEAR and A,B,C = ANGULAR. - This setting is effective with the AXIS GUI but note that other - GUI's may handle things differently. + X, Y and Z are always `LINEAR`. + A `LINEAR` axis is a length: its words, offsets, stored positions and tool offsets follow G20 and G21 and `[TRAJ]LINEAR_UNITS`. + An `ANGULAR` axis is an angle in degrees whatever G20 or G21 says, in `[TRAJ]ANGULAR_UNITS` for the machine, and may be a `WRAPPED_ROTARY` or have a `LOCKING_INDEXER_JOINT`. + The feed of a move depends on the type too, see `FEED_AXES` in the <>. + The AXIS GUI shows the axis in the units of its type; other GUIs may not. * `MAX_VELOCITY = 1.2` - (real) Maximum velocity for this axis in <> per second. * `MAX_ACCELERATION = 20.0` - (real) Maximum acceleration for this axis in machine units per second squared. * `MAX_JERK = 0.0` - (real) Maximum jerk for this axis in machine units per second cubed. @@ -1039,11 +1049,12 @@ The __ specifies one of: X Y Z A B C U V W For a rotary axis (A,B,C typ) with unlimited rotation having no `MAX_LIMIT` for that axis in the `[AXIS_``]` section a value of 1e99 is used. * `WRAPPED_ROTARY = 1` - (bool) When this is set to 1 for an ANGULAR axis the axis will move 0-359.999 degrees. Positive Numbers will move the axis in a positive direction and negative numbers will move the axis in the negative direction. + It is ignored on a `LINEAR` axis. Mutually exclusive with `ROTARY_MODULO` (only one of the two can be set in a configuration). -* `ROTARY_MODULO = 1` - (bool) Continuous-modulo rotary mode. +* `ROTARY_MODULO = 1` - (bool) Continuous-modulo rotary mode for an ANGULAR axis, ignored on a `LINEAR` one. Absolute moves on this axis take the shortest path: a `G0 A0` from a current position of A=50000 moves only the residual angle (40 deg in this example), not 138 full revolutions. G-code may use absolute values of any magnitude (e.g. `A=200000` for helical-spiral CAM output) without the +/-360 limit imposed by `WRAPPED_ROTARY`. - System parameters `#5423`/`#5424`/`#5425` (and the named equivalents `#<_a>`/`#<_b>`/`#<_c>`, `#<_abs_a>`/`#<_abs_b>`/`#<_abs_c>`) report position wrapped to the range 0 up to but not including 360 degrees; probing parameters `#5064`/`#5065`/`#5066` likewise. + The axis's current position parameter (`#5423` to `#5428`, and the named equivalents `#<_a>` to `#<_w>` and `#<_abs_a>` to `#<_abs_w>`) reports position wrapped to the range 0 up to but not including 360 degrees; its probing parameter (`#5064` to `#5069`) likewise. Internal commanded position and motion-side `motion.traj.position`/`joint..pos-fb` HAL pins remain accumulated to keep stepgens, encoders and PID loops consistent (no phantom unwind on wrap). The accumulated physical position can still be read into G-code from the HAL pin via the `#<_hal[...]>` syntax (see <> and the `HAL_PIN_VARS` setting in <>); for example `#<_hal[joint.3.pos-fb]>` returns the unwrapped joint position. Threading (`G33`, `G33.1`, `G76`) is rejected with an error if the modulo axis word is present in the block. @@ -1057,6 +1068,7 @@ The __ specifies one of: X Y Z A B C U V W When set, a G0 move for this axis will initiate an unlock with the `joint.4.unlock pin` then wait for the `joint.4.is-unlocked` pin then move the joint at the rapid rate for that joint. After the move the `joint.4.unlock` will be false and motion will wait for `joint.4.is-unlocked` to go false. Moving with other joints is not allowed when moving a locked rotary joint. + It is ignored on a `LINEAR` axis. To create the unlock pins, use the motmod parameter: + [source,ini] diff --git a/docs/src/gcode/machining-center.adoc b/docs/src/gcode/machining-center.adoc index 27df8428626..818880163f5 100644 --- a/docs/src/gcode/machining-center.adoc +++ b/docs/src/gcode/machining-center.adoc @@ -179,14 +179,17 @@ rate is interpreted as follows (unless 'inverse time feed' or 'feed per revolution' modes are being used, in which case see section <>). -. If any of XYZ are moving, F is in units per minute in the XYZ - cartesian system, and all other axes (ABCUVW) move so as to start and - stop in coordinated fashion. -. Otherwise, if any of UVW are moving, F is in units per minute in the - UVW cartesian system, and all other axes (ABC) move so as to start and - stop in coordinated fashion. +. If any of the feed axes are moving, F is in units per minute in the + cartesian system of the feed axes, and all other axes move so as to + start and stop in coordinated fashion. The feed axes are X Y Z unless + `[TRAJ]FEED_AXES` in the INI file names others. +. Otherwise, if any of the other linear axes are moving, F is in units + per minute in their cartesian system, and the angular axes move so as + to start and stop in coordinated fashion. These are U V W unless + `[AXIS_]TYPE` in the INI file says otherwise. . Otherwise, the move is pure rotary motion and the F word is in rotary - units in the ABC 'pseudo-cartesian' system. + units in the 'pseudo-cartesian' system of the angular axes, A B C + unless `[AXIS_]TYPE` says otherwise. === Cooling @@ -208,11 +211,14 @@ the previous programmed move, as though it was in exact path mode. === Units (((units))) -Units used for distances along the X, Y, and Z axes may be measured in -millimeters or inches. Units for all other quantities involved in -machine control cannot be changed. Different quantities use different -specific units. Spindle speed is measured in revolutions per minute. -The positions of rotational axes are measured in degrees. Feed rates +Units used for distances along the X, Y, and Z axes, and along any other +linear axis, may be measured in millimeters or inches. Units for all +other quantities involved in machine control cannot be changed. +Different quantities use different specific units. Spindle speed is +measured in revolutions per minute. The positions of rotational axes are +measured in degrees. An axis is linear or rotational as +`[AXIS_]TYPE` in the INI file says: U V W linear and A B C +rotational unless it says otherwise. Feed rates are expressed in current length units per minute, or degrees per minute, or length units per spindle revolution, as described in section <>. diff --git a/docs/src/gui/axis.adoc b/docs/src/gui/axis.adoc index cded6f29d73..2cd01987cc5 100644 --- a/docs/src/gui/axis.adoc +++ b/docs/src/gui/axis.adoc @@ -607,7 +607,7 @@ This slider sets the jog rate for the rotary axes (A, B and C). By moving this slider, the maximum velocity can be set. This caps the maximum velocity for all programmed moves except spindle-synchronized -moves. +moves and moves of angular axes only. == Keyboard Controls diff --git a/src/Makefile b/src/Makefile index c05d9071f95..3fab1c5dae2 100644 --- a/src/Makefile +++ b/src/Makefile @@ -403,6 +403,7 @@ SRCHEADERS := \ emc/linuxcnc.h \ emc/kinematics/kinematics.h \ emc/nml_intf/emcmotcfg.h \ + emc/ini/axis_kinds.hh \ emc/ini/inifile.hh \ emc/ini/inifile.h \ emc/nml_intf/emcpos.h \ diff --git a/src/emc/ini/axis_kinds.hh b/src/emc/ini/axis_kinds.hh new file mode 100644 index 00000000000..86b8b673aa2 --- /dev/null +++ b/src/emc/ini/axis_kinds.hh @@ -0,0 +1,114 @@ +/******************************************************************** +* Description: axis_kinds.hh +* Which of the nine axes are lengths and which are angles, and which +* axes the programmed feed is measured along, as the INI file says. +* The interpreter and canon both read it, so that they measure a move +* the same way. +* +* License: GPL Version 2 +********************************************************************/ +#ifndef AXIS_KINDS_HH +#define AXIS_KINDS_HH + +#include +#include +#include +#include +#include + +#define AXIS_KINDS_ALL 0x1ffu /* X Y Z A B C U V W, bit 0 is X */ +#define AXIS_KINDS_ABC 0x038u /* angular unless the INI says otherwise */ +#define AXIS_KINDS_XYZ 0x007u /* the feed group unless the INI says otherwise */ + +struct AxisKinds { + unsigned angular; /* a bit per axis whose [AXIS_] TYPE is ANGULAR */ + unsigned feed; /* a bit per axis in [TRAJ] FEED_AXES */ +}; + +static inline AxisKinds axisKindsDefault() +{ + return AxisKinds{AXIS_KINDS_ABC, AXIS_KINDS_XYZ}; +} + +static inline bool axisKindsAngular(const AxisKinds &k, int axis) +{ + return (k.angular >> axis) & 1; +} + +/* Read [AXIS_] TYPE (default: A B C angular) and [TRAJ] FEED_AXES + (default XYZ). X Y Z must be linear. Returns 0, or -1 with *err set. */ +static inline int axisKindsRead(const linuxcnc::IniFile &ini, AxisKinds *k, std::string *err) +{ + *k = axisKindsDefault(); + for (int i = 0; i < 9; i++) { + char section[] = "AXIS_X"; + section[5] = "XYZABCUVW"[i]; + auto type = ini.findString("TYPE", section); + if (!type) { continue; } + if (*type == "ANGULAR") { + k->angular |= 1u << i; + } else if (*type == "LINEAR") { + k->angular &= ~(1u << i); + } else { + *err = "[" + std::string(section) + "] TYPE must be LINEAR or ANGULAR, not " + *type; + return -1; + } + } + if (k->angular & AXIS_KINDS_XYZ) { + *err = "[AXIS_X], [AXIS_Y] and [AXIS_Z] TYPE must be LINEAR"; + return -1; + } + auto feed = ini.findString("FEED_AXES", "TRAJ"); + if (!feed) { return 0; } + k->feed = 0; + const char *letters = "XYZABCUVW"; + for (char ch : *feed) { + if (ch == ' ' || ch == '\t') { continue; } + const char *at = strchr(letters, toupper((unsigned char)ch)); + if (!at || !*at) { + *err = std::string("[TRAJ] FEED_AXES: ") + ch + " is not an axis letter"; + return -1; + } + int i = at - letters; + if (axisKindsAngular(*k, i)) { + *err = std::string("[TRAJ] FEED_AXES: ") + letters[i] + " is an ANGULAR axis; the feed group holds linear axes only"; + return -1; + } + k->feed |= 1u << i; + } + if (!k->feed) { + *err = "[TRAJ] FEED_AXES names no axis"; + return -1; + } + return 0; +} + +/* The axes a move is measured along: the feed group if any of it moves, + else the other linear axes, else the angular ones. By default XYZ, else + UVW, else ABC. */ +static inline unsigned axisKindsMeasured(const AxisKinds &k, unsigned moving) +{ + unsigned linear = ~k.angular & ~k.feed & AXIS_KINDS_ALL; + if (moving & k.feed) { return k.feed; } + if (moving & linear) { return linear; } + return k.angular & ~k.feed & AXIS_KINDS_ALL; +} + +/* Whether a set from axisKindsMeasured() is angles. */ +static inline bool axisKindsMeasuredAngular(const AxisKinds &k, unsigned set) +{ + return (set & k.angular) != 0; +} + +/* The Euclidean length of the deltas d over the axes of set, summed in + axis order. */ +static inline double axisKindsLength(unsigned set, const double d[9]) +{ + double sum = 0.0; + for (int i = 0; i < 9; i++) { + if (set & (1u << i)) { sum += d[i] * d[i]; } + } + return sqrt(sum); +} + +#endif diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index b4a1503ed0e..e2ecb07679d 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -1098,7 +1098,8 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) emcmotCommand->vel, emcmotCommand->ini_maxvel, emcmotCommand->acc, - emcmotCommand->ini_maxjerk, + emcmotCommand->ini_maxjerk, + emcmotCommand->vlimit_scale, emcmotStatus->enables_new, issue_atspeed, emcmotCommand->turn, @@ -1158,7 +1159,8 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) emcmotCommand->center, emcmotCommand->normal, emcmotCommand->turn, emcmotCommand->motion_type, emcmotCommand->vel, emcmotCommand->ini_maxvel, - emcmotCommand->acc, emcmotCommand->ini_maxjerk, emcmotStatus->enables_new, + emcmotCommand->acc, emcmotCommand->ini_maxjerk, + emcmotCommand->vlimit_scale, emcmotStatus->enables_new, issue_atspeed, emcmotCommand->tag); if (res_addcircle < 0) { reportError(_("can't add circular move at line %d, error code %d"), @@ -1637,6 +1639,7 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) emcmotCommand->ini_maxvel, emcmotCommand->acc, emcmotCommand->ini_maxjerk, + emcmotCommand->vlimit_scale, emcmotStatus->enables_new, issue_atspeed, -1, diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 2dea7c387aa..b98bfc0642d 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -223,6 +223,7 @@ extern "C" { double acc; /* max acceleration */ double jerk; /* jerk for traj */ double ini_maxjerk; + double vlimit_scale; /* limitVel scale for this move, 0 no limit */ int planner_type; /* planner type: 0 = trapezoidal, 1 = S-curve */ double scurve_peak_scale; /* S-curve rest-to-rest peak scale (0.5=faithful..1.0=full) */ double backlash; /* amount of backlash */ diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index dc35cb4209d..3809d6bd90c 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -1072,6 +1072,7 @@ void EMC_TRAJ_LINEAR_MOVE::update(CMS * cms) cms->update(ini_maxvel); cms->update(ini_maxjerk); cms->update(acc); + cms->update(vlimit_scale); cms->update(feed_mode); cms->update(indexer_jnum); } @@ -1096,6 +1097,7 @@ void EMC_TRAJ_CIRCULAR_MOVE::update(CMS * cms) cms->update(ini_maxvel); cms->update(ini_maxjerk); cms->update(acc); + cms->update(vlimit_scale); cms->update(feed_mode); } @@ -2090,6 +2092,7 @@ void EMC_TRAJ_PROBE::update(CMS * cms) cms->update(ini_maxvel); cms->update(ini_maxjerk); cms->update(acc); + cms->update(vlimit_scale); cms->update(probe_type); } diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index 2d98a5b1dbd..c2f4d1b6923 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -375,16 +375,19 @@ extern int emcTrajStep(); extern int emcTrajResume(); extern int emcTrajDelay(double delay); extern int emcTrajLinearMove(const EmcPose& end, int type, double vel, - double ini_maxvel, double acc, double ini_maxjerk, int indexer_jnum); + double ini_maxvel, double acc, double ini_maxjerk, + double vlimit_scale, int indexer_jnum); extern int emcTrajCircularMove(const EmcPose& end, const PM_CARTESIAN& center, const PM_CARTESIAN& - normal, int turn, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk); + normal, int turn, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk, + double vlimit_scale); extern int emcTrajSetTermCond(int cond, double tolerance); extern int emcTrajSetSpindleSync(int spindle, double feed_per_revolution, bool wait_for_index); extern int emcTrajSetOffset(const EmcPose& tool_offset); extern int emcTrajSetHome(const EmcPose& home); extern int emcTrajClearProbeTrippedFlag(); extern int emcTrajProbe(const EmcPose& pos, int type, double vel, - double ini_maxvel, double acc, double ini_maxjerk, unsigned char probe_type); + double ini_maxvel, double acc, double ini_maxjerk, + double vlimit_scale, unsigned char probe_type); extern int emcTrajRigidTap(const EmcPose& pos, double vel, double ini_maxvel, double acc, double ini_maxjerk, double scale); extern int emcTrajUpdate(EMC_TRAJ_STAT * stat); diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index 358582d08c0..d31f9a5096a 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -741,6 +741,7 @@ class EMC_TRAJ_LINEAR_MOVE:public EMC_TRAJ_CMD_MSG { ini_maxvel(0.0), acc(0.0), ini_maxjerk(0.0), + vlimit_scale(1.0), feed_mode(0), indexer_jnum(0) {}; @@ -753,6 +754,7 @@ class EMC_TRAJ_LINEAR_MOVE:public EMC_TRAJ_CMD_MSG { int type; EmcPose end; // end point double vel, ini_maxvel, acc, ini_maxjerk; + double vlimit_scale; // see emcmot_command_t int feed_mode; int indexer_jnum; }; @@ -770,6 +772,7 @@ class EMC_TRAJ_CIRCULAR_MOVE:public EMC_TRAJ_CMD_MSG { ini_maxvel(0.0), acc(0.0), ini_maxjerk(0.0), + vlimit_scale(1.0), feed_mode(0) {}; @@ -784,6 +787,7 @@ class EMC_TRAJ_CIRCULAR_MOVE:public EMC_TRAJ_CMD_MSG { int turn; int type; double vel, ini_maxvel, acc, ini_maxjerk; + double vlimit_scale; // see emcmot_command_t int feed_mode; }; @@ -930,6 +934,7 @@ class EMC_TRAJ_PROBE:public EMC_TRAJ_CMD_MSG { ini_maxvel(0.0), acc(0.0), ini_maxjerk(0.0), + vlimit_scale(1.0), probe_type(0) {}; @@ -941,6 +946,7 @@ class EMC_TRAJ_PROBE:public EMC_TRAJ_CMD_MSG { EmcPose pos; int type; double vel, ini_maxvel, acc, ini_maxjerk; + double vlimit_scale; // see emcmot_command_t unsigned char probe_type; }; diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index 40992b41268..fc5ae57586e 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -489,7 +489,7 @@ int Interp::check_spindle_sync_feed(setup_pointer settings, //!< pointer to mac double length = 0.0; for (int ax = 0; ax < 9; ax++) { - if (ax >= 3 && ax <= 5) + if (axisKindsAngular(settings->axis_kinds, ax)) continue; /* rotary */ length += delta[ax] * delta[ax]; } @@ -501,7 +501,7 @@ int Interp::check_spindle_sync_feed(setup_pointer settings, //!< pointer to mac double required_rate = fabs(pitch) * speed; for (int ax = 0; ax < 9; ax++) { - if (ax >= 3 && ax <= 5) + if (axisKindsAngular(settings->axis_kinds, ax)) continue; if (delta[ax] == 0.0) continue; diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 173a805b781..552625420f5 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -1641,18 +1641,30 @@ int Interp::convert_axis_offsets(int g_code, //!< g_code being executed (mus CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), /* not "== true" */ NCE_CANNOT_CHANGE_AXIS_OFFSETS_WITH_CUTTER_RADIUS_COMP); - CHKS((block->a_flag && settings->a_axis_wrapped && + CHKS((block->a_flag && settings->axis_wrapped[3] && (block->a_number <= -360.0 || block->a_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->a_number, 'A'); - CHKS((block->b_flag && settings->b_axis_wrapped && + CHKS((block->b_flag && settings->axis_wrapped[4] && (block->b_number <= -360.0 || block->b_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->b_number, 'B'); - CHKS((block->c_flag && settings->c_axis_wrapped && + CHKS((block->c_flag && settings->axis_wrapped[5] && (block->c_number <= -360.0 || block->c_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->c_number, 'C'); + CHKS((block->u_flag && settings->axis_wrapped[6] && + (block->u_number <= -360.0 || block->u_number >= 360.0)), + (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), + block->u_number, 'U'); + CHKS((block->v_flag && settings->axis_wrapped[7] && + (block->v_number <= -360.0 || block->v_number >= 360.0)), + (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), + block->v_number, 'V'); + CHKS((block->w_flag && settings->axis_wrapped[8] && + (block->w_number <= -360.0 || block->w_number >= 360.0)), + (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), + block->w_number, 'W'); pars = settings->parameters; if ((g_code == G_52) || (g_code == G_92)) { pars[G92_APPLIED] = 1.0; @@ -1760,12 +1772,12 @@ int Interp::convert_axis_offsets(int g_code, //!< g_code being executed (mus pars[5211] = PROGRAM_TO_USER_LEN(settings->axis_offset_x); pars[5212] = PROGRAM_TO_USER_LEN(settings->axis_offset_y); pars[5213] = PROGRAM_TO_USER_LEN(settings->axis_offset_z); - pars[5214] = PROGRAM_TO_USER_ANG(settings->AA_axis_offset); - pars[5215] = PROGRAM_TO_USER_ANG(settings->BB_axis_offset); - pars[5216] = PROGRAM_TO_USER_ANG(settings->CC_axis_offset); - pars[5217] = PROGRAM_TO_USER_LEN(settings->u_axis_offset); - pars[5218] = PROGRAM_TO_USER_LEN(settings->v_axis_offset); - pars[5219] = PROGRAM_TO_USER_LEN(settings->w_axis_offset); + pars[5214] = PROGRAM_TO_USER_AX(3, settings->AA_axis_offset); + pars[5215] = PROGRAM_TO_USER_AX(4, settings->BB_axis_offset); + pars[5216] = PROGRAM_TO_USER_AX(5, settings->CC_axis_offset); + pars[5217] = PROGRAM_TO_USER_AX(6, settings->u_axis_offset); + pars[5218] = PROGRAM_TO_USER_AX(7, settings->v_axis_offset); + pars[5219] = PROGRAM_TO_USER_AX(8, settings->w_axis_offset); } else if ((g_code == G_92_1) || (g_code == G_92_2)) { pars[5210] = 0.0; @@ -1810,27 +1822,27 @@ int Interp::convert_axis_offsets(int g_code, //!< g_code being executed (mus settings->current_z = settings->current_z + settings->axis_offset_z - USER_TO_PROGRAM_LEN(pars[5213]); settings->AA_current = - settings->AA_current + settings->AA_axis_offset - USER_TO_PROGRAM_ANG(pars[5214]); + settings->AA_current + settings->AA_axis_offset - USER_TO_PROGRAM_AX(3, pars[5214]); settings->BB_current = - settings->BB_current + settings->BB_axis_offset - USER_TO_PROGRAM_ANG(pars[5215]); + settings->BB_current + settings->BB_axis_offset - USER_TO_PROGRAM_AX(4, pars[5215]); settings->CC_current = - settings->CC_current + settings->CC_axis_offset - USER_TO_PROGRAM_ANG(pars[5216]); + settings->CC_current + settings->CC_axis_offset - USER_TO_PROGRAM_AX(5, pars[5216]); settings->u_current = - settings->u_current + settings->u_axis_offset - USER_TO_PROGRAM_LEN(pars[5217]); + settings->u_current + settings->u_axis_offset - USER_TO_PROGRAM_AX(6, pars[5217]); settings->v_current = - settings->v_current + settings->v_axis_offset - USER_TO_PROGRAM_LEN(pars[5218]); + settings->v_current + settings->v_axis_offset - USER_TO_PROGRAM_AX(7, pars[5218]); settings->w_current = - settings->w_current + settings->w_axis_offset - USER_TO_PROGRAM_LEN(pars[5219]); + settings->w_current + settings->w_axis_offset - USER_TO_PROGRAM_AX(8, pars[5219]); settings->axis_offset_x = USER_TO_PROGRAM_LEN(pars[5211]); settings->axis_offset_y = USER_TO_PROGRAM_LEN(pars[5212]); settings->axis_offset_z = USER_TO_PROGRAM_LEN(pars[5213]); - settings->AA_axis_offset = USER_TO_PROGRAM_ANG(pars[5214]); - settings->BB_axis_offset = USER_TO_PROGRAM_ANG(pars[5215]); - settings->CC_axis_offset = USER_TO_PROGRAM_ANG(pars[5216]); - settings->u_axis_offset = USER_TO_PROGRAM_LEN(pars[5217]); - settings->v_axis_offset = USER_TO_PROGRAM_LEN(pars[5218]); - settings->w_axis_offset = USER_TO_PROGRAM_LEN(pars[5219]); + settings->AA_axis_offset = USER_TO_PROGRAM_AX(3, pars[5214]); + settings->BB_axis_offset = USER_TO_PROGRAM_AX(4, pars[5215]); + settings->CC_axis_offset = USER_TO_PROGRAM_AX(5, pars[5216]); + settings->u_axis_offset = USER_TO_PROGRAM_AX(6, pars[5217]); + settings->v_axis_offset = USER_TO_PROGRAM_AX(7, pars[5218]); + settings->w_axis_offset = USER_TO_PROGRAM_AX(8, pars[5219]); SET_G92_OFFSET(settings->axis_offset_x, settings->axis_offset_y, @@ -2418,12 +2430,12 @@ int Interp::convert_coordinate_system(int g_code, //!< g_code called (mus settings->origin_offset_x = USER_TO_PROGRAM_LEN(parameters[5201 + (origin * 20)]); settings->origin_offset_y = USER_TO_PROGRAM_LEN(parameters[5202 + (origin * 20)]); settings->origin_offset_z = USER_TO_PROGRAM_LEN(parameters[5203 + (origin * 20)]); - settings->AA_origin_offset = USER_TO_PROGRAM_ANG(parameters[5204 + (origin * 20)]); - settings->BB_origin_offset = USER_TO_PROGRAM_ANG(parameters[5205 + (origin * 20)]); - settings->CC_origin_offset = USER_TO_PROGRAM_ANG(parameters[5206 + (origin * 20)]); - settings->u_origin_offset = USER_TO_PROGRAM_LEN(parameters[5207 + (origin * 20)]); - settings->v_origin_offset = USER_TO_PROGRAM_LEN(parameters[5208 + (origin * 20)]); - settings->w_origin_offset = USER_TO_PROGRAM_LEN(parameters[5209 + (origin * 20)]); + settings->AA_origin_offset = USER_TO_PROGRAM_AX(3, parameters[5204 + (origin * 20)]); + settings->BB_origin_offset = USER_TO_PROGRAM_AX(4, parameters[5205 + (origin * 20)]); + settings->CC_origin_offset = USER_TO_PROGRAM_AX(5, parameters[5206 + (origin * 20)]); + settings->u_origin_offset = USER_TO_PROGRAM_AX(6, parameters[5207 + (origin * 20)]); + settings->v_origin_offset = USER_TO_PROGRAM_AX(7, parameters[5208 + (origin * 20)]); + settings->w_origin_offset = USER_TO_PROGRAM_AX(8, parameters[5209 + (origin * 20)]); settings->rotation_xy = parameters[5210 + (origin * 20)]; SET_G5X_OFFSET(origin, @@ -3083,28 +3095,43 @@ int Interp::convert_savehome(int code, block_pointer /*block*/, setup_pointer s) x = PROGRAM_TO_USER_LEN(x + s->tool_offset.tran.x + s->origin_offset_x); y = PROGRAM_TO_USER_LEN(y + s->tool_offset.tran.y + s->origin_offset_y); double z = PROGRAM_TO_USER_LEN(s->current_z + s->tool_offset.tran.z + s->origin_offset_z + s->axis_offset_z); - double a = PROGRAM_TO_USER_ANG(s->AA_current + s->tool_offset.a + s->AA_origin_offset + s->AA_axis_offset); - double b = PROGRAM_TO_USER_ANG(s->BB_current + s->tool_offset.b + s->BB_origin_offset + s->BB_axis_offset); - double c = PROGRAM_TO_USER_ANG(s->CC_current + s->tool_offset.c + s->CC_origin_offset + s->CC_axis_offset); - double u = PROGRAM_TO_USER_LEN(s->u_current + s->tool_offset.u + s->u_origin_offset + s->u_axis_offset); - double v = PROGRAM_TO_USER_LEN(s->v_current + s->tool_offset.v + s->v_origin_offset + s->v_axis_offset); - double w = PROGRAM_TO_USER_LEN(s->w_current + s->tool_offset.w + s->w_origin_offset + s->w_axis_offset); - - if(s->a_axis_wrapped) { + double a = PROGRAM_TO_USER_AX(3, s->AA_current + s->tool_offset.a + s->AA_origin_offset + s->AA_axis_offset); + double b = PROGRAM_TO_USER_AX(4, s->BB_current + s->tool_offset.b + s->BB_origin_offset + s->BB_axis_offset); + double c = PROGRAM_TO_USER_AX(5, s->CC_current + s->tool_offset.c + s->CC_origin_offset + s->CC_axis_offset); + double u = PROGRAM_TO_USER_AX(6, s->u_current + s->tool_offset.u + s->u_origin_offset + s->u_axis_offset); + double v = PROGRAM_TO_USER_AX(7, s->v_current + s->tool_offset.v + s->v_origin_offset + s->v_axis_offset); + double w = PROGRAM_TO_USER_AX(8, s->w_current + s->tool_offset.w + s->w_origin_offset + s->w_axis_offset); + + if(s->axis_wrapped[3]) { a = fmod(a, 360.0); if(a<0) a += 360.0; } - if(s->b_axis_wrapped) { + if(s->axis_wrapped[4]) { b = fmod(b, 360.0); if(b<0) b += 360.0; } - if(s->c_axis_wrapped) { + if(s->axis_wrapped[5]) { c = fmod(c, 360.0); if(c<0) c += 360.0; } + if(s->axis_wrapped[6]) { + u = fmod(u, 360.0); + if(u<0) u += 360.0; + } + + if(s->axis_wrapped[7]) { + v = fmod(v, 360.0); + if(v<0) v += 360.0; + } + + if(s->axis_wrapped[8]) { + w = fmod(w, 360.0); + if(w<0) w += 360.0; + } + if(code == G_28_1) { p[5161] = x; p[5162] = y; @@ -3290,12 +3317,18 @@ int Interp::convert_home(int move, //!< G-code, must be G_28 or G_30 // move indexers first, one at a time // JOINTS_AXES settings->*_indexer_jnum == -1 means notused - if (AA_end != settings->AA_current && (-1 != settings->a_indexer_jnum) ) - issue_straight_index(3,settings->a_indexer_jnum, AA_end, block->line_number, settings); - if (BB_end != settings->BB_current && (-1 != settings->b_indexer_jnum) ) - issue_straight_index(4,settings->b_indexer_jnum, BB_end, block->line_number, settings); - if (CC_end != settings->CC_current && (-1 != settings->c_indexer_jnum) ) - issue_straight_index(5,settings->c_indexer_jnum, CC_end, block->line_number, settings); + if (AA_end != settings->AA_current && (-1 != settings->axis_indexer_jnum[3]) ) + issue_straight_index(3,settings->axis_indexer_jnum[3], AA_end, block->line_number, settings); + if (BB_end != settings->BB_current && (-1 != settings->axis_indexer_jnum[4]) ) + issue_straight_index(4,settings->axis_indexer_jnum[4], BB_end, block->line_number, settings); + if (CC_end != settings->CC_current && (-1 != settings->axis_indexer_jnum[5]) ) + issue_straight_index(5,settings->axis_indexer_jnum[5], CC_end, block->line_number, settings); + if (u_end != settings->u_current && (-1 != settings->axis_indexer_jnum[6]) ) + issue_straight_index(6,settings->axis_indexer_jnum[6], u_end, block->line_number, settings); + if (v_end != settings->v_current && (-1 != settings->axis_indexer_jnum[7]) ) + issue_straight_index(7,settings->axis_indexer_jnum[7], v_end, block->line_number, settings); + if (w_end != settings->w_current && (-1 != settings->axis_indexer_jnum[8]) ) + issue_straight_index(8,settings->axis_indexer_jnum[8], w_end, block->line_number, settings); // Create a state tag and dump it to canon write_canon_state_tag(block, settings); @@ -3318,12 +3351,12 @@ int Interp::convert_home(int move, //!< G-code, must be G_28 or G_30 find_relative(USER_TO_PROGRAM_LEN(parameters[5161]), USER_TO_PROGRAM_LEN(parameters[5162]), USER_TO_PROGRAM_LEN(parameters[5163]), - USER_TO_PROGRAM_ANG(parameters[5164]), - USER_TO_PROGRAM_ANG(parameters[5165]), - USER_TO_PROGRAM_ANG(parameters[5166]), - USER_TO_PROGRAM_LEN(parameters[5167]), - USER_TO_PROGRAM_LEN(parameters[5168]), - USER_TO_PROGRAM_LEN(parameters[5169]), + USER_TO_PROGRAM_AX(3, parameters[5164]), + USER_TO_PROGRAM_AX(4, parameters[5165]), + USER_TO_PROGRAM_AX(5, parameters[5166]), + USER_TO_PROGRAM_AX(6, parameters[5167]), + USER_TO_PROGRAM_AX(7, parameters[5168]), + USER_TO_PROGRAM_AX(8, parameters[5169]), &end_x_home, &end_y_home, &end_z_home, &AA_end_home, &BB_end_home, &CC_end_home, &u_end_home, &v_end_home, &w_end_home, settings); @@ -3331,12 +3364,12 @@ int Interp::convert_home(int move, //!< G-code, must be G_28 or G_30 find_relative(USER_TO_PROGRAM_LEN(parameters[5181]), USER_TO_PROGRAM_LEN(parameters[5182]), USER_TO_PROGRAM_LEN(parameters[5183]), - USER_TO_PROGRAM_ANG(parameters[5184]), - USER_TO_PROGRAM_ANG(parameters[5185]), - USER_TO_PROGRAM_ANG(parameters[5186]), - USER_TO_PROGRAM_LEN(parameters[5187]), - USER_TO_PROGRAM_LEN(parameters[5188]), - USER_TO_PROGRAM_LEN(parameters[5189]), + USER_TO_PROGRAM_AX(3, parameters[5184]), + USER_TO_PROGRAM_AX(4, parameters[5185]), + USER_TO_PROGRAM_AX(5, parameters[5186]), + USER_TO_PROGRAM_AX(6, parameters[5187]), + USER_TO_PROGRAM_AX(7, parameters[5188]), + USER_TO_PROGRAM_AX(8, parameters[5189]), &end_x_home, &end_y_home, &end_z_home, &AA_end_home, &BB_end_home, &CC_end_home, &u_end_home, &v_end_home, &w_end_home, settings); @@ -3375,12 +3408,18 @@ int Interp::convert_home(int move, //!< G-code, must be G_28 or G_30 // move indexers first, one at a time // JOINTS_AXES settings->*_indexer_jnum == -1 means notused - if (AA_end != settings->AA_current && (-1 != settings->a_indexer_jnum) ) - issue_straight_index(3,settings->a_indexer_jnum, AA_end, block->line_number, settings); - if (BB_end != settings->BB_current && (-1 != settings->b_indexer_jnum) ) - issue_straight_index(4,settings->b_indexer_jnum, BB_end, block->line_number, settings); - if (CC_end != settings->CC_current && (-1 != settings->c_indexer_jnum) ) - issue_straight_index(5,settings->c_indexer_jnum, CC_end, block->line_number, settings); + if (AA_end != settings->AA_current && (-1 != settings->axis_indexer_jnum[3]) ) + issue_straight_index(3,settings->axis_indexer_jnum[3], AA_end, block->line_number, settings); + if (BB_end != settings->BB_current && (-1 != settings->axis_indexer_jnum[4]) ) + issue_straight_index(4,settings->axis_indexer_jnum[4], BB_end, block->line_number, settings); + if (CC_end != settings->CC_current && (-1 != settings->axis_indexer_jnum[5]) ) + issue_straight_index(5,settings->axis_indexer_jnum[5], CC_end, block->line_number, settings); + if (u_end != settings->u_current && (-1 != settings->axis_indexer_jnum[6]) ) + issue_straight_index(6,settings->axis_indexer_jnum[6], u_end, block->line_number, settings); + if (v_end != settings->v_current && (-1 != settings->axis_indexer_jnum[7]) ) + issue_straight_index(7,settings->axis_indexer_jnum[7], v_end, block->line_number, settings); + if (w_end != settings->w_current && (-1 != settings->axis_indexer_jnum[8]) ) + issue_straight_index(8,settings->axis_indexer_jnum[8], w_end, block->line_number, settings); STRAIGHT_TRAVERSE(block->line_number, end_x, end_y, end_z, AA_end, BB_end, CC_end, @@ -3400,6 +3439,25 @@ int Interp::convert_home(int move, //!< G-code, must be G_28 or G_30 /****************************************************************************/ +// G20/G21 on the axes past X Y Z that are lengths: A B C U V W as their +// [AXIS_] TYPE says +static void scale_linear_axes(setup_pointer settings, double factor) +{ + double *current[6] = {&settings->AA_current, &settings->BB_current, &settings->CC_current, + &settings->u_current, &settings->v_current, &settings->w_current}; + double *axis_offset[6] = {&settings->AA_axis_offset, &settings->BB_axis_offset, &settings->CC_axis_offset, + &settings->u_axis_offset, &settings->v_axis_offset, &settings->w_axis_offset}; + double *origin_offset[6] = {&settings->AA_origin_offset, &settings->BB_origin_offset, &settings->CC_origin_offset, + &settings->u_origin_offset, &settings->v_origin_offset, &settings->w_origin_offset}; + + for (int n = 0; n < 6; n++) { + if (axisKindsAngular(settings->axis_kinds, n + 3)) { continue; } + *current[n] = (*current[n] * factor); + *axis_offset[n] = (*axis_offset[n] * factor); + *origin_offset[n] = (*origin_offset[n] * factor); + } +} + /*! convert_length_units Returned Value: int @@ -3447,7 +3505,7 @@ int Interp::convert_length_units(int g_code, //!< g_code being executed (mus settings->program_x = (settings->program_x * INCH_PER_MM); settings->program_y = (settings->program_y * INCH_PER_MM); settings->program_z = (settings->program_z * INCH_PER_MM); - qc_scale(INCH_PER_MM); + qc_scale(INCH_PER_MM, settings->axis_kinds); settings->cutter_comp_radius *= INCH_PER_MM; settings->axis_offset_x = (settings->axis_offset_x * INCH_PER_MM); settings->axis_offset_y = (settings->axis_offset_y * INCH_PER_MM); @@ -3456,15 +3514,7 @@ int Interp::convert_length_units(int g_code, //!< g_code being executed (mus settings->origin_offset_y = (settings->origin_offset_y * INCH_PER_MM); settings->origin_offset_z = (settings->origin_offset_z * INCH_PER_MM); - settings->u_current = (settings->u_current * INCH_PER_MM); - settings->v_current = (settings->v_current * INCH_PER_MM); - settings->w_current = (settings->w_current * INCH_PER_MM); - settings->u_axis_offset = (settings->u_axis_offset * INCH_PER_MM); - settings->v_axis_offset = (settings->v_axis_offset * INCH_PER_MM); - settings->w_axis_offset = (settings->w_axis_offset * INCH_PER_MM); - settings->u_origin_offset = (settings->u_origin_offset * INCH_PER_MM); - settings->v_origin_offset = (settings->v_origin_offset * INCH_PER_MM); - settings->w_origin_offset = (settings->w_origin_offset * INCH_PER_MM); + scale_linear_axes(settings, INCH_PER_MM); settings->tool_offset.tran.x = GET_EXTERNAL_TOOL_LENGTH_XOFFSET(); settings->tool_offset.tran.y = GET_EXTERNAL_TOOL_LENGTH_YOFFSET(); @@ -3490,7 +3540,7 @@ int Interp::convert_length_units(int g_code, //!< g_code being executed (mus settings->program_x = (settings->program_x * MM_PER_INCH); settings->program_y = (settings->program_y * MM_PER_INCH); settings->program_z = (settings->program_z * MM_PER_INCH); - qc_scale(MM_PER_INCH); + qc_scale(MM_PER_INCH, settings->axis_kinds); settings->cutter_comp_radius *= MM_PER_INCH; settings->axis_offset_x = (settings->axis_offset_x * MM_PER_INCH); settings->axis_offset_y = (settings->axis_offset_y * MM_PER_INCH); @@ -3499,15 +3549,7 @@ int Interp::convert_length_units(int g_code, //!< g_code being executed (mus settings->origin_offset_y = (settings->origin_offset_y * MM_PER_INCH); settings->origin_offset_z = (settings->origin_offset_z * MM_PER_INCH); - settings->u_current = (settings->u_current * MM_PER_INCH); - settings->v_current = (settings->v_current * MM_PER_INCH); - settings->w_current = (settings->w_current * MM_PER_INCH); - settings->u_axis_offset = (settings->u_axis_offset * MM_PER_INCH); - settings->v_axis_offset = (settings->v_axis_offset * MM_PER_INCH); - settings->w_axis_offset = (settings->w_axis_offset * MM_PER_INCH); - settings->u_origin_offset = (settings->u_origin_offset * MM_PER_INCH); - settings->v_origin_offset = (settings->v_origin_offset * MM_PER_INCH); - settings->w_origin_offset = (settings->w_origin_offset * MM_PER_INCH); + scale_linear_axes(settings, MM_PER_INCH); settings->tool_offset.tran.x = GET_EXTERNAL_TOOL_LENGTH_XOFFSET(); settings->tool_offset.tran.y = GET_EXTERNAL_TOOL_LENGTH_YOFFSET(); @@ -4498,36 +4540,31 @@ int Interp::convert_motion(int motion, //!< g_code for a line, arc, canned cyc block_pointer block, //!< pointer to a block of RS274 instructions setup_pointer settings) //!< pointer to machine settings { - int ai = block->a_flag && (-1 != settings->a_indexer_jnum); - int bi = block->b_flag && (-1 != settings->b_indexer_jnum); - int ci = block->c_flag && (-1 != settings->c_indexer_jnum); + const bool axis_flag[9] = {block->x_flag, block->y_flag, block->z_flag, + block->a_flag, block->b_flag, block->c_flag, + block->u_flag, block->v_flag, block->w_flag}; + int indexed = -1; // the first axis word on a locking indexer - - if (motion != G_0) { - CHKS((ai), (_("Indexing axis %c can only be moved with G0")), 'A'); - CHKS((bi), (_("Indexing axis %c can only be moved with G0")), 'B'); - CHKS((ci), (_("Indexing axis %c can only be moved with G0")), 'C'); + for (int n = 8; n >= 3; n--) { + if (axis_flag[n] && -1 != settings->axis_indexer_jnum[n]) { indexed = n; } + } + for (int n = 3; n < 9 && motion != G_0; n++) { + CHKS((axis_flag[n] && -1 != settings->axis_indexer_jnum[n]), + (_("Indexing axis %c can only be moved with G0")), "XYZABCUVW"[n]); + } + for (int n = 3; n < 9; n++) { + if (!axis_flag[n] || -1 == settings->axis_indexer_jnum[n]) { continue; } + for (int other = 0; other < 9; other++) { + CHKS((other != n && axis_flag[other]), + (_("Indexing axis %c can only be moved alone")), "XYZABCUVW"[n]); + } } - - int xyzuvw_flag = (block->x_flag || block->y_flag || block->z_flag || - block->u_flag || block->v_flag || block->w_flag); - - CHKS((ai && (xyzuvw_flag || block->b_flag || block->c_flag)), - (_("Indexing axis %c can only be moved alone")), 'A'); - CHKS((bi && (xyzuvw_flag || block->a_flag || block->c_flag)), - (_("Indexing axis %c can only be moved alone")), 'B'); - CHKS((ci && (xyzuvw_flag || block->a_flag || block->b_flag)), - (_("Indexing axis %c can only be moved alone")), 'C'); if (!is_a_cycle(motion)) settings->cycle_il_flag = false; - if (ai || bi || ci) { - int anum=-1,jnum=-1; - if ( ai) {anum = 3; jnum = settings->a_indexer_jnum;} - else if (bi) {anum = 4; jnum = settings->b_indexer_jnum;} - else if (ci) {anum = 5; jnum = settings->c_indexer_jnum;} - CHP(convert_straight_indexer(anum, jnum, block, settings)); + if (indexed != -1) { + CHP(convert_straight_indexer(indexed, settings->axis_indexer_jnum[indexed], block, settings)); } else if ((motion == G_0) || (motion == G_1) || (motion == G_33) || (motion == G_33_1) || (motion == G_76)) { CHP(convert_straight(motion, block, settings)); } else if ((motion == G_3) || (motion == G_2)) { @@ -4715,17 +4752,17 @@ int Interp::convert_setup_tool(block_pointer block, setup_pointer settings) { if(block->z_flag) settings->tool_table[idx].offset.tran.z = PROGRAM_TO_USER_LEN(block->z_number); if(block->a_flag) - settings->tool_table[idx].offset.a = PROGRAM_TO_USER_ANG(block->a_number); + settings->tool_table[idx].offset.a = PROGRAM_TO_USER_AX(3, block->a_number); if(block->b_flag) - settings->tool_table[idx].offset.b = PROGRAM_TO_USER_ANG(block->b_number); + settings->tool_table[idx].offset.b = PROGRAM_TO_USER_AX(4, block->b_number); if(block->c_flag) - settings->tool_table[idx].offset.c = PROGRAM_TO_USER_ANG(block->c_number); + settings->tool_table[idx].offset.c = PROGRAM_TO_USER_AX(5, block->c_number); if(block->u_flag) - settings->tool_table[idx].offset.u = PROGRAM_TO_USER_LEN(block->u_number); + settings->tool_table[idx].offset.u = PROGRAM_TO_USER_AX(6, block->u_number); if(block->v_flag) - settings->tool_table[idx].offset.v = PROGRAM_TO_USER_LEN(block->v_number); + settings->tool_table[idx].offset.v = PROGRAM_TO_USER_AX(7, block->v_number); if(block->w_flag) - settings->tool_table[idx].offset.w = PROGRAM_TO_USER_LEN(block->w_number); + settings->tool_table[idx].offset.w = PROGRAM_TO_USER_AX(8, block->w_number); } else { int to_fixture = block->l_number == 11; int destination_system = to_fixture? 9 : settings->origin_index; // maybe 9 (g59.3) should be user configurable? @@ -4742,12 +4779,12 @@ int Interp::convert_setup_tool(block_pointer block, setup_pointer settings) { tx += USER_TO_PROGRAM_LEN(settings->parameters[5211]); ty += USER_TO_PROGRAM_LEN(settings->parameters[5212]); tz += USER_TO_PROGRAM_LEN(settings->parameters[5213]); - ta += USER_TO_PROGRAM_ANG(settings->parameters[5214]); - tb += USER_TO_PROGRAM_ANG(settings->parameters[5215]); - tc += USER_TO_PROGRAM_ANG(settings->parameters[5216]); - tu += USER_TO_PROGRAM_LEN(settings->parameters[5217]); - tv += USER_TO_PROGRAM_LEN(settings->parameters[5218]); - tw += USER_TO_PROGRAM_LEN(settings->parameters[5219]); + ta += USER_TO_PROGRAM_AX(3, settings->parameters[5214]); + tb += USER_TO_PROGRAM_AX(4, settings->parameters[5215]); + tc += USER_TO_PROGRAM_AX(5, settings->parameters[5216]); + tu += USER_TO_PROGRAM_AX(6, settings->parameters[5217]); + tv += USER_TO_PROGRAM_AX(7, settings->parameters[5218]); + tw += USER_TO_PROGRAM_AX(8, settings->parameters[5219]); } @@ -4794,17 +4831,17 @@ int Interp::convert_setup_tool(block_pointer block, setup_pointer settings) { if(block->z_flag) settings->tool_table[idx].offset.tran.z = PROGRAM_TO_USER_LEN(tz - block->z_number); if(block->a_flag) - settings->tool_table[idx].offset.a = PROGRAM_TO_USER_ANG(ta - block->a_number); + settings->tool_table[idx].offset.a = PROGRAM_TO_USER_AX(3, ta - block->a_number); if(block->b_flag) - settings->tool_table[idx].offset.b = PROGRAM_TO_USER_ANG(tb - block->b_number); + settings->tool_table[idx].offset.b = PROGRAM_TO_USER_AX(4, tb - block->b_number); if(block->c_flag) - settings->tool_table[idx].offset.c = PROGRAM_TO_USER_ANG(tc - block->c_number); + settings->tool_table[idx].offset.c = PROGRAM_TO_USER_AX(5, tc - block->c_number); if(block->u_flag) - settings->tool_table[idx].offset.u = PROGRAM_TO_USER_LEN(tu - block->u_number); + settings->tool_table[idx].offset.u = PROGRAM_TO_USER_AX(6, tu - block->u_number); if(block->v_flag) - settings->tool_table[idx].offset.v = PROGRAM_TO_USER_LEN(tv - block->v_number); + settings->tool_table[idx].offset.v = PROGRAM_TO_USER_AX(7, tv - block->v_number); if(block->w_flag) - settings->tool_table[idx].offset.w = PROGRAM_TO_USER_LEN(tw - block->w_number); + settings->tool_table[idx].offset.w = PROGRAM_TO_USER_AX(8, tw - block->w_number); } if(block->r_flag) settings->tool_table[idx].diameter = PROGRAM_TO_USER_LEN(block->r_number) * 2.; @@ -4950,15 +4987,24 @@ int Interp::convert_setup(block_pointer block, //!< pointer to a block of RS27 p_int = settings->origin_index; } - CHKS((block->l_number == 20 && block->a_flag && settings->a_axis_wrapped && + CHKS((block->l_number == 20 && block->a_flag && settings->axis_wrapped[3] && (block->a_number <= -360.0 || block->a_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->a_number, 'A'); - CHKS((block->l_number == 20 && block->b_flag && settings->b_axis_wrapped && + CHKS((block->l_number == 20 && block->b_flag && settings->axis_wrapped[4] && (block->b_number <= -360.0 || block->b_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->b_number, 'B'); - CHKS((block->l_number == 20 && block->c_flag && settings->c_axis_wrapped && + CHKS((block->l_number == 20 && block->c_flag && settings->axis_wrapped[5] && (block->c_number <= -360.0 || block->c_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->c_number, 'C'); + CHKS((block->l_number == 20 && block->u_flag && settings->axis_wrapped[6] && + (block->u_number <= -360.0 || block->u_number >= 360.0)), + (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->u_number, 'U'); + CHKS((block->l_number == 20 && block->v_flag && settings->axis_wrapped[7] && + (block->v_number <= -360.0 || block->v_number >= 360.0)), + (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->v_number, 'V'); + CHKS((block->l_number == 20 && block->w_flag && settings->axis_wrapped[8] && + (block->w_number <= -360.0 || block->w_number >= 360.0)), + (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->w_number, 'W'); CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF && p_int == settings->origin_index), (_("Cannot change the active coordinate system with cutter radius compensation on"))); @@ -5032,45 +5078,45 @@ int Interp::convert_setup(block_pointer block, //!< pointer to a block of RS27 if (block->a_flag) { a = block->a_number; - if (block->l_number == 20) a = ca + USER_TO_PROGRAM_ANG(parameters[5204 + (p_int * 20)]) - a; - parameters[5204 + (p_int * 20)] = PROGRAM_TO_USER_ANG(a); + if (block->l_number == 20) a = ca + USER_TO_PROGRAM_AX(3, parameters[5204 + (p_int * 20)]) - a; + parameters[5204 + (p_int * 20)] = PROGRAM_TO_USER_AX(3, a); } else - a = USER_TO_PROGRAM_ANG(parameters[5204 + (p_int * 20)]); + a = USER_TO_PROGRAM_AX(3, parameters[5204 + (p_int * 20)]); if (block->b_flag) { b = block->b_number; - if (block->l_number == 20) b = cb + USER_TO_PROGRAM_ANG(parameters[5205 + (p_int * 20)]) - b; - parameters[5205 + (p_int * 20)] = PROGRAM_TO_USER_ANG(b); + if (block->l_number == 20) b = cb + USER_TO_PROGRAM_AX(4, parameters[5205 + (p_int * 20)]) - b; + parameters[5205 + (p_int * 20)] = PROGRAM_TO_USER_AX(4, b); } else - b = USER_TO_PROGRAM_ANG(parameters[5205 + (p_int * 20)]); + b = USER_TO_PROGRAM_AX(4, parameters[5205 + (p_int * 20)]); if (block->c_flag) { c = block->c_number; - if (block->l_number == 20) c = cc + USER_TO_PROGRAM_ANG(parameters[5206 + (p_int * 20)]) - c; - parameters[5206 + (p_int * 20)] = PROGRAM_TO_USER_ANG(c); + if (block->l_number == 20) c = cc + USER_TO_PROGRAM_AX(5, parameters[5206 + (p_int * 20)]) - c; + parameters[5206 + (p_int * 20)] = PROGRAM_TO_USER_AX(5, c); } else - c = USER_TO_PROGRAM_ANG(parameters[5206 + (p_int * 20)]); + c = USER_TO_PROGRAM_AX(5, parameters[5206 + (p_int * 20)]); if (block->u_flag) { u = block->u_number; - if (block->l_number == 20) u = cu + USER_TO_PROGRAM_LEN(parameters[5207 + (p_int * 20)]) - u; - parameters[5207 + (p_int * 20)] = PROGRAM_TO_USER_LEN(u); + if (block->l_number == 20) u = cu + USER_TO_PROGRAM_AX(6, parameters[5207 + (p_int * 20)]) - u; + parameters[5207 + (p_int * 20)] = PROGRAM_TO_USER_AX(6, u); } else - u = USER_TO_PROGRAM_LEN(parameters[5207 + (p_int * 20)]); + u = USER_TO_PROGRAM_AX(6, parameters[5207 + (p_int * 20)]); if (block->v_flag) { v = block->v_number; - if (block->l_number == 20) v = cv + USER_TO_PROGRAM_LEN(parameters[5208 + (p_int * 20)]) - v; - parameters[5208 + (p_int * 20)] = PROGRAM_TO_USER_LEN(v); + if (block->l_number == 20) v = cv + USER_TO_PROGRAM_AX(7, parameters[5208 + (p_int * 20)]) - v; + parameters[5208 + (p_int * 20)] = PROGRAM_TO_USER_AX(7, v); } else - v = USER_TO_PROGRAM_LEN(parameters[5208 + (p_int * 20)]); + v = USER_TO_PROGRAM_AX(7, parameters[5208 + (p_int * 20)]); if (block->w_flag) { w = block->w_number; - if (block->l_number == 20) w = cw + USER_TO_PROGRAM_LEN(parameters[5209 + (p_int * 20)]) - w; - parameters[5209 + (p_int * 20)] = PROGRAM_TO_USER_LEN(w); + if (block->l_number == 20) w = cw + USER_TO_PROGRAM_AX(8, parameters[5209 + (p_int * 20)]) - w; + parameters[5209 + (p_int * 20)] = PROGRAM_TO_USER_AX(8, w); } else - w = USER_TO_PROGRAM_LEN(parameters[5209 + (p_int * 20)]); + w = USER_TO_PROGRAM_AX(8, parameters[5209 + (p_int * 20)]); if (p_int == settings->origin_index) { /* system is currently used */ @@ -5272,6 +5318,21 @@ static void sync_move_delta(setup_pointer settings, delta[8] = w_end - settings->w_current; } +/* The letter of an axis word in the block on a ROTARY_MODULO axis, 0 if none. */ + +static char rotary_modulo_word(block_pointer block, setup_pointer settings) +{ + const bool flag[9] = {block->x_flag, block->y_flag, block->z_flag, + block->a_flag, block->b_flag, block->c_flag, + block->u_flag, block->v_flag, block->w_flag}; + for (int n = 0; n < 9; n++) { + if (flag[n] && settings->axis_rotary_modulo[n]) { + return "XYZABCUVW"[n]; + } + } + return 0; +} + /****************************************************************************/ /*! convert_stop @@ -5411,12 +5472,12 @@ int Interp::convert_stop(block_pointer block, //!< pointer to a block of RS27 settings->origin_offset_x = USER_TO_PROGRAM_LEN(settings->parameters[5221]); settings->origin_offset_y = USER_TO_PROGRAM_LEN(settings->parameters[5222]); settings->origin_offset_z = USER_TO_PROGRAM_LEN(settings->parameters[5223]); - settings->AA_origin_offset = USER_TO_PROGRAM_ANG(settings->parameters[5224]); - settings->BB_origin_offset = USER_TO_PROGRAM_ANG(settings->parameters[5225]); - settings->CC_origin_offset = USER_TO_PROGRAM_ANG(settings->parameters[5226]); - settings->u_origin_offset = USER_TO_PROGRAM_LEN(settings->parameters[5227]); - settings->v_origin_offset = USER_TO_PROGRAM_LEN(settings->parameters[5228]); - settings->w_origin_offset = USER_TO_PROGRAM_LEN(settings->parameters[5229]); + settings->AA_origin_offset = USER_TO_PROGRAM_AX(3, settings->parameters[5224]); + settings->BB_origin_offset = USER_TO_PROGRAM_AX(4, settings->parameters[5225]); + settings->CC_origin_offset = USER_TO_PROGRAM_AX(5, settings->parameters[5226]); + settings->u_origin_offset = USER_TO_PROGRAM_AX(6, settings->parameters[5227]); + settings->v_origin_offset = USER_TO_PROGRAM_AX(7, settings->parameters[5228]); + settings->w_origin_offset = USER_TO_PROGRAM_AX(8, settings->parameters[5229]); settings->rotation_xy = settings->parameters[5230]; settings->current_x -= settings->origin_offset_x; @@ -5682,12 +5743,9 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 (_("Invalid spindle ($) number in G33 move"))); settings->active_spindle = (int)block->dollar_number; } - CHKS((block->a_flag && settings->a_rotary_modulo), - _("G33 incompatible with ROTARY_MODULO on axis A")); - CHKS((block->b_flag && settings->b_rotary_modulo), - _("G33 incompatible with ROTARY_MODULO on axis B")); - CHKS((block->c_flag && settings->c_rotary_modulo), - _("G33 incompatible with ROTARY_MODULO on axis C")); + CHKS((rotary_modulo_word(block, settings)), + _("G33 incompatible with ROTARY_MODULO on axis %c"), + rotary_modulo_word(block, settings)); CHKS(((settings->spindle_turning[settings->active_spindle] != CANON_CLOCKWISE) && (settings->spindle_turning[settings->active_spindle] != CANON_COUNTERCLOCKWISE)), _("Spindle not turning in G33")); @@ -5710,12 +5768,9 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 (_("Invalid spindle ($) number in G33.1 move"))); settings->active_spindle = (int)block->dollar_number; } - CHKS((block->a_flag && settings->a_rotary_modulo), - _("G33.1 incompatible with ROTARY_MODULO on axis A")); - CHKS((block->b_flag && settings->b_rotary_modulo), - _("G33.1 incompatible with ROTARY_MODULO on axis B")); - CHKS((block->c_flag && settings->c_rotary_modulo), - _("G33.1 incompatible with ROTARY_MODULO on axis C")); + CHKS((rotary_modulo_word(block, settings)), + _("G33.1 incompatible with ROTARY_MODULO on axis %c"), + rotary_modulo_word(block, settings)); CHKS(((settings->spindle_turning[settings->active_spindle] != CANON_CLOCKWISE) && (settings->spindle_turning[settings->active_spindle] != CANON_COUNTERCLOCKWISE)), _("Spindle not turning in G33.1")); @@ -5742,12 +5797,9 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 (_("Invalid D-number in G76 cycle"))); settings->active_spindle = (int)block->dollar_number; } - CHKS((block->a_flag && settings->a_rotary_modulo), - _("G76 incompatible with ROTARY_MODULO on axis A")); - CHKS((block->b_flag && settings->b_rotary_modulo), - _("G76 incompatible with ROTARY_MODULO on axis B")); - CHKS((block->c_flag && settings->c_rotary_modulo), - _("G76 incompatible with ROTARY_MODULO on axis C")); + CHKS((rotary_modulo_word(block, settings)), + _("G76 incompatible with ROTARY_MODULO on axis %c"), + rotary_modulo_word(block, settings)); CHKS(((settings->spindle_turning[settings->active_spindle] != CANON_CLOCKWISE) && (settings->spindle_turning[settings->active_spindle] != CANON_COUNTERCLOCKWISE)), _("Chosen spindle (%i) not turning in G76"), settings->active_spindle); @@ -5772,37 +5824,20 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 } int Interp::convert_straight_indexer(int axis, int jnum, block_pointer block, setup_pointer settings) { - double end_x, end_y, end_z; - double AA_end, BB_end, CC_end; - double u_end, v_end, w_end; - - find_ends(block, settings, &end_x, &end_y, &end_z, - &AA_end, &BB_end, &CC_end, &u_end, &v_end, &w_end); - - CHKS((end_x != settings->current_x || - end_y != settings->current_y || - end_z != settings->current_z || - u_end != settings->u_current || - v_end != settings->v_current || - w_end != settings->w_current || - (axis != 3 && AA_end != settings->AA_current) || - (axis != 4 && BB_end != settings->BB_current) || - (axis != 5 && CC_end != settings->CC_current)), - _("BUG: An axis incorrectly moved along with an indexer")); - - switch(axis) { - case 3: - issue_straight_index(axis, jnum, AA_end, block->line_number, settings); - break; - case 4: - issue_straight_index(axis, jnum, BB_end, block->line_number, settings); - break; - case 5: - issue_straight_index(axis, jnum, CC_end, block->line_number, settings); - break; - default: - ERS((_("BUG: trying to index incorrect axis"))); + double end[9]; + + find_ends(block, settings, &end[0], &end[1], &end[2], + &end[3], &end[4], &end[5], &end[6], &end[7], &end[8]); + + const double current[9] = {settings->current_x, settings->current_y, settings->current_z, + settings->AA_current, settings->BB_current, settings->CC_current, + settings->u_current, settings->v_current, settings->w_current}; + CHKS((axis < 3 || axis > 8), (_("BUG: trying to index incorrect axis"))); + for (int n = 0; n < 9; n++) { + CHKS((n != axis && end[n] != current[n]), + _("BUG: An axis incorrectly moved along with an indexer")); } + issue_straight_index(axis, jnum, end[axis], block->line_number, settings); return INTERP_OK; } @@ -5816,15 +5851,16 @@ int Interp::issue_straight_index(int axis, int jnum, double target, int lineno, if (save_mode != CANON_EXACT_PATH) SET_MOTION_CONTROL_MODE(CANON_EXACT_PATH, 0); - double AA_end = axis == 3? target: settings->AA_current; - double BB_end = axis == 4? target: settings->BB_current; - double CC_end = axis == 5? target: settings->CC_current; + double end[9] = {settings->current_x, settings->current_y, settings->current_z, + settings->AA_current, settings->BB_current, settings->CC_current, + settings->u_current, settings->v_current, settings->w_current}; + end[axis] = target; // tell canon that this is a special indexing move UNLOCK_ROTARY(lineno, jnum); - STRAIGHT_TRAVERSE(lineno, settings->current_x, settings->current_y, settings->current_z, - AA_end, BB_end, CC_end, - settings->u_current, settings->v_current, settings->w_current); + STRAIGHT_TRAVERSE(lineno, end[0], end[1], end[2], + end[3], end[4], end[5], + end[6], end[7], end[8]); LOCK_ROTARY(lineno, jnum); // restore path mode @@ -5833,9 +5869,12 @@ int Interp::issue_straight_index(int axis, int jnum, double target, int lineno, SET_NAIVECAM_TOLERANCE(save_cam_tolerance); } - settings->AA_current = AA_end; - settings->BB_current = BB_end; - settings->CC_current = CC_end; + settings->AA_current = end[3]; + settings->BB_current = end[4]; + settings->CC_current = end[5]; + settings->u_current = end[6]; + settings->v_current = end[7]; + settings->w_current = end[8]; return INTERP_OK; } @@ -6452,12 +6491,12 @@ int Interp::convert_tool_change(setup_pointer settings) //!< pointer to machine find_relative(USER_TO_PROGRAM_LEN(settings->parameters[5181]), USER_TO_PROGRAM_LEN(settings->parameters[5182]), USER_TO_PROGRAM_LEN(settings->parameters[5183]), - USER_TO_PROGRAM_ANG(settings->parameters[5184]), - USER_TO_PROGRAM_ANG(settings->parameters[5185]), - USER_TO_PROGRAM_ANG(settings->parameters[5186]), - USER_TO_PROGRAM_LEN(settings->parameters[5187]), - USER_TO_PROGRAM_LEN(settings->parameters[5188]), - USER_TO_PROGRAM_LEN(settings->parameters[5189]), + USER_TO_PROGRAM_AX(3, settings->parameters[5184]), + USER_TO_PROGRAM_AX(4, settings->parameters[5185]), + USER_TO_PROGRAM_AX(5, settings->parameters[5186]), + USER_TO_PROGRAM_AX(6, settings->parameters[5187]), + USER_TO_PROGRAM_AX(7, settings->parameters[5188]), + USER_TO_PROGRAM_AX(8, settings->parameters[5189]), &end_x, &end_y, &end_z, &AA_end, &BB_end, &CC_end, &u_end, &v_end, &w_end, settings); @@ -6465,12 +6504,18 @@ int Interp::convert_tool_change(setup_pointer settings) //!< pointer to machine // move indexers first, one at a time // JOINTS_AXES settings->*_indexer_jnum == -1 means notused - if (AA_end != settings->AA_current && (-1 != settings->a_indexer_jnum) ) - issue_straight_index(3,settings->a_indexer_jnum, AA_end, -1, settings); - if (BB_end != settings->BB_current && (-1 != settings->b_indexer_jnum) ) - issue_straight_index(4,settings->b_indexer_jnum, BB_end, -1, settings); - if (CC_end != settings->CC_current && (-1 != settings->c_indexer_jnum) ) - issue_straight_index(5,settings->c_indexer_jnum, CC_end, -1, settings); + if (AA_end != settings->AA_current && (-1 != settings->axis_indexer_jnum[3]) ) + issue_straight_index(3,settings->axis_indexer_jnum[3], AA_end, -1, settings); + if (BB_end != settings->BB_current && (-1 != settings->axis_indexer_jnum[4]) ) + issue_straight_index(4,settings->axis_indexer_jnum[4], BB_end, -1, settings); + if (CC_end != settings->CC_current && (-1 != settings->axis_indexer_jnum[5]) ) + issue_straight_index(5,settings->axis_indexer_jnum[5], CC_end, -1, settings); + if (u_end != settings->u_current && (-1 != settings->axis_indexer_jnum[6]) ) + issue_straight_index(6,settings->axis_indexer_jnum[6], u_end, -1, settings); + if (v_end != settings->v_current && (-1 != settings->axis_indexer_jnum[7]) ) + issue_straight_index(7,settings->axis_indexer_jnum[7], v_end, -1, settings); + if (w_end != settings->w_current && (-1 != settings->axis_indexer_jnum[8]) ) + issue_straight_index(8,settings->axis_indexer_jnum[8], w_end, -1, settings); STRAIGHT_TRAVERSE(-1, end_x, end_y, end_z, AA_end, BB_end, CC_end, @@ -6618,12 +6663,12 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu tool_offset.tran.x = USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.tran.x); tool_offset.tran.y = USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.tran.y); tool_offset.tran.z = USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.tran.z); - tool_offset.a = USER_TO_PROGRAM_ANG(settings->tool_table[idx].offset.a); - tool_offset.b = USER_TO_PROGRAM_ANG(settings->tool_table[idx].offset.b); - tool_offset.c = USER_TO_PROGRAM_ANG(settings->tool_table[idx].offset.c); - tool_offset.u = USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.u); - tool_offset.v = USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.v); - tool_offset.w = USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.w); + tool_offset.a = USER_TO_PROGRAM_AX(3, settings->tool_table[idx].offset.a); + tool_offset.b = USER_TO_PROGRAM_AX(4, settings->tool_table[idx].offset.b); + tool_offset.c = USER_TO_PROGRAM_AX(5, settings->tool_table[idx].offset.c); + tool_offset.u = USER_TO_PROGRAM_AX(6, settings->tool_table[idx].offset.u); + tool_offset.v = USER_TO_PROGRAM_AX(7, settings->tool_table[idx].offset.v); + tool_offset.w = USER_TO_PROGRAM_AX(8, settings->tool_table[idx].offset.w); settings->g43_with_zero_offset = !(tool_offset.tran.x || tool_offset.tran.y || tool_offset.tran.z || tool_offset.a || tool_offset.b || tool_offset.c || @@ -6653,12 +6698,12 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu tool_offset.tran.x += USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.tran.x); tool_offset.tran.y += USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.tran.y); tool_offset.tran.z += USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.tran.z); - tool_offset.a += USER_TO_PROGRAM_ANG(settings->tool_table[idx].offset.a); - tool_offset.b += USER_TO_PROGRAM_ANG(settings->tool_table[idx].offset.b); - tool_offset.c += USER_TO_PROGRAM_ANG(settings->tool_table[idx].offset.c); - tool_offset.u += USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.u); - tool_offset.v += USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.v); - tool_offset.w += USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.w); + tool_offset.a += USER_TO_PROGRAM_AX(3, settings->tool_table[idx].offset.a); + tool_offset.b += USER_TO_PROGRAM_AX(4, settings->tool_table[idx].offset.b); + tool_offset.c += USER_TO_PROGRAM_AX(5, settings->tool_table[idx].offset.c); + tool_offset.u += USER_TO_PROGRAM_AX(6, settings->tool_table[idx].offset.u); + tool_offset.v += USER_TO_PROGRAM_AX(7, settings->tool_table[idx].offset.v); + tool_offset.w += USER_TO_PROGRAM_AX(8, settings->tool_table[idx].offset.w); } else { if(block->x_flag) tool_offset.tran.x += block->x_number; if(block->y_flag) tool_offset.tran.y += block->y_number; @@ -6705,12 +6750,12 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu settings->parameters[5081] = PROGRAM_TO_USER_LEN(tool_offset.tran.x); settings->parameters[5082] = PROGRAM_TO_USER_LEN(tool_offset.tran.y); settings->parameters[5083] = PROGRAM_TO_USER_LEN(tool_offset.tran.z); - settings->parameters[5084] = PROGRAM_TO_USER_ANG(tool_offset.a); - settings->parameters[5085] = PROGRAM_TO_USER_ANG(tool_offset.b); - settings->parameters[5086] = PROGRAM_TO_USER_ANG(tool_offset.c); - settings->parameters[5087] = PROGRAM_TO_USER_LEN(tool_offset.u); - settings->parameters[5088] = PROGRAM_TO_USER_LEN(tool_offset.v); - settings->parameters[5089] = PROGRAM_TO_USER_LEN(tool_offset.w); + settings->parameters[5084] = PROGRAM_TO_USER_AX(3, tool_offset.a); + settings->parameters[5085] = PROGRAM_TO_USER_AX(4, tool_offset.b); + settings->parameters[5086] = PROGRAM_TO_USER_AX(5, tool_offset.c); + settings->parameters[5087] = PROGRAM_TO_USER_AX(6, tool_offset.u); + settings->parameters[5088] = PROGRAM_TO_USER_AX(7, tool_offset.v); + settings->parameters[5089] = PROGRAM_TO_USER_AX(8, tool_offset.w); if (g_code == G_49 && settings->kins_by_g43_4) { // G49 undoes what G43.4 did: after the cancel it drops the machine diff --git a/src/emc/rs274ngc/interp_find.cc b/src/emc/rs274ngc/interp_find.cc index 2eafbdcdfea..bb420f1a4db 100644 --- a/src/emc/rs274ngc/interp_find.cc +++ b/src/emc/rs274ngc/interp_find.cc @@ -217,11 +217,11 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->a_flag) { - if(s->a_axis_wrapped) { + if(s->axis_wrapped[3]) { CHP(unwrap_rotary(AA_p, block->a_number, block->a_number - s->AA_origin_offset - s->AA_axis_offset - s->tool_offset.a, s->AA_current, 'A')); - } else if (s->a_rotary_modulo) { + } else if (s->axis_rotary_modulo[3]) { *AA_p = rotary_modulo_target(block->a_number, s->AA_origin_offset + s->AA_axis_offset + s->tool_offset.a, s->AA_current, s->rotary_modulo_literal); @@ -233,11 +233,11 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->b_flag) { - if(s->b_axis_wrapped) { + if(s->axis_wrapped[4]) { CHP(unwrap_rotary(BB_p, block->b_number, block->b_number - s->BB_origin_offset - s->BB_axis_offset - s->tool_offset.b, s->BB_current, 'B')); - } else if (s->b_rotary_modulo) { + } else if (s->axis_rotary_modulo[4]) { *BB_p = rotary_modulo_target(block->b_number, s->BB_origin_offset + s->BB_axis_offset + s->tool_offset.b, s->BB_current, s->rotary_modulo_literal); @@ -249,11 +249,11 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->c_flag) { - if(s->c_axis_wrapped) { + if(s->axis_wrapped[5]) { CHP(unwrap_rotary(CC_p, block->c_number, block->c_number - s->CC_origin_offset - s->CC_axis_offset - s->tool_offset.c, s->CC_current, 'C')); - } else if (s->c_rotary_modulo) { + } else if (s->axis_rotary_modulo[5]) { *CC_p = rotary_modulo_target(block->c_number, s->CC_origin_offset + s->CC_axis_offset + s->tool_offset.c, s->CC_current, s->rotary_modulo_literal); @@ -265,19 +265,49 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->u_flag) { - *u_p = block->u_number - s->u_origin_offset - s->u_axis_offset - s->tool_offset.u; + if(s->axis_wrapped[6]) { + CHP(unwrap_rotary(u_p, block->u_number, + block->u_number - s->u_origin_offset - s->u_axis_offset - s->tool_offset.u, + s->u_current, 'U')); + } else if (s->axis_rotary_modulo[6]) { + *u_p = rotary_modulo_target(block->u_number, + s->u_origin_offset + s->u_axis_offset + s->tool_offset.u, + s->u_current, s->rotary_modulo_literal); + } else { + *u_p = block->u_number - s->u_origin_offset - s->u_axis_offset - s->tool_offset.u; + } } else { *u_p = s->u_current; } if(block->v_flag) { - *v_p = block->v_number - s->v_origin_offset - s->v_axis_offset - s->tool_offset.v; + if(s->axis_wrapped[7]) { + CHP(unwrap_rotary(v_p, block->v_number, + block->v_number - s->v_origin_offset - s->v_axis_offset - s->tool_offset.v, + s->v_current, 'V')); + } else if (s->axis_rotary_modulo[7]) { + *v_p = rotary_modulo_target(block->v_number, + s->v_origin_offset + s->v_axis_offset + s->tool_offset.v, + s->v_current, s->rotary_modulo_literal); + } else { + *v_p = block->v_number - s->v_origin_offset - s->v_axis_offset - s->tool_offset.v; + } } else { *v_p = s->v_current; } if(block->w_flag) { - *w_p = block->w_number - s->w_origin_offset - s->w_axis_offset - s->tool_offset.w; + if(s->axis_wrapped[8]) { + CHP(unwrap_rotary(w_p, block->w_number, + block->w_number - s->w_origin_offset - s->w_axis_offset - s->tool_offset.w, + s->w_current, 'W')); + } else if (s->axis_rotary_modulo[8]) { + *w_p = rotary_modulo_target(block->w_number, + s->w_origin_offset + s->w_axis_offset + s->tool_offset.w, + s->w_current, s->rotary_modulo_literal); + } else { + *w_p = block->w_number - s->w_origin_offset - s->w_axis_offset - s->tool_offset.w; + } } else { *w_p = s->w_current; } @@ -324,9 +354,9 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->a_flag) { - if(s->a_axis_wrapped) { + if(s->axis_wrapped[3]) { CHP(unwrap_rotary(AA_p, block->a_number, block->a_number, s->AA_current, 'A')); - } else if (s->a_rotary_modulo) { + } else if (s->axis_rotary_modulo[3]) { *AA_p = rotary_modulo_target(block->a_number, 0.0, s->AA_current, s->rotary_modulo_literal); } else { @@ -337,9 +367,9 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->b_flag) { - if(s->b_axis_wrapped) { + if(s->axis_wrapped[4]) { CHP(unwrap_rotary(BB_p, block->b_number, block->b_number, s->BB_current, 'B')); - } else if (s->b_rotary_modulo) { + } else if (s->axis_rotary_modulo[4]) { *BB_p = rotary_modulo_target(block->b_number, 0.0, s->BB_current, s->rotary_modulo_literal); } else { @@ -350,9 +380,9 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->c_flag) { - if(s->c_axis_wrapped) { + if(s->axis_wrapped[5]) { CHP(unwrap_rotary(CC_p, block->c_number, block->c_number, s->CC_current, 'C')); - } else if (s->c_rotary_modulo) { + } else if (s->axis_rotary_modulo[5]) { *CC_p = rotary_modulo_target(block->c_number, 0.0, s->CC_current, s->rotary_modulo_literal); } else { @@ -362,9 +392,42 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 *CC_p = s->CC_current; } - *u_p = (block->u_flag) ? block->u_number : s->u_current; - *v_p = (block->v_flag) ? block->v_number : s->v_current; - *w_p = (block->w_flag) ? block->w_number : s->w_current; + if(block->u_flag) { + if(s->axis_wrapped[6]) { + CHP(unwrap_rotary(u_p, block->u_number, block->u_number, s->u_current, 'U')); + } else if (s->axis_rotary_modulo[6]) { + *u_p = rotary_modulo_target(block->u_number, 0.0, s->u_current, + s->rotary_modulo_literal); + } else { + *u_p = block->u_number; + } + } else { + *u_p = s->u_current; + } + if(block->v_flag) { + if(s->axis_wrapped[7]) { + CHP(unwrap_rotary(v_p, block->v_number, block->v_number, s->v_current, 'V')); + } else if (s->axis_rotary_modulo[7]) { + *v_p = rotary_modulo_target(block->v_number, 0.0, s->v_current, + s->rotary_modulo_literal); + } else { + *v_p = block->v_number; + } + } else { + *v_p = s->v_current; + } + if(block->w_flag) { + if(s->axis_wrapped[8]) { + CHP(unwrap_rotary(w_p, block->w_number, block->w_number, s->w_current, 'W')); + } else if (s->axis_rotary_modulo[8]) { + *w_p = rotary_modulo_target(block->w_number, 0.0, s->w_current, + s->rotary_modulo_literal); + } else { + *w_p = block->w_number; + } + } else { + *w_p = s->w_current; + } } else { /* mode is DISTANCE_MODE::INCREMENTAL */ @@ -470,11 +533,11 @@ int Interp::find_relative(double x1, //!< absolute x position *y2 -= settings->axis_offset_y; *z2 = z1 - settings->origin_offset_z - settings->axis_offset_z - settings->tool_offset.tran.z; - if(settings->a_axis_wrapped) { + if(settings->axis_wrapped[3]) { CHP(unwrap_rotary(AA_2, AA_1, AA_1 - settings->AA_origin_offset - settings->AA_axis_offset - settings->tool_offset.a, settings->AA_current, 'A')); - } else if (settings->a_rotary_modulo) { + } else if (settings->axis_rotary_modulo[3]) { // stored positions carry no programmed sign for M27 to read a direction // from, so G28/G30/tool change always take the shortest path *AA_2 = rotary_modulo_target(AA_1, @@ -484,11 +547,11 @@ int Interp::find_relative(double x1, //!< absolute x position *AA_2 = AA_1 - settings->AA_origin_offset - settings->AA_axis_offset - settings->tool_offset.a; } - if(settings->b_axis_wrapped) { + if(settings->axis_wrapped[4]) { CHP(unwrap_rotary(BB_2, BB_1, BB_1 - settings->BB_origin_offset - settings->BB_axis_offset - settings->tool_offset.b, settings->BB_current, 'B')); - } else if (settings->b_rotary_modulo) { + } else if (settings->axis_rotary_modulo[4]) { *BB_2 = rotary_modulo_target(BB_1, settings->BB_origin_offset + settings->BB_axis_offset + settings->tool_offset.b, settings->BB_current, 0); @@ -496,11 +559,11 @@ int Interp::find_relative(double x1, //!< absolute x position *BB_2 = BB_1 - settings->BB_origin_offset - settings->BB_axis_offset - settings->tool_offset.b; } - if(settings->c_axis_wrapped) { + if(settings->axis_wrapped[5]) { CHP(unwrap_rotary(CC_2, CC_1, CC_1 - settings->CC_origin_offset - settings->CC_axis_offset - settings->tool_offset.c, settings->CC_current, 'C')); - } else if (settings->c_rotary_modulo) { + } else if (settings->axis_rotary_modulo[5]) { *CC_2 = rotary_modulo_target(CC_1, settings->CC_origin_offset + settings->CC_axis_offset + settings->tool_offset.c, settings->CC_current, 0); @@ -508,9 +571,41 @@ int Interp::find_relative(double x1, //!< absolute x position *CC_2 = CC_1 - settings->CC_origin_offset - settings->CC_axis_offset - settings->tool_offset.c; } - *u_2 = u_1 - settings->u_origin_offset - settings->u_axis_offset - settings->tool_offset.u; - *v_2 = v_1 - settings->v_origin_offset - settings->v_axis_offset - settings->tool_offset.v; - *w_2 = w_1 - settings->w_origin_offset - settings->w_axis_offset - settings->tool_offset.w; + if(settings->axis_wrapped[6]) { + CHP(unwrap_rotary(u_2, u_1, + u_1 - settings->u_origin_offset - settings->u_axis_offset - settings->tool_offset.u, + settings->u_current, 'U')); + } else if (settings->axis_rotary_modulo[6]) { + *u_2 = rotary_modulo_target(u_1, + settings->u_origin_offset + settings->u_axis_offset + settings->tool_offset.u, + settings->u_current, 0); + } else { + *u_2 = u_1 - settings->u_origin_offset - settings->u_axis_offset - settings->tool_offset.u; + } + + if(settings->axis_wrapped[7]) { + CHP(unwrap_rotary(v_2, v_1, + v_1 - settings->v_origin_offset - settings->v_axis_offset - settings->tool_offset.v, + settings->v_current, 'V')); + } else if (settings->axis_rotary_modulo[7]) { + *v_2 = rotary_modulo_target(v_1, + settings->v_origin_offset + settings->v_axis_offset + settings->tool_offset.v, + settings->v_current, 0); + } else { + *v_2 = v_1 - settings->v_origin_offset - settings->v_axis_offset - settings->tool_offset.v; + } + + if(settings->axis_wrapped[8]) { + CHP(unwrap_rotary(w_2, w_1, + w_1 - settings->w_origin_offset - settings->w_axis_offset - settings->tool_offset.w, + settings->w_current, 'W')); + } else if (settings->axis_rotary_modulo[8]) { + *w_2 = rotary_modulo_target(w_1, + settings->w_origin_offset + settings->w_axis_offset + settings->tool_offset.w, + settings->w_current, 0); + } else { + *w_2 = w_1 - settings->w_origin_offset - settings->w_axis_offset - settings->tool_offset.w; + } return INTERP_OK; } @@ -557,12 +652,12 @@ int Interp::find_current_in_system(setup_pointer s, int system, *x -= USER_TO_PROGRAM_LEN(p[5201 + system * 20]); *y -= USER_TO_PROGRAM_LEN(p[5202 + system * 20]); *z -= USER_TO_PROGRAM_LEN(p[5203 + system * 20]); - *a -= USER_TO_PROGRAM_ANG(p[5204 + system * 20]); - *b -= USER_TO_PROGRAM_ANG(p[5205 + system * 20]); - *c -= USER_TO_PROGRAM_ANG(p[5206 + system * 20]); - *u -= USER_TO_PROGRAM_LEN(p[5207 + system * 20]); - *v -= USER_TO_PROGRAM_LEN(p[5208 + system * 20]); - *w -= USER_TO_PROGRAM_LEN(p[5209 + system * 20]); + *a -= USER_TO_PROGRAM_AX(3, p[5204 + system * 20]); + *b -= USER_TO_PROGRAM_AX(4, p[5205 + system * 20]); + *c -= USER_TO_PROGRAM_AX(5, p[5206 + system * 20]); + *u -= USER_TO_PROGRAM_AX(6, p[5207 + system * 20]); + *v -= USER_TO_PROGRAM_AX(7, p[5208 + system * 20]); + *w -= USER_TO_PROGRAM_AX(8, p[5209 + system * 20]); rotate(x, y, -p[5210 + system * 20]); @@ -570,12 +665,12 @@ int Interp::find_current_in_system(setup_pointer s, int system, *x -= USER_TO_PROGRAM_LEN(p[5211]); *y -= USER_TO_PROGRAM_LEN(p[5212]); *z -= USER_TO_PROGRAM_LEN(p[5213]); - *a -= USER_TO_PROGRAM_ANG(p[5214]); - *b -= USER_TO_PROGRAM_ANG(p[5215]); - *c -= USER_TO_PROGRAM_ANG(p[5216]); - *u -= USER_TO_PROGRAM_LEN(p[5217]); - *v -= USER_TO_PROGRAM_LEN(p[5218]); - *w -= USER_TO_PROGRAM_LEN(p[5219]); + *a -= USER_TO_PROGRAM_AX(3, p[5214]); + *b -= USER_TO_PROGRAM_AX(4, p[5215]); + *c -= USER_TO_PROGRAM_AX(5, p[5216]); + *u -= USER_TO_PROGRAM_AX(6, p[5217]); + *v -= USER_TO_PROGRAM_AX(7, p[5218]); + *w -= USER_TO_PROGRAM_AX(8, p[5219]); } return INTERP_OK; @@ -636,12 +731,12 @@ int Interp::find_current_in_system_without_tlo(setup_pointer s, int system, *x -= USER_TO_PROGRAM_LEN(p[5201 + system * 20]); *y -= USER_TO_PROGRAM_LEN(p[5202 + system * 20]); *z -= USER_TO_PROGRAM_LEN(p[5203 + system * 20]); - *a -= USER_TO_PROGRAM_ANG(p[5204 + system * 20]); - *b -= USER_TO_PROGRAM_ANG(p[5205 + system * 20]); - *c -= USER_TO_PROGRAM_ANG(p[5206 + system * 20]); - *u -= USER_TO_PROGRAM_LEN(p[5207 + system * 20]); - *v -= USER_TO_PROGRAM_LEN(p[5208 + system * 20]); - *w -= USER_TO_PROGRAM_LEN(p[5209 + system * 20]); + *a -= USER_TO_PROGRAM_AX(3, p[5204 + system * 20]); + *b -= USER_TO_PROGRAM_AX(4, p[5205 + system * 20]); + *c -= USER_TO_PROGRAM_AX(5, p[5206 + system * 20]); + *u -= USER_TO_PROGRAM_AX(6, p[5207 + system * 20]); + *v -= USER_TO_PROGRAM_AX(7, p[5208 + system * 20]); + *w -= USER_TO_PROGRAM_AX(8, p[5209 + system * 20]); rotate(x, y, -p[5210 + system * 20]); @@ -649,12 +744,12 @@ int Interp::find_current_in_system_without_tlo(setup_pointer s, int system, *x -= USER_TO_PROGRAM_LEN(p[5211]); *y -= USER_TO_PROGRAM_LEN(p[5212]); *z -= USER_TO_PROGRAM_LEN(p[5213]); - *a -= USER_TO_PROGRAM_ANG(p[5214]); - *b -= USER_TO_PROGRAM_ANG(p[5215]); - *c -= USER_TO_PROGRAM_ANG(p[5216]); - *u -= USER_TO_PROGRAM_LEN(p[5217]); - *v -= USER_TO_PROGRAM_LEN(p[5218]); - *w -= USER_TO_PROGRAM_LEN(p[5219]); + *a -= USER_TO_PROGRAM_AX(3, p[5214]); + *b -= USER_TO_PROGRAM_AX(4, p[5215]); + *c -= USER_TO_PROGRAM_AX(5, p[5216]); + *u -= USER_TO_PROGRAM_AX(6, p[5217]); + *v -= USER_TO_PROGRAM_AX(7, p[5218]); + *w -= USER_TO_PROGRAM_AX(8, p[5219]); } return INTERP_OK; @@ -714,12 +809,16 @@ double Interp::find_straight_length(double x2, //!< X-coordinate of end point ) { #define tiny 1e-7 - if ( (fabs(x1-x2) > tiny) || (fabs(y1-y2) > tiny) || (fabs(z1-z2) > tiny) ) - return sqrt(pow((x2 - x1), 2) + pow((y2 - y1), 2) + pow((z2 - z1), 2)); - else if ( (fabs(u_1-u_2) > tiny) || (fabs(v_1-v_2) > tiny) || (fabs(w_1-w_2) > tiny) ) - return sqrt(pow((u_2 - u_1), 2) + pow((v_2 - v_1), 2) + pow((w_2 - w_1), 2)); - else - return sqrt(pow((AA_2 - AA_1), 2) + pow((BB_2 - BB_1), 2) + pow((CC_2 - CC_1), 2)); + // along the feed group when it moves, else the other linear axes, else + // the angular ones: XYZ, else UVW, else ABC with the default axis types + const double d[9] = {x2 - x1, y2 - y1, z2 - z1, + AA_2 - AA_1, BB_2 - BB_1, CC_2 - CC_1, + u_2 - u_1, v_2 - v_1, w_2 - w_1}; + unsigned moving = 0; + for (int n = 0; n < 9; n++) { + if (fabs(d[n]) > tiny) { moving |= 1u << n; } + } + return axisKindsLength(axisKindsMeasured(_setup.axis_kinds, moving), d); } /****************************************************************************/ diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index ff222678177..3b7749a62c4 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -441,33 +441,47 @@ Called by: Interp::read int Interp::set_probe_data(setup_pointer settings) //!< pointer to machine settings { - double a, b, c; + double a, b, c, u, v, w; refresh_actual_position(settings); settings->parameters[5061] = GET_EXTERNAL_PROBE_POSITION_X(); settings->parameters[5062] = GET_EXTERNAL_PROBE_POSITION_Y(); settings->parameters[5063] = GET_EXTERNAL_PROBE_POSITION_Z(); a = GET_EXTERNAL_PROBE_POSITION_A(); - if(settings->a_axis_wrapped || settings->a_rotary_modulo) { + if(settings->axis_wrapped[3] || settings->axis_rotary_modulo[3]) { a = wrap_rotary_to_360(a); } settings->parameters[5064] = a; b = GET_EXTERNAL_PROBE_POSITION_B(); - if(settings->b_axis_wrapped || settings->b_rotary_modulo) { + if(settings->axis_wrapped[4] || settings->axis_rotary_modulo[4]) { b = wrap_rotary_to_360(b); } settings->parameters[5065] = b; c = GET_EXTERNAL_PROBE_POSITION_C(); - if(settings->c_axis_wrapped || settings->c_rotary_modulo) { + if(settings->axis_wrapped[5] || settings->axis_rotary_modulo[5]) { c = wrap_rotary_to_360(c); } settings->parameters[5066] = c; - settings->parameters[5067] = GET_EXTERNAL_PROBE_POSITION_U(); - settings->parameters[5068] = GET_EXTERNAL_PROBE_POSITION_V(); - settings->parameters[5069] = GET_EXTERNAL_PROBE_POSITION_W(); + u = GET_EXTERNAL_PROBE_POSITION_U(); + if(settings->axis_wrapped[6] || settings->axis_rotary_modulo[6]) { + u = wrap_rotary_to_360(u); + } + settings->parameters[5067] = u; + + v = GET_EXTERNAL_PROBE_POSITION_V(); + if(settings->axis_wrapped[7] || settings->axis_rotary_modulo[7]) { + v = wrap_rotary_to_360(v); + } + settings->parameters[5068] = v; + + w = GET_EXTERNAL_PROBE_POSITION_W(); + if(settings->axis_wrapped[8] || settings->axis_rotary_modulo[8]) { + w = wrap_rotary_to_360(w); + } + settings->parameters[5069] = w; settings->parameters[5070] = (double) GET_EXTERNAL_PROBE_TRIPPED_VALUE(); // was an undocumented feature?: settings->parameters[5067] = GET_EXTERNAL_PROBE_VALUE(); diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 6a987ba97eb..55bfa138f4d 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -31,6 +31,7 @@ #include "interp_fwd.hh" #include "interp_base.hh" #include "tooldata/tooldata.hh" +#include #define _(s) gettext(s) @@ -858,17 +859,11 @@ struct setup int tool_change_with_spindle_on; double parameter_g73_peck_clearance; double parameter_g83_peck_clearance; - int a_axis_wrapped; - int b_axis_wrapped; - int c_axis_wrapped; - int a_rotary_modulo; - int b_rotary_modulo; - int c_rotary_modulo; + AxisKinds axis_kinds; // [AXIS_] TYPE, [TRAJ] FEED_AXES + int axis_wrapped[9]; // per axis, X 0 to W 8; angular axes only + int axis_rotary_modulo[9]; // angular axes only int rotary_modulo_literal; // M26 = shortest path (default), M27 = literal absolute - - int a_indexer_jnum; - int b_indexer_jnum; - int c_indexer_jnum; + int axis_indexer_jnum[9]; // -1 where the axis has no locking indexer bool lathe_diameter_mode; //Lathe diameter mode (g07/G08) bool mdi_interrupt; diff --git a/src/emc/rs274ngc/interp_namedparams.cc b/src/emc/rs274ngc/interp_namedparams.cc index 18d8ed9ed49..131f4e2e3c8 100644 --- a/src/emc/rs274ngc/interp_namedparams.cc +++ b/src/emc/rs274ngc/interp_namedparams.cc @@ -732,30 +732,33 @@ int Interp::lookup_named_param(const char *nameBuf, break; case NP_A: // current position - *value = _setup.a_rotary_modulo + *value = _setup.axis_rotary_modulo[3] ? wrap_rotary_to_360(_setup.AA_current) : _setup.AA_current; break; case NP_B: // current position - *value = _setup.b_rotary_modulo + *value = _setup.axis_rotary_modulo[4] ? wrap_rotary_to_360(_setup.BB_current) : _setup.BB_current; break; case NP_C: // current position - *value = _setup.c_rotary_modulo + *value = _setup.axis_rotary_modulo[5] ? wrap_rotary_to_360(_setup.CC_current) : _setup.CC_current; break; case NP_U: // current position - *value = _setup.u_current; + *value = _setup.axis_rotary_modulo[6] + ? wrap_rotary_to_360(_setup.u_current) : _setup.u_current; break; case NP_V: // current position - *value = _setup.v_current; + *value = _setup.axis_rotary_modulo[7] + ? wrap_rotary_to_360(_setup.v_current) : _setup.v_current; break; case NP_W: // current position - *value = _setup.w_current; + *value = _setup.axis_rotary_modulo[8] + ? wrap_rotary_to_360(_setup.w_current) : _setup.w_current; break; case NP_ABS_X: // abs position @@ -786,7 +789,7 @@ int Interp::lookup_named_param(const char *nameBuf, { double v = _setup.AA_current + _setup.AA_axis_offset + _setup.AA_origin_offset + _setup.tool_offset.a; - *value = _setup.a_rotary_modulo ? wrap_rotary_to_360(v) : v; + *value = _setup.axis_rotary_modulo[3] ? wrap_rotary_to_360(v) : v; } break; @@ -794,7 +797,7 @@ int Interp::lookup_named_param(const char *nameBuf, { double v = _setup.BB_current + _setup.BB_axis_offset + _setup.BB_origin_offset + _setup.tool_offset.b; - *value = _setup.b_rotary_modulo ? wrap_rotary_to_360(v) : v; + *value = _setup.axis_rotary_modulo[4] ? wrap_rotary_to_360(v) : v; } break; @@ -802,23 +805,32 @@ int Interp::lookup_named_param(const char *nameBuf, { double v = _setup.CC_current + _setup.CC_axis_offset + _setup.CC_origin_offset + _setup.tool_offset.c; - *value = _setup.c_rotary_modulo ? wrap_rotary_to_360(v) : v; + *value = _setup.axis_rotary_modulo[5] ? wrap_rotary_to_360(v) : v; } break; case NP_ABS_U: // abs position - *value = _setup.u_current + _setup.u_axis_offset + - _setup.u_origin_offset + _setup.tool_offset.u; + { + double v = _setup.u_current + _setup.u_axis_offset + + _setup.u_origin_offset + _setup.tool_offset.u; + *value = _setup.axis_rotary_modulo[6] ? wrap_rotary_to_360(v) : v; + } break; case NP_ABS_V: // abs position - *value = _setup.v_current + _setup.v_axis_offset + - _setup.v_origin_offset + _setup.tool_offset.v; + { + double v = _setup.v_current + _setup.v_axis_offset + + _setup.v_origin_offset + _setup.tool_offset.v; + *value = _setup.axis_rotary_modulo[7] ? wrap_rotary_to_360(v) : v; + } break; case NP_ABS_W: // abs position - *value = _setup.w_current + _setup.w_axis_offset + - _setup.w_origin_offset + _setup.tool_offset.w; + { + double v = _setup.w_current + _setup.w_axis_offset + + _setup.w_origin_offset + _setup.tool_offset.w; + *value = _setup.axis_rotary_modulo[8] ? wrap_rotary_to_360(v) : v; + } break; // o-word subs may optionally have an diff --git a/src/emc/rs274ngc/interp_queue.cc b/src/emc/rs274ngc/interp_queue.cc index 921450caf45..ee4ccde31d6 100644 --- a/src/emc/rs274ngc/interp_queue.cc +++ b/src/emc/rs274ngc/interp_queue.cc @@ -385,7 +385,16 @@ void enqueue_M_USER_COMMAND (int index, double p_number, double q_number) { qc().push_back(q); } -void qc_scale(double scale) { +// the axes past X Y Z that are lengths, as their [AXIS_] TYPE says +static void scale_linear(const AxisKinds &kinds, double &a, double &b, double &c, + double &u, double &v, double &w, double scale) { + double *axis[6] = {&a, &b, &c, &u, &v, &w}; + for (int n = 0; n < 6; n++) { + if (!axisKindsAngular(kinds, n + 3)) { *axis[n] *= scale; } + } +} + +void qc_scale(double scale, const AxisKinds &kinds) { if(qc().empty()) { if(debug_qc) printf("not scaling because qc is empty\n"); @@ -405,25 +414,22 @@ void qc_scale(double scale) { q.data.arc_feed.end3 *= scale; q.data.arc_feed.center1 *= scale; q.data.arc_feed.center2 *= scale; - q.data.arc_feed.u *= scale; - q.data.arc_feed.v *= scale; - q.data.arc_feed.w *= scale; + scale_linear(kinds, q.data.arc_feed.a, q.data.arc_feed.b, q.data.arc_feed.c, + q.data.arc_feed.u, q.data.arc_feed.v, q.data.arc_feed.w, scale); break; case QSTRAIGHT_FEED: q.data.straight_feed.x *= scale; q.data.straight_feed.y *= scale; q.data.straight_feed.z *= scale; - q.data.straight_feed.u *= scale; - q.data.straight_feed.v *= scale; - q.data.straight_feed.w *= scale; + scale_linear(kinds, q.data.straight_feed.a, q.data.straight_feed.b, q.data.straight_feed.c, + q.data.straight_feed.u, q.data.straight_feed.v, q.data.straight_feed.w, scale); break; case QSTRAIGHT_TRAVERSE: q.data.straight_traverse.x *= scale; q.data.straight_traverse.y *= scale; q.data.straight_traverse.z *= scale; - q.data.straight_traverse.u *= scale; - q.data.straight_traverse.v *= scale; - q.data.straight_traverse.w *= scale; + scale_linear(kinds, q.data.straight_traverse.a, q.data.straight_traverse.b, q.data.straight_traverse.c, + q.data.straight_traverse.u, q.data.straight_traverse.v, q.data.straight_traverse.w, scale); break; default: ; diff --git a/src/emc/rs274ngc/interp_queue.hh b/src/emc/rs274ngc/interp_queue.hh index bb203411d9c..e1706955b8c 100644 --- a/src/emc/rs274ngc/interp_queue.hh +++ b/src/emc/rs274ngc/interp_queue.hh @@ -144,6 +144,6 @@ void set_endpoint(double x, double y); void set_endpoint_zx(double z, double x); int move_endpoint_and_flush(setup_pointer settings, double x, double y); void qc_reset(void); -void qc_scale(double scale); +void qc_scale(double scale, const AxisKinds &kinds); #endif diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 326969df7a0..7ac607b8edc 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -181,17 +181,11 @@ setup::setup() : tool_change_with_spindle_on(0), parameter_g73_peck_clearance(0.0), parameter_g83_peck_clearance(0.0), - a_axis_wrapped(0), - b_axis_wrapped(0), - c_axis_wrapped(0), - a_rotary_modulo(0), - b_rotary_modulo(0), - c_rotary_modulo(0), + axis_kinds(axisKindsDefault()), + axis_wrapped{}, + axis_rotary_modulo{}, rotary_modulo_literal(0), - - a_indexer_jnum(0), - b_indexer_jnum(0), - c_indexer_jnum(0), + axis_indexer_jnum{-1, -1, -1, -1, -1, -1, -1, -1, -1}, lathe_diameter_mode(0), mdi_interrupt(0), diff --git a/src/emc/rs274ngc/interpmodule.cc b/src/emc/rs274ngc/interpmodule.cc index b44125eb365..5c4172b9492 100644 --- a/src/emc/rs274ngc/interpmodule.cc +++ b/src/emc/rs274ngc/interpmodule.cc @@ -589,40 +589,40 @@ static inline void set_w_origin_offset(Interp &interp, double value) { interp._setup.w_origin_offset = value; } static inline int get_a_axis_wrapped (Interp &interp) { - return interp._setup.a_axis_wrapped; + return interp._setup.axis_wrapped[3]; } static inline void set_a_axis_wrapped(Interp &interp, int value) { - interp._setup.a_axis_wrapped = value; + interp._setup.axis_wrapped[3] = value; } static inline int get_a_indexer (Interp &interp) { - return interp._setup.a_indexer_jnum; + return interp._setup.axis_indexer_jnum[3]; } static inline void set_a_indexer(Interp &interp, int value) { - interp._setup.a_indexer_jnum = value; + interp._setup.axis_indexer_jnum[3] = value; } static inline int get_b_axis_wrapped (Interp &interp) { - return interp._setup.b_axis_wrapped; + return interp._setup.axis_wrapped[4]; } static inline void set_b_axis_wrapped(Interp &interp, int value) { - interp._setup.b_axis_wrapped = value; + interp._setup.axis_wrapped[4] = value; } static inline int get_b_indexer (Interp &interp) { - return interp._setup.b_indexer_jnum; + return interp._setup.axis_indexer_jnum[4]; } static inline void set_b_indexer(Interp &interp, int value) { - interp._setup.b_indexer_jnum = value; + interp._setup.axis_indexer_jnum[4] = value; } static inline int get_c_axis_wrapped (Interp &interp) { - return interp._setup.c_axis_wrapped; + return interp._setup.axis_wrapped[5]; } static inline void set_c_axis_wrapped(Interp &interp, int value) { - interp._setup.c_axis_wrapped = value; + interp._setup.axis_wrapped[5] = value; } static inline int get_c_indexer (Interp &interp) { - return interp._setup.c_indexer_jnum; + return interp._setup.axis_indexer_jnum[5]; } static inline void set_c_indexer(Interp &interp, int value) { - interp._setup.c_indexer_jnum = value; + interp._setup.axis_indexer_jnum[5] = value; } static inline int get_call_level (Interp &interp) { return interp._setup.call_level; diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index 8196bf70fb4..8be056791cf 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -854,17 +854,14 @@ int Interp::init() _setup.parameter_g73_peck_clearance = 1; _setup.parameter_g83_peck_clearance = 1; } - _setup.a_axis_wrapped = 0; - _setup.b_axis_wrapped = 0; - _setup.c_axis_wrapped = 0; - _setup.a_rotary_modulo = 0; - _setup.b_rotary_modulo = 0; - _setup.c_rotary_modulo = 0; + _setup.axis_kinds = axisKindsDefault(); + for (int n = 0; n < 9; n++) { + _setup.axis_wrapped[n] = 0; + _setup.axis_rotary_modulo[n] = 0; + _setup.axis_indexer_jnum[n] = -1; // -1 means not used + } _setup.rotary_modulo_literal = 0; _setup.random_toolchanger = 0; - _setup.a_indexer_jnum = -1; // -1 means not used - _setup.b_indexer_jnum = -1; // -1 means not used - _setup.c_indexer_jnum = -1; // -1 means not used _setup.return_value = 0; _setup.value_returned = 0; _setup.remap_level = 0; // remapped blocks stack index @@ -887,41 +884,41 @@ int Interp::init() _setup.tool_change_at_g30 = inifile.findBoolV("TOOL_CHANGE_AT_G30", "EMCIO", false); _setup.tool_change_quill_up = inifile.findBoolV("TOOL_CHANGE_QUILL_UP", "EMCIO", false); _setup.tool_change_with_spindle_on = inifile.findBoolV("TOOL_CHANGE_WITH_SPINDLE_ON", "EMCIO", false); - _setup.a_axis_wrapped = inifile.findBoolV("WRAPPED_ROTARY", "AXIS_A", false); - _setup.b_axis_wrapped = inifile.findBoolV("WRAPPED_ROTARY", "AXIS_B", false); - _setup.c_axis_wrapped = inifile.findBoolV("WRAPPED_ROTARY", "AXIS_C", false); - _setup.a_rotary_modulo = inifile.findBoolV("ROTARY_MODULO", "AXIS_A", false); - _setup.b_rotary_modulo = inifile.findBoolV("ROTARY_MODULO", "AXIS_B", false); - _setup.c_rotary_modulo = inifile.findBoolV("ROTARY_MODULO", "AXIS_C", false); - { - struct { const char *name; int *wrapped; int *modulo; } axes[] = { - {"AXIS_A", &_setup.a_axis_wrapped, &_setup.a_rotary_modulo}, - {"AXIS_B", &_setup.b_axis_wrapped, &_setup.b_rotary_modulo}, - {"AXIS_C", &_setup.c_axis_wrapped, &_setup.c_rotary_modulo}, - }; - for (auto &a : axes) { - if (*a.wrapped && *a.modulo) { + std::string kinds_err; + if (axisKindsRead(inifile, &_setup.axis_kinds, &kinds_err)) { + ERS("%s", kinds_err.c_str()); + } + // a wrapped rotary, a modulo rotary or a locking indexer is an angular axis + for (int n = 0; n < 9; n++) { + char section[] = "AXIS_X"; + section[5] = "XYZABCUVW"[n]; + if (!axisKindsAngular(_setup.axis_kinds, n)) { continue; } + _setup.axis_wrapped[n] = inifile.findBoolV("WRAPPED_ROTARY", section, false); + _setup.axis_rotary_modulo[n] = inifile.findBoolV("ROTARY_MODULO", section, false); + if (_setup.axis_wrapped[n] && _setup.axis_rotary_modulo[n]) { + fprintf(stderr, + "%s: WRAPPED_ROTARY and ROTARY_MODULO are mutually exclusive; " + "ROTARY_MODULO disabled\n", section); + _setup.axis_rotary_modulo[n] = 0; + } + if (_setup.axis_rotary_modulo[n]) { + // the commanded position accumulates, so motion is refused + // once it leaves MIN/MAX_LIMIT: a bounded range is the one + // that stops working after a few turns + std::optional lo = inifile.findReal("MIN_LIMIT", section); + std::optional hi = inifile.findReal("MAX_LIMIT", section); + if (lo && hi && (*hi - *lo) < ROTARY_MODULO_MIN_RANGE) { fprintf(stderr, - "%s: WRAPPED_ROTARY and ROTARY_MODULO are mutually exclusive; " - "ROTARY_MODULO disabled\n", a.name); - *a.modulo = 0; - } - if (*a.modulo) { - // the commanded position accumulates, so motion is refused - // once it leaves MIN/MAX_LIMIT: a bounded range is the one - // that stops working after a few turns - std::optional lo = inifile.findReal("MIN_LIMIT", a.name); - std::optional hi = inifile.findReal("MAX_LIMIT", a.name); - if (lo && hi && (*hi - *lo) < ROTARY_MODULO_MIN_RANGE) { - fprintf(stderr, - "%s: ROTARY_MODULO=1 with a bounded travel of %.2f deg. " - "The commanded position accumulates instead of wrapping, " - "so motion is refused once it leaves MIN_LIMIT/MAX_LIMIT. " - "Leave both limits unset for a continuously rotating axis.\n", - a.name, *hi - *lo); - } + "%s: ROTARY_MODULO=1 with a bounded travel of %.2f deg. " + "The commanded position accumulates instead of wrapping, " + "so motion is refused once it leaves MIN_LIMIT/MAX_LIMIT. " + "Leave both limits unset for a continuously rotating axis.\n", + section, *hi - *lo); } } + if (auto inival = inifile.findInt("LOCKING_INDEXER_JOINT", section)) { + _setup.axis_indexer_jnum[n] = *inival; + } } _setup.random_toolchanger = inifile.findBoolV("RANDOM_TOOLCHANGER", "EMCIO", false); _setup.num_spindles = inifile.findIntV("SPINDLES", "TRAJ", 1); @@ -945,15 +942,6 @@ int Interp::init() if (inifile.findBoolV("OWORD_WARNONLY", "RS274NGC", false)) _setup.feature_set |= FEATURE_OWORD_WARNONLY; - if (auto inival = inifile.findInt("LOCKING_INDEXER_JOINT", "AXIS_A")) { - _setup.a_indexer_jnum = *inival; - } - if (auto inival = inifile.findInt("LOCKING_INDEXER_JOINT", "AXIS_B")) { - _setup.b_indexer_jnum = *inival; - } - if (auto inival = inifile.findInt("LOCKING_INDEXER_JOINT", "AXIS_C")) { - _setup.c_indexer_jnum = *inival; - } _setup.orient_offset = inifile.findRealV("ORIENT_OFFSET", "RS274NGC", 0.0); double clr = _setup.length_units == CANON_UNITS_INCHES ? 0.050 : 1.0; _setup.parameter_g73_peck_clearance = inifile.findRealV("G73_PECK_CLEARANCE", "RS274NGC", clr); @@ -1133,12 +1121,12 @@ int Interp::init() _setup.origin_offset_x = USER_TO_PROGRAM_LEN(pars[k + 1]); _setup.origin_offset_y = USER_TO_PROGRAM_LEN(pars[k + 2]); _setup.origin_offset_z = USER_TO_PROGRAM_LEN(pars[k + 3]); - _setup.AA_origin_offset = USER_TO_PROGRAM_ANG(pars[k + 4]); - _setup.BB_origin_offset = USER_TO_PROGRAM_ANG(pars[k + 5]); - _setup.CC_origin_offset = USER_TO_PROGRAM_ANG(pars[k + 6]); - _setup.u_origin_offset = USER_TO_PROGRAM_LEN(pars[k + 7]); - _setup.v_origin_offset = USER_TO_PROGRAM_LEN(pars[k + 8]); - _setup.w_origin_offset = USER_TO_PROGRAM_LEN(pars[k + 9]); + _setup.AA_origin_offset = USER_TO_PROGRAM_AX(3, pars[k + 4]); + _setup.BB_origin_offset = USER_TO_PROGRAM_AX(4, pars[k + 5]); + _setup.CC_origin_offset = USER_TO_PROGRAM_AX(5, pars[k + 6]); + _setup.u_origin_offset = USER_TO_PROGRAM_AX(6, pars[k + 7]); + _setup.v_origin_offset = USER_TO_PROGRAM_AX(7, pars[k + 8]); + _setup.w_origin_offset = USER_TO_PROGRAM_AX(8, pars[k + 9]); SET_G5X_OFFSET(_setup.origin_index, _setup.origin_offset_x , @@ -1164,12 +1152,12 @@ int Interp::init() _setup.axis_offset_x = USER_TO_PROGRAM_LEN(pars[5211]); _setup.axis_offset_y = USER_TO_PROGRAM_LEN(pars[5212]); _setup.axis_offset_z = USER_TO_PROGRAM_LEN(pars[5213]); - _setup.AA_axis_offset = USER_TO_PROGRAM_ANG(pars[5214]); - _setup.BB_axis_offset = USER_TO_PROGRAM_ANG(pars[5215]); - _setup.CC_axis_offset = USER_TO_PROGRAM_ANG(pars[5216]); - _setup.u_axis_offset = USER_TO_PROGRAM_LEN(pars[5217]); - _setup.v_axis_offset = USER_TO_PROGRAM_LEN(pars[5218]); - _setup.w_axis_offset = USER_TO_PROGRAM_LEN(pars[5219]); + _setup.AA_axis_offset = USER_TO_PROGRAM_AX(3, pars[5214]); + _setup.BB_axis_offset = USER_TO_PROGRAM_AX(4, pars[5215]); + _setup.CC_axis_offset = USER_TO_PROGRAM_AX(5, pars[5216]); + _setup.u_axis_offset = USER_TO_PROGRAM_AX(6, pars[5217]); + _setup.v_axis_offset = USER_TO_PROGRAM_AX(7, pars[5218]); + _setup.w_axis_offset = USER_TO_PROGRAM_AX(8, pars[5219]); } else { _setup.axis_offset_x = 0.0; _setup.axis_offset_y = 0.0; @@ -1677,17 +1665,20 @@ int Interp::_read(const char *command) //!< may be NULL or a string to read _setup.parameters[5420] = _setup.current_x; _setup.parameters[5421] = _setup.current_y; _setup.parameters[5422] = _setup.current_z; - // ROTARY_MODULO axes: present #5423-#5425 wrapped to [0,360); internal - // AA/BB/CC_current stay accumulated to keep sync with motion.traj.position. - _setup.parameters[5423] = _setup.a_rotary_modulo + // ROTARY_MODULO axes: present #5423-#5428 wrapped to [0,360); internal + // positions stay accumulated to keep sync with motion.traj.position. + _setup.parameters[5423] = _setup.axis_rotary_modulo[3] ? wrap_rotary_to_360(_setup.AA_current) : _setup.AA_current; - _setup.parameters[5424] = _setup.b_rotary_modulo + _setup.parameters[5424] = _setup.axis_rotary_modulo[4] ? wrap_rotary_to_360(_setup.BB_current) : _setup.BB_current; - _setup.parameters[5425] = _setup.c_rotary_modulo + _setup.parameters[5425] = _setup.axis_rotary_modulo[5] ? wrap_rotary_to_360(_setup.CC_current) : _setup.CC_current; - _setup.parameters[5426] = _setup.u_current; - _setup.parameters[5427] = _setup.v_current; - _setup.parameters[5428] = _setup.w_current; + _setup.parameters[5426] = _setup.axis_rotary_modulo[6] + ? wrap_rotary_to_360(_setup.u_current) : _setup.u_current; + _setup.parameters[5427] = _setup.axis_rotary_modulo[7] + ? wrap_rotary_to_360(_setup.v_current) : _setup.v_current; + _setup.parameters[5428] = _setup.axis_rotary_modulo[8] + ? wrap_rotary_to_360(_setup.w_current) : _setup.w_current; double abs_pos[9]; get_abs_position(&_setup, abs_pos); diff --git a/src/emc/rs274ngc/units.h b/src/emc/rs274ngc/units.h index 5256c1a186c..2b1a3de8209 100644 --- a/src/emc/rs274ngc/units.h +++ b/src/emc/rs274ngc/units.h @@ -35,3 +35,7 @@ #define PROGRAM_TO_USER_ANG(p) (TO_EXT_ANG(FROM_PROG_ANG(p))) +/* the same for axis n, 0 X to 8 W, a length or an angle as its + [AXIS_] TYPE says */ +#define USER_TO_PROGRAM_AX(n, u) (axisKindsAngular(_setup.axis_kinds, (n)) ? USER_TO_PROGRAM_ANG(u) : USER_TO_PROGRAM_LEN(u)) +#define PROGRAM_TO_USER_AX(n, p) (axisKindsAngular(_setup.axis_kinds, (n)) ? PROGRAM_TO_USER_ANG(p) : PROGRAM_TO_USER_LEN(p)) diff --git a/src/emc/sai/driver.cc b/src/emc/sai/driver.cc index b0275236c3a..177aa2176b9 100644 --- a/src/emc/sai/driver.cc +++ b/src/emc/sai/driver.cc @@ -27,6 +27,7 @@ #include /* gets, etc. */ #include /* exit */ #include /* strcpy */ +#include /* toupper */ #include #include #include @@ -711,6 +712,13 @@ int main (int argc, char ** argv) _sai._external_length_units = 1.0; } } + if (auto coordinates = ini.findString("COORDINATES", "TRAJ")) { + _sai._axis_mask = 0; + for (char ch : *coordinates) { + const char *at = strchr("XYZABCUVW", toupper((unsigned char)ch)); + if (ch && at) { _sai._axis_mask |= 1 << (at - "XYZABCUVW"); } + } + } setenv("INI_FILE_NAME",inifile,1); } else unsetenv("INI_FILE_NAME"); diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index fd946d5c3af..d60d661d4f7 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -739,7 +739,7 @@ double GET_EXTERNAL_MOTION_CONTROL_NAIVECAM_TOLERANCE() { return _sai.naivecam_tolerance; } double GET_EXTERNAL_LENGTH_UNITS() {return _sai._external_length_units;} int GET_EXTERNAL_FEED_HOLD_ENABLE() {return 1;} -int GET_EXTERNAL_AXIS_MASK() {return 0x3f;} // XYZABC machine +int GET_EXTERNAL_AXIS_MASK() {return _sai._axis_mask;} double GET_EXTERNAL_ANGLE_UNITS() {return 1.0;} int GET_EXTERNAL_SELECTED_TOOL_SLOT() { return 0; } int GET_EXTERNAL_SPINDLE_OVERRIDE_ENABLE(int /*spindle*/) {return so_enable;} @@ -1169,6 +1169,7 @@ StandaloneInterpInternals::StandaloneInterpInternals() : _feed_rate(0.0), _flood(0), _external_length_units(1.0), + _axis_mask(0x3f), /* XYZABC unless the INI names the axes */ _length_unit_factor(1), /* 1 for MM 25.4 for inch */ _length_unit_type(CANON_UNITS_MM), _line_number(1), diff --git a/src/emc/sai/saicanon.hh b/src/emc/sai/saicanon.hh index d00233c02fd..f5946ef5039 100644 --- a/src/emc/sai/saicanon.hh +++ b/src/emc/sai/saicanon.hh @@ -26,6 +26,7 @@ struct StandaloneInterpInternals double _feed_rate; int _flood; double _external_length_units; + int _axis_mask; double _length_unit_factor; CANON_UNITS _length_unit_type; int _line_number; diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index bb6c2deafb3..d3ef981de02 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -63,6 +63,7 @@ #include "nml_intf/emcglb.h" // TRAJ_MAX_VELOCITY #include "nml_intf/modal_state.hh" #include "tooldata/tooldata.hh" +#include #include //#define EMCCANON_DEBUG @@ -114,6 +115,15 @@ void UPDATE_TAG(const StateTag& tag) { #define FROM_PROG_LEN(prog) ((prog) * (canon.lengthUnits == CANON_UNITS_INCHES ? 25.4 : canon.lengthUnits == CANON_UNITS_CM ? 10.0 : 1.0)) #define FROM_PROG_ANG(prog) (prog) +/* [AXIS_] TYPE and [TRAJ] FEED_AXES, axis n 0 X to 8 W */ +static AxisKinds kinds = axisKindsDefault(); + +#define AXIS_ANG(n) axisKindsAngular(kinds, (n)) +#define TO_EXT_AX(n, v) (AXIS_ANG(n) ? TO_EXT_ANG(v) : TO_EXT_LEN(v)) +#define FROM_EXT_AX(n, v) (AXIS_ANG(n) ? FROM_EXT_ANG(v) : FROM_EXT_LEN(v)) +#define TO_PROG_AX(n, v) (AXIS_ANG(n) ? TO_PROG_ANG(v) : TO_PROG_LEN(v)) +#define FROM_PROG_AX(n, v) (AXIS_ANG(n) ? FROM_PROG_ANG(v) : FROM_PROG_LEN(v)) + /* Certain axes are periodic. Hardcode this for now */ #define IS_PERIODIC(axisnum) \ ((axisnum) == 3 || (axisnum) == 4 || (axisnum) == 5) @@ -279,29 +289,24 @@ static void from_prog(double &x, double &y, double &z, double &a, double &b, dou x = FROM_PROG_LEN(x); y = FROM_PROG_LEN(y); z = FROM_PROG_LEN(z); - // Compiler will optimize: a=FROM_PROG_ANG(a) ==> a=a. - // 2.10 cannot handle suppress-macro - // cppcheck-suppress selfAssignment - a = FROM_PROG_ANG(a); - // cppcheck-suppress selfAssignment - b = FROM_PROG_ANG(b); - // cppcheck-suppress selfAssignment - c = FROM_PROG_ANG(c); - u = FROM_PROG_LEN(u); - v = FROM_PROG_LEN(v); - w = FROM_PROG_LEN(w); + a = FROM_PROG_AX(3, a); + b = FROM_PROG_AX(4, b); + c = FROM_PROG_AX(5, c); + u = FROM_PROG_AX(6, u); + v = FROM_PROG_AX(7, v); + w = FROM_PROG_AX(8, w); } static void from_prog(CANON_POSITION &pos) { pos.x = FROM_PROG_LEN(pos.x); pos.y = FROM_PROG_LEN(pos.y); pos.z = FROM_PROG_LEN(pos.z); - pos.a = FROM_PROG_ANG(pos.a); - pos.b = FROM_PROG_ANG(pos.b); - pos.c = FROM_PROG_ANG(pos.c); - pos.u = FROM_PROG_LEN(pos.u); - pos.v = FROM_PROG_LEN(pos.v); - pos.w = FROM_PROG_LEN(pos.w); + pos.a = FROM_PROG_AX(3, pos.a); + pos.b = FROM_PROG_AX(4, pos.b); + pos.c = FROM_PROG_AX(5, pos.c); + pos.u = FROM_PROG_AX(6, pos.u); + pos.v = FROM_PROG_AX(7, pos.v); + pos.w = FROM_PROG_AX(8, pos.w); } static void from_prog_len(PM_CARTESIAN &vec) { @@ -314,24 +319,24 @@ static void to_ext(double &x, double &y, double &z, double &a, double &b, double x = TO_EXT_LEN(x); y = TO_EXT_LEN(y); z = TO_EXT_LEN(z); - a = TO_EXT_ANG(a); - b = TO_EXT_ANG(b); - c = TO_EXT_ANG(c); - u = TO_EXT_LEN(u); - v = TO_EXT_LEN(v); - w = TO_EXT_LEN(w); + a = TO_EXT_AX(3, a); + b = TO_EXT_AX(4, b); + c = TO_EXT_AX(5, c); + u = TO_EXT_AX(6, u); + v = TO_EXT_AX(7, v); + w = TO_EXT_AX(8, w); } static void to_ext(CANON_POSITION & pos) { pos.x=TO_EXT_LEN(pos.x); pos.y=TO_EXT_LEN(pos.y); pos.z=TO_EXT_LEN(pos.z); - pos.a=TO_EXT_ANG(pos.a); - pos.b=TO_EXT_ANG(pos.b); - pos.c=TO_EXT_ANG(pos.c); - pos.u=TO_EXT_LEN(pos.u); - pos.v=TO_EXT_LEN(pos.v); - pos.w=TO_EXT_LEN(pos.w); + pos.a=TO_EXT_AX(3, pos.a); + pos.b=TO_EXT_AX(4, pos.b); + pos.c=TO_EXT_AX(5, pos.c); + pos.u=TO_EXT_AX(6, pos.u); + pos.v=TO_EXT_AX(7, pos.v); + pos.w=TO_EXT_AX(8, pos.w); } #endif @@ -348,12 +353,12 @@ static EmcPose to_ext_pose(double x, double y, double z, double a, double b, dou result.tran.x = TO_EXT_LEN(x); result.tran.y = TO_EXT_LEN(y); result.tran.z = TO_EXT_LEN(z); - result.a = TO_EXT_ANG(a); - result.b = TO_EXT_ANG(b); - result.c = TO_EXT_ANG(c); - result.u = TO_EXT_LEN(u); - result.v = TO_EXT_LEN(v); - result.w = TO_EXT_LEN(w); + result.a = TO_EXT_AX(3, a); + result.b = TO_EXT_AX(4, b); + result.c = TO_EXT_AX(5, c); + result.u = TO_EXT_AX(6, u); + result.v = TO_EXT_AX(7, v); + result.w = TO_EXT_AX(8, w); return result; } @@ -362,12 +367,12 @@ static EmcPose to_ext_pose(const CANON_POSITION & pos) { result.tran.x = TO_EXT_LEN(pos.x); result.tran.y = TO_EXT_LEN(pos.y); result.tran.z = TO_EXT_LEN(pos.z); - result.a = TO_EXT_ANG(pos.a); - result.b = TO_EXT_ANG(pos.b); - result.c = TO_EXT_ANG(pos.c); - result.u = TO_EXT_LEN(pos.u); - result.v = TO_EXT_LEN(pos.v); - result.w = TO_EXT_LEN(pos.w); + result.a = TO_EXT_AX(3, pos.a); + result.b = TO_EXT_AX(4, pos.b); + result.c = TO_EXT_AX(5, pos.c); + result.u = TO_EXT_AX(6, pos.u); + result.v = TO_EXT_AX(7, pos.v); + result.w = TO_EXT_AX(8, pos.w); return result; } @@ -375,12 +380,12 @@ static void to_prog(CANON_POSITION &e) { e.x = TO_PROG_LEN(e.x); e.y = TO_PROG_LEN(e.y); e.z = TO_PROG_LEN(e.z); - e.a = TO_PROG_ANG(e.a); - e.b = TO_PROG_ANG(e.b); - e.c = TO_PROG_ANG(e.c); - e.u = TO_PROG_LEN(e.u); - e.v = TO_PROG_LEN(e.v); - e.w = TO_PROG_LEN(e.w); + e.a = TO_PROG_AX(3, e.a); + e.b = TO_PROG_AX(4, e.b); + e.c = TO_PROG_AX(5, e.c); + e.u = TO_PROG_AX(6, e.u); + e.v = TO_PROG_AX(7, e.v); + e.w = TO_PROG_AX(8, e.w); } static int axis_valid(int n) { @@ -416,8 +421,8 @@ void CANON_UPDATE_END_POINT(double x, double y, double z, double u, double v, double w) { canonUpdateEndPoint(FROM_PROG_LEN(x),FROM_PROG_LEN(y),FROM_PROG_LEN(z), - FROM_PROG_ANG(a),FROM_PROG_ANG(b),FROM_PROG_ANG(c), - FROM_PROG_LEN(u),FROM_PROG_LEN(v),FROM_PROG_LEN(w)); + FROM_PROG_AX(3, a),FROM_PROG_AX(4, b),FROM_PROG_AX(5, c), + FROM_PROG_AX(6, u),FROM_PROG_AX(7, v),FROM_PROG_AX(8, w)); } static double toExtVel(double vel) { @@ -600,36 +605,6 @@ static double getMinAngularDisplacement() return FROM_EXT_ANG(CART_FUZZ); } -/** - * Apply the minimum displacement check to each axis delta. - * - * Checks that the axis is valid / active, and looks up the appropriate minimum - * displacement for the axis type and user units. - */ -static void applyMinDisplacement(double &dx, - double &dy, - double &dz, - double &da, - double &db, - double &dc, - double &du, - double &dv, - double &dw - ) -{ - const double tiny_linear = getMinLinearDisplacement(); - const double tiny_angular = getMinAngularDisplacement(); - if(!axis_valid(0) || dx < tiny_linear) dx = 0.0; - if(!axis_valid(1) || dy < tiny_linear) dy = 0.0; - if(!axis_valid(2) || dz < tiny_linear) dz = 0.0; - if(!axis_valid(3) || da < tiny_angular) da = 0.0; - if(!axis_valid(4) || db < tiny_linear) db = 0.0; - if(!axis_valid(5) || dc < tiny_linear) dc = 0.0; - if(!axis_valid(6) || du < tiny_linear) du = 0.0; - if(!axis_valid(7) || dv < tiny_linear) dv = 0.0; - if(!axis_valid(8) || dw < tiny_linear) dw = 0.0; -} - #ifndef MIN #define MIN(a,b) ((a)<(b)?(a):(b)) #endif @@ -682,124 +657,122 @@ static int __attribute__((unused)) findMinMoveJoint(double &dx, return saxis; } /** - * 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. + * A straight move from canon.endPoint: per axis distance, the axes that + * move, the axes it is measured along and its length. Sets + * canon.cartesian_move and canon.angular_move. */ -static double getStraightJerk(double x, double y, double z, - double a, double b, double c, - double u, double v, double w){ +struct StraightSpan { + double d[9]; + unsigned moving; + unsigned measured; + double length; +}; - double dx, dy, dz, du, dv, dw, da, db, dc; - double tx, ty, tz, tu, tv, tw, ta, tb, tc; - JerkData out; +static StraightSpan getStraightSpan(double x, double y, double z, + double a, double b, double c, + double u, double v, double w) +{ + const double end[9] = {x, y, z, a, b, c, u, v, w}; + const double start[9] = {canon.endPoint.x, canon.endPoint.y, canon.endPoint.z, + canon.endPoint.a, canon.endPoint.b, canon.endPoint.c, + canon.endPoint.u, canon.endPoint.v, canon.endPoint.w}; + const double tiny_linear = getMinLinearDisplacement(); + const double tiny_angular = getMinAngularDisplacement(); + StraightSpan span; - out.jerk = 0.0; // if a move to nowhere - out.tmax = 0.0; - out.dtot = 0.0; + span.moving = 0; + for (int n = 0; n < 9; n++) { + span.d[n] = fabs(end[n] - start[n]); + if (!axis_valid(n) || span.d[n] < (AXIS_ANG(n) ? tiny_angular : tiny_linear)) { + span.d[n] = 0.0; + } + if (span.d[n] > 0.0) { + span.moving |= 1u << n; + } + } + canon.cartesian_move = (span.moving & ~kinds.angular) != 0; + canon.angular_move = (span.moving & kinds.angular) != 0; + span.measured = axisKindsMeasured(kinds, span.moving); + span.length = axisKindsLength(span.measured, span.d); - // Compute absolute travel distance for each axis: - dx = fabs(x - canon.endPoint.x); - dy = fabs(y - canon.endPoint.y); - dz = fabs(z - canon.endPoint.z); - da = fabs(a - canon.endPoint.a); - db = fabs(b - canon.endPoint.b); - dc = fabs(c - canon.endPoint.c); - du = fabs(u - canon.endPoint.u); - dv = fabs(v - canon.endPoint.v); - dw = fabs(w - canon.endPoint.w); + if(debug_velacc) + printf("getStraightSpan dx %g dy %g dz %g da %g db %g dc %g du %g dv %g dw %g length %g\n", + span.d[0], span.d[1], span.d[2], span.d[3], span.d[4], span.d[5], + span.d[6], span.d[7], span.d[8], span.length); + return span; +} - applyMinDisplacement(dx, dy, dz, da, db, dc, du, dv, dw); +/** + * Motion measures a line along X Y Z, else U V W, else A B C, whatever the + * axis types. Motion's length over canon's: 1 with the default types. + */ +static double motionLengthRatio(const StraightSpan &span) +{ + unsigned tier; - if(debug_velacc) - printf("getStraightJerk dx %g dy %g dz %g da %g db %g dc %g du %g dv %g dw %g ", - 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 && - du <= 0.0 && dv <= 0.0 && dw <= 0.0) { - canon.cartesian_move = 0; + if (span.moving & 0x007u) { + tier = 0x007u; + } else if (span.moving & 0x1c0u) { + tier = 0x1c0u; + } else if (span.moving & 0x038u) { + tier = 0x038u; } else { - canon.cartesian_move = 1; + return 1.0; } - if (da <= 0.0 && db <= 0.0 && dc <= 0.0) { - canon.angular_move = 0; - } else { - canon.angular_move = 1; + unsigned tier_angular = tier & kinds.angular; + if (tier == span.measured && (tier_angular == 0 || tier_angular == tier)) { + return 1.0; } + double ext[9]; + for (int n = 0; n < 9; n++) { + ext[n] = TO_EXT_AX(n, span.d[n]); + } + double own = axisKindsMeasuredAngular(kinds, span.measured) ? + TO_EXT_ANG(span.length) : TO_EXT_LEN(span.length); + if (own <= 0.0) { + return 1.0; + } + return axisKindsLength(tier, ext) / own; +} + +// Scale the rates to motion's length. The max velocity slider caps canon's +// length too; a move measured in degrees it does not cap (0). +template static void toMotionLength(M &msg, const StraightSpan &span) +{ + double ratio = motionLengthRatio(span); + + msg.vel *= ratio; + msg.ini_maxvel *= ratio; + msg.acc *= ratio; + msg.ini_maxjerk *= ratio; + msg.vlimit_scale = axisKindsMeasuredAngular(kinds, span.measured) ? 0.0 : ratio; +} + +/** + * Get the limiting jerk for a displacement from the current position to the + * given position: the path jerk at which the first axis reaches its own. + */ +static double getStraightJerk(double x, double y, double z, + double a, double b, double c, + double u, double v, double w){ + + StraightSpan span = getStraightSpan(x, y, z, a, b, c, u, v, w); + double tmax = 0.0; - // Pure linear move: // For jerk-limited motion: d = (1/6)*j*t³, so t = cbrt(6*d/j) // We use t = cbrt(d/j) as a characteristic time (omitting the constant factor, // which cancels out when we compute path jerk = dtot / tmax³) - if (canon.cartesian_move && !canon.angular_move) { - tx = dx? cbrt(dx / FROM_EXT_LEN(emcAxisGetMaxJerk(0))): 0.0; - ty = dy? cbrt(dy / FROM_EXT_LEN(emcAxisGetMaxJerk(1))): 0.0; - tz = dz? cbrt(dz / FROM_EXT_LEN(emcAxisGetMaxJerk(2))): 0.0; - tu = du? cbrt(du / FROM_EXT_LEN(emcAxisGetMaxJerk(6))): 0.0; - tv = dv? cbrt(dv / FROM_EXT_LEN(emcAxisGetMaxJerk(7))): 0.0; - tw = dw? cbrt(dw / FROM_EXT_LEN(emcAxisGetMaxJerk(8))): 0.0; - out.tmax = MAX3(tx, ty ,tz); - out.tmax = MAX4(tu, tv, tw, out.tmax); - - if(dx || dy || dz) - out.dtot = sqrt(dx * dx + dy * dy + dz * dz); - else - out.dtot = sqrt(du * du + dv * dv + dw * dw); - - if (out.tmax > 0.0) { - out.jerk = out.dtot / (out.tmax * out.tmax * out.tmax); - } - } - // Pure angular move: - else if (!canon.cartesian_move && canon.angular_move) { - ta = da? cbrt(da / FROM_EXT_ANG(emcAxisGetMaxJerk(3))): 0.0; - tb = db? cbrt(db / FROM_EXT_ANG(emcAxisGetMaxJerk(4))): 0.0; - tc = dc? cbrt(dc / FROM_EXT_ANG(emcAxisGetMaxJerk(5))): 0.0; - out.tmax = MAX3(ta, tb, tc); - - out.dtot = sqrt(da * da + db * db + dc * dc); - if (out.tmax > 0.0) { - out.jerk = out.dtot / (out.tmax * out.tmax * out.tmax); + for (int n = 0; n < 9; n++) { + if (span.d[n]) { + tmax = std::max(tmax, cbrt(span.d[n] / FROM_EXT_AX(n, emcAxisGetMaxJerk(n)))); } } - // Combination angular and linear move: - else if (canon.cartesian_move && canon.angular_move) { - tx = dx? cbrt(dx / FROM_EXT_LEN(emcAxisGetMaxJerk(0))): 0.0; - ty = dy? cbrt(dy / FROM_EXT_LEN(emcAxisGetMaxJerk(1))): 0.0; - tz = dz? cbrt(dz / FROM_EXT_LEN(emcAxisGetMaxJerk(2))): 0.0; - ta = da? cbrt(da / FROM_EXT_ANG(emcAxisGetMaxJerk(3))): 0.0; - tb = db? cbrt(db / FROM_EXT_ANG(emcAxisGetMaxJerk(4))): 0.0; - tc = dc? cbrt(dc / FROM_EXT_ANG(emcAxisGetMaxJerk(5))): 0.0; - tu = du? cbrt(du / FROM_EXT_LEN(emcAxisGetMaxJerk(6))): 0.0; - tv = dv? cbrt(dv / FROM_EXT_LEN(emcAxisGetMaxJerk(7))): 0.0; - tw = dw? cbrt(dw / FROM_EXT_LEN(emcAxisGetMaxJerk(8))): 0.0; - out.tmax = MAX9(tx, ty, tz, - ta, tb, tc, - tu, tv, tw); - - if(debug_velacc) - printf("getStraightJerk t tx %g ty %g tz %g ta %g tb %g tc %g tu %g tv %g tw %g\n", - tx, ty, tz, ta, tb, tc, tu, tv, tw); - - if(dx || dy || dz) - out.dtot = sqrt(dx * dx + dy * dy + dz * dz); - else - out.dtot = sqrt(du * du + dv * dv + dw * dw); - - if (out.tmax > 0.0) { - out.jerk = out.dtot / (out.tmax * out.tmax * out.tmax); - } + if (tmax > 0.0) { + return span.length / (tmax * tmax * tmax); } - //if(debug_velacc) - //printf("#### CALC THE JERK #### cartesian %d ang %d jerk %g\n", canon.cartesian_move, canon.angular_move, out.jerk); - return out.jerk; + return 0.0; // a move to nowhere } -static double __attribute__((unused)) getStraightJerk(CANON_POSITION pos) -{ - return getStraightJerk(pos.x, pos.y, pos.z, pos.a, pos.b, pos.c, pos.u, pos.v, pos.w); -} /** * 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. @@ -808,111 +781,28 @@ static AccelData getStraightAcceleration(double x, double y, double z, double a, double b, double c, double u, double v, double w) { - double dx, dy, dz, du, dv, dw, da, db, dc; - double tx, ty, tz, tu, tv, tw, ta, tb, tc; + StraightSpan span = getStraightSpan(x, y, z, a, b, c, u, v, w); AccelData out; out.acc = 0.0; // if a move to nowhere out.tmax = 0.0; - out.dtot = 0.0; - - // Compute absolute travel distance for each axis: - dx = fabs(x - canon.endPoint.x); - dy = fabs(y - canon.endPoint.y); - dz = fabs(z - canon.endPoint.z); - da = fabs(a - canon.endPoint.a); - db = fabs(b - canon.endPoint.b); - dc = fabs(c - canon.endPoint.c); - du = fabs(u - canon.endPoint.u); - dv = fabs(v - canon.endPoint.v); - dw = fabs(w - canon.endPoint.w); - - 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 ", - 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 && - du <= 0.0 && dv <= 0.0 && dw <= 0.0) { - canon.cartesian_move = 0; - } else { - canon.cartesian_move = 1; - } - if (da <= 0.0 && db <= 0.0 && dc <= 0.0) { - canon.angular_move = 0; - } else { - canon.angular_move = 1; - } + out.dtot = span.length; - // Pure linear move: - if (canon.cartesian_move && !canon.angular_move) { - tx = dx? (dx / FROM_EXT_LEN(emcAxisGetMaxAcceleration(0))): 0.0; - ty = dy? (dy / FROM_EXT_LEN(emcAxisGetMaxAcceleration(1))): 0.0; - tz = dz? (dz / FROM_EXT_LEN(emcAxisGetMaxAcceleration(2))): 0.0; - tu = du? (du / FROM_EXT_LEN(emcAxisGetMaxAcceleration(6))): 0.0; - tv = dv? (dv / FROM_EXT_LEN(emcAxisGetMaxAcceleration(7))): 0.0; - tw = dw? (dw / FROM_EXT_LEN(emcAxisGetMaxAcceleration(8))): 0.0; - out.tmax = std::max({tx, ty ,tz}); - out.tmax = std::max({tu, tv, tw, out.tmax}); - - if(dx || dy || dz) - out.dtot = sqrt(dx * dx + dy * dy + dz * dz); - else - out.dtot = sqrt(du * du + dv * dv + dw * dw); - - if (out.tmax > 0.0) { - out.acc = out.dtot / out.tmax; - } - } - // Pure angular move: - else if (!canon.cartesian_move && canon.angular_move) { - ta = da? (da / FROM_EXT_ANG(emcAxisGetMaxAcceleration(3))): 0.0; - tb = db? (db / FROM_EXT_ANG(emcAxisGetMaxAcceleration(4))): 0.0; - tc = dc? (dc / FROM_EXT_ANG(emcAxisGetMaxAcceleration(5))): 0.0; - out.tmax = std::max({ta, tb, tc}); - - out.dtot = sqrt(da * da + db * db + dc * dc); - if (out.tmax > 0.0) { - out.acc = out.dtot / out.tmax; - } - } - // Combination angular and linear move: - else if (canon.cartesian_move && canon.angular_move) { - tx = dx? (dx / FROM_EXT_LEN(emcAxisGetMaxAcceleration(0))): 0.0; - ty = dy? (dy / FROM_EXT_LEN(emcAxisGetMaxAcceleration(1))): 0.0; - tz = dz? (dz / FROM_EXT_LEN(emcAxisGetMaxAcceleration(2))): 0.0; - ta = da? (da / FROM_EXT_ANG(emcAxisGetMaxAcceleration(3))): 0.0; - tb = db? (db / FROM_EXT_ANG(emcAxisGetMaxAcceleration(4))): 0.0; - tc = dc? (dc / FROM_EXT_ANG(emcAxisGetMaxAcceleration(5))): 0.0; - tu = du? (du / FROM_EXT_LEN(emcAxisGetMaxAcceleration(6))): 0.0; - tv = dv? (dv / FROM_EXT_LEN(emcAxisGetMaxAcceleration(7))): 0.0; - tw = dw? (dw / FROM_EXT_LEN(emcAxisGetMaxAcceleration(8))): 0.0; - out.tmax = std::max({tx, ty, tz, - ta, tb, tc, - tu, tv, tw}); - - if(debug_velacc) - printf("getStraightAcceleration t^2 tx %g ty %g tz %g ta %g tb %g tc %g tu %g tv %g tw %g\n", - tx, ty, tz, ta, tb, tc, tu, tv, tw); /* According to NIST IR6556 Section 2.1.2.5 Paragraph A a combnation move is handled like a linear move, except that the angular axes are allowed sufficient time to complete their motion coordinated with the motion of the linear axes. */ - if(dx || dy || dz) - out.dtot = sqrt(dx * dx + dy * dy + dz * dz); - else - out.dtot = sqrt(du * du + dv * dv + dw * dw); - - if (out.tmax > 0.0) { - out.acc = out.dtot / out.tmax; - } + for (int n = 0; n < 9; n++) { + if (span.d[n]) { + out.tmax = std::max(out.tmax, span.d[n] / FROM_EXT_AX(n, emcAxisGetMaxAcceleration(n))); + } + } + if (out.tmax > 0.0) { + out.acc = out.dtot / out.tmax; } - if(debug_velacc) + if(debug_velacc) printf("cartesian %d ang %d acc %g\n", canon.cartesian_move, canon.angular_move, out.acc); return out; } @@ -935,120 +825,28 @@ static VelData getStraightVelocity(double x, double y, double z, 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; + StraightSpan span = getStraightSpan(x, y, z, a, b, c, u, v, w); VelData out; -/* If we get a move to nowhere (!canon.cartesian_move && !canon.angular_move) - we might as well go there at the canon.linearFeedRate... -*/ - out.vel = canon.linearFeedRate; out.tmax = 0; - out.dtot = 0; - - // Compute absolute travel distance for each axis: - dx = fabs(x - canon.endPoint.x); - dy = fabs(y - canon.endPoint.y); - dz = fabs(z - canon.endPoint.z); - da = fabs(a - canon.endPoint.a); - db = fabs(b - canon.endPoint.b); - dc = fabs(c - canon.endPoint.c); - du = fabs(u - canon.endPoint.u); - dv = fabs(v - canon.endPoint.v); - dw = fabs(w - canon.endPoint.w); - - applyMinDisplacement(dx, dy, dz, da, db, dc, du, dv, dw); - - if(debug_velacc) - printf("getStraightVelocity 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: - if (dx <= 0.0 && dy <= 0.0 && dz <= 0.0 && - du <= 0.0 && dv <= 0.0 && dw <= 0.0) { - canon.cartesian_move = 0; - } else { - canon.cartesian_move = 1; - } - if (da <= 0.0 && db <= 0.0 && dc <= 0.0) { - canon.angular_move = 0; - } else { - canon.angular_move = 1; - } - - // Pure linear move: - if (canon.cartesian_move && !canon.angular_move) { - tx = dx? fabs(dx / FROM_EXT_LEN(emcAxisGetMaxVelocity(0))): 0.0; - ty = dy? fabs(dy / FROM_EXT_LEN(emcAxisGetMaxVelocity(1))): 0.0; - tz = dz? fabs(dz / FROM_EXT_LEN(emcAxisGetMaxVelocity(2))): 0.0; - tu = du? fabs(du / FROM_EXT_LEN(emcAxisGetMaxVelocity(6))): 0.0; - tv = dv? fabs(dv / FROM_EXT_LEN(emcAxisGetMaxVelocity(7))): 0.0; - tw = dw? fabs(dw / FROM_EXT_LEN(emcAxisGetMaxVelocity(8))): 0.0; - out.tmax = std::max({tx, ty ,tz}); - out.tmax = std::max({tu, tv, tw, out.tmax}); - - if(dx || dy || dz) - out.dtot = sqrt(dx * dx + dy * dy + dz * dz); - else - out.dtot = sqrt(du * du + dv * dv + dw * dw); - - if (out.tmax <= 0.0) { - out.vel = canon.linearFeedRate; - } else { - out.vel = out.dtot / out.tmax; + out.dtot = span.length; + for (int n = 0; n < 9; n++) { + if (span.d[n]) { + out.tmax = std::max(out.tmax, fabs(span.d[n] / FROM_EXT_AX(n, emcAxisGetMaxVelocity(n)))); } } - // Pure angular move: - else if (!canon.cartesian_move && canon.angular_move) { - ta = da? fabs(da / FROM_EXT_ANG(emcAxisGetMaxVelocity(3))): 0.0; - tb = db? fabs(db / FROM_EXT_ANG(emcAxisGetMaxVelocity(4))): 0.0; - tc = dc? fabs(dc / FROM_EXT_ANG(emcAxisGetMaxVelocity(5))): 0.0; - out.tmax = std::max({ta, tb, tc}); - - out.dtot = sqrt(da * da + db * db + dc * dc); - if (out.tmax <= 0.0) { - out.vel = canon.angularFeedRate; - } else { - out.vel = out.dtot / out.tmax; - } - } - // Combination angular and linear move: - else if (canon.cartesian_move && canon.angular_move) { - tx = dx? fabs(dx / FROM_EXT_LEN(emcAxisGetMaxVelocity(0))): 0.0; - ty = dy? fabs(dy / FROM_EXT_LEN(emcAxisGetMaxVelocity(1))): 0.0; - tz = dz? fabs(dz / FROM_EXT_LEN(emcAxisGetMaxVelocity(2))): 0.0; - ta = da? fabs(da / FROM_EXT_ANG(emcAxisGetMaxVelocity(3))): 0.0; - tb = db? fabs(db / FROM_EXT_ANG(emcAxisGetMaxVelocity(4))): 0.0; - tc = dc? fabs(dc / FROM_EXT_ANG(emcAxisGetMaxVelocity(5))): 0.0; - tu = du? fabs(du / FROM_EXT_LEN(emcAxisGetMaxVelocity(6))): 0.0; - tv = dv? fabs(dv / FROM_EXT_LEN(emcAxisGetMaxVelocity(7))): 0.0; - tw = dw? fabs(dw / FROM_EXT_LEN(emcAxisGetMaxVelocity(8))): 0.0; - out.tmax = std::max({tx, ty, tz, - ta, tb, tc, - tu, tv, tw}); - - if(debug_velacc) - printf("getStraightVelocity times tx %g ty %g tz %g ta %g tb %g tc %g tu %g tv %g tw %g\n", - tx, ty, tz, ta, tb, tc, tu, tv, tw); -/* According to NIST IR6556 Section 2.1.2.5 Paragraph A - a combnation move is handled like a linear move, except - that the angular axes are allowed sufficient time to - complete their motion coordinated with the motion of - the linear axes. +/* If we get a move to nowhere (!canon.cartesian_move && !canon.angular_move) + we might as well go there at the canon.linearFeedRate... */ - if(dx || dy || dz) - out.dtot = sqrt(dx * dx + dy * dy + dz * dz); - else - out.dtot = sqrt(du * du + dv * dv + dw * dw); - - if (out.tmax <= 0.0) { - out.vel = canon.linearFeedRate; - } else { - out.vel = out.dtot / out.tmax; - } + if (out.tmax > 0.0) { + out.vel = out.dtot / out.tmax; + } else if (!canon.cartesian_move && canon.angular_move) { + out.vel = canon.angularFeedRate; + } else { + out.vel = canon.linearFeedRate; } - if(debug_velacc) + if(debug_velacc) printf("cartesian %d ang %d vel %g\n", canon.cartesian_move, canon.angular_move, out.vel); return out; } @@ -1123,14 +921,14 @@ static void flush_segments(void) { linearMoveMsg->end.tran.y = TO_EXT_LEN(y); linearMoveMsg->end.tran.z = TO_EXT_LEN(z); - linearMoveMsg->end.u = TO_EXT_LEN(u); - linearMoveMsg->end.v = TO_EXT_LEN(v); - linearMoveMsg->end.w = TO_EXT_LEN(w); + linearMoveMsg->end.u = TO_EXT_AX(6, u); + linearMoveMsg->end.v = TO_EXT_AX(7, v); + linearMoveMsg->end.w = TO_EXT_AX(8, w); // fill in the orientation - linearMoveMsg->end.a = TO_EXT_ANG(a); - linearMoveMsg->end.b = TO_EXT_ANG(b); - linearMoveMsg->end.c = TO_EXT_ANG(c); + linearMoveMsg->end.a = TO_EXT_AX(3, a); + linearMoveMsg->end.b = TO_EXT_AX(4, b); + linearMoveMsg->end.c = TO_EXT_AX(5, c); linearMoveMsg->vel = toExtVel(vel); linearMoveMsg->ini_maxvel = toExtVel(linedata.vel); @@ -1138,6 +936,7 @@ static void flush_segments(void) { double acc = lineaccdata.acc; linearMoveMsg->ini_maxjerk = toExtVel(jerk); linearMoveMsg->acc = toExtAcc(acc); + toMotionLength(*linearMoveMsg, getStraightSpan(x, y, z, a, b, c, u, v, w)); linearMoveMsg->type = EMC_MOTION_TYPE_FEED; linearMoveMsg->indexer_jnum = -1; @@ -1266,6 +1065,7 @@ void generate_fast_move(double x, double y, double z, linearMoveMsg->vel = linearMoveMsg->ini_maxvel = toExtVel(vel); linearMoveMsg->acc = toExtAcc(acc); linearMoveMsg->ini_maxjerk = toExtVel(jerk); + toMotionLength(*linearMoveMsg, getStraightSpan(x, y, z, a, b, c, u, v, w)); linearMoveMsg->type = EMC_MOTION_TYPE_FEED; linearMoveMsg->feed_mode = 0; @@ -1300,6 +1100,7 @@ void generate_move(double vel,double x, double y, double z, linearMoveMsg->vel = linearMoveMsg->ini_maxvel = toExtVel(vel); linearMoveMsg->acc = toExtAcc(acc); linearMoveMsg->ini_maxjerk = toExtVel(jerk); + toMotionLength(*linearMoveMsg, getStraightSpan(x, y, z, a, b, c, u, v, w)); linearMoveMsg->type = EMC_MOTION_TYPE_FEED; linearMoveMsg->feed_mode = 0; linearMoveMsg->indexer_jnum = -1; @@ -1349,6 +1150,7 @@ void STRAIGHT_TRAVERSE(int line_number, linearMoveMsg->vel = linearMoveMsg->ini_maxvel = toExtVel(vel); linearMoveMsg->acc = toExtAcc(acc); linearMoveMsg->ini_maxjerk = toExtVel(jerk); + toMotionLength(*linearMoveMsg, getStraightSpan(x, y, z, a, b, c, u, v, w)); linearMoveMsg->indexer_jnum = canon.rotary_unlock_for_traverse; int old_feed_mode = canon.feed_mode; @@ -1464,6 +1266,7 @@ void STRAIGHT_PROBE(int line_number, probeMsg->ini_maxvel = toExtVel(ini_maxvel); probeMsg->acc = toExtAcc(acc); probeMsg->ini_maxjerk = toExtVel(jerk); + toMotionLength(*probeMsg, getStraightSpan(x, y, z, a, b, c, u, v, w)); probeMsg->type = EMC_MOTION_TYPE_PROBING; probeMsg->probe_type = probe_type; @@ -2636,17 +2439,12 @@ void ARC_FEED(int line_number, rotate_and_offset_pos(fe, se, ae, unused, unused, unused, unused, unused, unused); rotate_and_offset_pos(fa, sa, unused, unused, unused, unused, unused, unused, unused); if (chord_deviation(lx, ly, fe, se, fa, sa, rotation, mx, my) < canon.naivecamTolerance) { - // Compiler will optimize: a=FROM_PROG_ANG(a) ==> a=a. - // 2.10 cannot handle suppress-macro - // cppcheck-suppress selfAssignment - a = FROM_PROG_ANG(a); - // cppcheck-suppress selfAssignment - b = FROM_PROG_ANG(b); - // cppcheck-suppress selfAssignment - c = FROM_PROG_ANG(c); - u = FROM_PROG_LEN(u); - v = FROM_PROG_LEN(v); - w = FROM_PROG_LEN(w); + a = FROM_PROG_AX(3, a); + b = FROM_PROG_AX(4, b); + c = FROM_PROG_AX(5, c); + u = FROM_PROG_AX(6, u); + v = FROM_PROG_AX(7, v); + w = FROM_PROG_AX(8, w); rotate_and_offset_pos(unused, unused, unused, a, b, c, u, v, w); see_segment(line_number, _tag, mx, my, @@ -3144,24 +2942,24 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) canon.toolOffset.tran.x = FROM_PROG_LEN(offset.tran.x); canon.toolOffset.tran.y = FROM_PROG_LEN(offset.tran.y); canon.toolOffset.tran.z = FROM_PROG_LEN(offset.tran.z); - canon.toolOffset.a = FROM_PROG_ANG(offset.a); - canon.toolOffset.b = FROM_PROG_ANG(offset.b); - canon.toolOffset.c = FROM_PROG_ANG(offset.c); - canon.toolOffset.u = FROM_PROG_LEN(offset.u); - canon.toolOffset.v = FROM_PROG_LEN(offset.v); - canon.toolOffset.w = FROM_PROG_LEN(offset.w); + canon.toolOffset.a = FROM_PROG_AX(3, offset.a); + canon.toolOffset.b = FROM_PROG_AX(4, offset.b); + canon.toolOffset.c = FROM_PROG_AX(5, offset.c); + canon.toolOffset.u = FROM_PROG_AX(6, offset.u); + canon.toolOffset.v = FROM_PROG_AX(7, offset.v); + canon.toolOffset.w = FROM_PROG_AX(8, offset.w); /* append it to interp list so it gets updated at the right time, not at read-ahead time */ set_offset_msg->offset.tran.x = TO_EXT_LEN(canon.toolOffset.tran.x); set_offset_msg->offset.tran.y = TO_EXT_LEN(canon.toolOffset.tran.y); set_offset_msg->offset.tran.z = TO_EXT_LEN(canon.toolOffset.tran.z); - set_offset_msg->offset.a = TO_EXT_ANG(canon.toolOffset.a); - set_offset_msg->offset.b = TO_EXT_ANG(canon.toolOffset.b); - set_offset_msg->offset.c = TO_EXT_ANG(canon.toolOffset.c); - set_offset_msg->offset.u = TO_EXT_LEN(canon.toolOffset.u); - set_offset_msg->offset.v = TO_EXT_LEN(canon.toolOffset.v); - set_offset_msg->offset.w = TO_EXT_LEN(canon.toolOffset.w); + set_offset_msg->offset.a = TO_EXT_AX(3, canon.toolOffset.a); + set_offset_msg->offset.b = TO_EXT_AX(4, canon.toolOffset.b); + set_offset_msg->offset.c = TO_EXT_AX(5, canon.toolOffset.c); + set_offset_msg->offset.u = TO_EXT_AX(6, canon.toolOffset.u); + set_offset_msg->offset.v = TO_EXT_AX(7, canon.toolOffset.v); + set_offset_msg->offset.w = TO_EXT_AX(8, canon.toolOffset.w); for (int s = 0; s < emcStatus->motion.traj.spindles; s++){ if(canon.spindle[s].css_maximum) { @@ -3200,15 +2998,15 @@ void CHANGE_TOOL() w = canon.endPoint.w; if (have_tool_change_position > 3) { - a = FROM_EXT_ANG(tool_change_position.a); - b = FROM_EXT_ANG(tool_change_position.b); - c = FROM_EXT_ANG(tool_change_position.c); + a = FROM_EXT_AX(3, tool_change_position.a); + b = FROM_EXT_AX(4, tool_change_position.b); + c = FROM_EXT_AX(5, tool_change_position.c); } if (have_tool_change_position > 6) { - u = FROM_EXT_LEN(tool_change_position.u); - v = FROM_EXT_LEN(tool_change_position.v); - w = FROM_EXT_LEN(tool_change_position.w); + u = FROM_EXT_AX(6, tool_change_position.u); + v = FROM_EXT_AX(7, tool_change_position.v); + w = FROM_EXT_AX(8, tool_change_position.w); } VelData veldata = getStraightVelocity(x, y, z, a, b, c, u, v, w); @@ -3225,6 +3023,7 @@ void CHANGE_TOOL() linearMoveMsg->vel = linearMoveMsg->ini_maxvel = toExtVel(vel); linearMoveMsg->acc = toExtAcc(acc); linearMoveMsg->ini_maxjerk = toExtVel(jerk); + toMotionLength(*linearMoveMsg, getStraightSpan(x, y, z, a, b, c, u, v, w)); linearMoveMsg->type = EMC_MOTION_TYPE_TOOLCHANGE; linearMoveMsg->feed_mode = 0; linearMoveMsg->indexer_jnum = -1; @@ -3603,32 +3402,32 @@ double GET_EXTERNAL_TOOL_LENGTH_ZOFFSET() double GET_EXTERNAL_TOOL_LENGTH_AOFFSET() { - return TO_PROG_ANG(canon.toolOffset.a); + return TO_PROG_AX(3, canon.toolOffset.a); } double GET_EXTERNAL_TOOL_LENGTH_BOFFSET() { - return TO_PROG_ANG(canon.toolOffset.b); + return TO_PROG_AX(4, canon.toolOffset.b); } double GET_EXTERNAL_TOOL_LENGTH_COFFSET() { - return TO_PROG_ANG(canon.toolOffset.c); + return TO_PROG_AX(5, canon.toolOffset.c); } double GET_EXTERNAL_TOOL_LENGTH_UOFFSET() { - return TO_PROG_LEN(canon.toolOffset.u); + return TO_PROG_AX(6, canon.toolOffset.u); } double GET_EXTERNAL_TOOL_LENGTH_VOFFSET() { - return TO_PROG_LEN(canon.toolOffset.v); + return TO_PROG_AX(7, canon.toolOffset.v); } double GET_EXTERNAL_TOOL_LENGTH_WOFFSET() { - return TO_PROG_LEN(canon.toolOffset.w); + return TO_PROG_AX(8, canon.toolOffset.w); } /* @@ -3678,6 +3477,14 @@ void INIT_CANON() canon.angularFeedRate = 0.0; ZERO_EMC_POSE(canon.toolOffset); + { + std::string err; + linuxcnc::IniFile ini(emc_inifile); + if (axisKindsRead(ini, &kinds, &err)) { + rcs_print_error("%s\n", err.c_str()); + } + } + /* to set the units, note that GET_EXTERNAL_LENGTH_UNITS() returns traj->linearUnits, which is already set from the INI file in @@ -3774,8 +3581,8 @@ CANON_POSITION GET_EXTERNAL_POSITION() // first update internal record of last position canonUpdateEndPoint(FROM_EXT_LEN(pos.tran.x), FROM_EXT_LEN(pos.tran.y), FROM_EXT_LEN(pos.tran.z), - FROM_EXT_ANG(pos.a), FROM_EXT_ANG(pos.b), FROM_EXT_ANG(pos.c), - FROM_EXT_LEN(pos.u), FROM_EXT_LEN(pos.v), FROM_EXT_LEN(pos.w)); + FROM_EXT_AX(3, pos.a), FROM_EXT_AX(4, pos.b), FROM_EXT_AX(5, pos.c), + FROM_EXT_AX(6, pos.u), FROM_EXT_AX(7, pos.v), FROM_EXT_AX(8, pos.w)); // now calculate position in program units, for interpreter position = unoffset_and_unrotate_pos(canon.endPoint); @@ -3799,13 +3606,13 @@ CANON_POSITION GET_EXTERNAL_PROBE_POSITION() pos.tran.y = FROM_EXT_LEN(pos.tran.y); pos.tran.z = FROM_EXT_LEN(pos.tran.z); - pos.a = FROM_EXT_ANG(pos.a); - pos.b = FROM_EXT_ANG(pos.b); - pos.c = FROM_EXT_ANG(pos.c); + pos.a = FROM_EXT_AX(3, pos.a); + pos.b = FROM_EXT_AX(4, pos.b); + pos.c = FROM_EXT_AX(5, pos.c); - pos.u = FROM_EXT_LEN(pos.u); - pos.v = FROM_EXT_LEN(pos.v); - pos.w = FROM_EXT_LEN(pos.w); + pos.u = FROM_EXT_AX(6, pos.u); + pos.v = FROM_EXT_AX(7, pos.v); + pos.w = FROM_EXT_AX(8, pos.w); // now calculate position in program units, for interpreter position = unoffset_and_unrotate_pos(pos); @@ -3864,10 +3671,7 @@ double GET_EXTERNAL_AXIS_MAX_VELOCITY(int axis) double vel = emcAxisGetMaxVelocity(axis); - if (axis >= 3 && axis <= 5) { - return TO_PROG_ANG(FROM_EXT_ANG(vel)) * 60.0; - } - return TO_PROG_LEN(FROM_EXT_LEN(vel)) * 60.0; + return TO_PROG_AX(axis, FROM_EXT_AX(axis, vel)) * 60.0; } double GET_EXTERNAL_SPINDLE_MAX_VELOCITY(int spindle) diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 8e638b08651..a3b52564450 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -1916,6 +1916,7 @@ static int emcTaskIssueCommand(NMLmsg * cmd) retval = emcTrajLinearMove(emcTrajLinearMoveMsg->end, emcTrajLinearMoveMsg->type, emcTrajLinearMoveMsg->vel, emcTrajLinearMoveMsg->ini_maxvel, emcTrajLinearMoveMsg->acc, emcTrajLinearMoveMsg->ini_maxjerk, + emcTrajLinearMoveMsg->vlimit_scale, emcTrajLinearMoveMsg->indexer_jnum); break; @@ -1927,7 +1928,8 @@ static int emcTaskIssueCommand(NMLmsg * cmd) emcTrajCircularMoveMsg->turn, emcTrajCircularMoveMsg->type, emcTrajCircularMoveMsg->vel, emcTrajCircularMoveMsg->ini_maxvel, - emcTrajCircularMoveMsg->acc, emcTrajCircularMoveMsg->ini_maxjerk); + emcTrajCircularMoveMsg->acc, emcTrajCircularMoveMsg->ini_maxjerk, + emcTrajCircularMoveMsg->vlimit_scale); break; case EMC_TRAJ_PAUSE_TYPE: @@ -2016,6 +2018,7 @@ static int emcTaskIssueCommand(NMLmsg * cmd) (reinterpret_cast(cmd))->ini_maxvel, (reinterpret_cast(cmd))->acc, (reinterpret_cast(cmd))->ini_maxjerk, + (reinterpret_cast(cmd))->vlimit_scale, (reinterpret_cast(cmd))->probe_type); break; diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index c93d82ede20..befed5a46b4 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1568,8 +1568,8 @@ int emcTrajSetTermCond(int cond, double tolerance) return usrmotWriteEmcmotCommand(&emcmotCommand); } -int emcTrajLinearMove(const EmcPose& end, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk, - int indexer_jnum) +int emcTrajLinearMove(const EmcPose& end, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk, + double vlimit_scale, int indexer_jnum) { #ifdef ISNAN_TRAP if (std::isnan(end.tran.x) || std::isnan(end.tran.y) || std::isnan(end.tran.z) || @@ -1591,13 +1591,15 @@ int emcTrajLinearMove(const EmcPose& end, int type, double vel, double ini_maxve emcmotCommand.ini_maxvel = ini_maxvel; emcmotCommand.acc = acc; emcmotCommand.ini_maxjerk = ini_maxjerk; + emcmotCommand.vlimit_scale = vlimit_scale; emcmotCommand.turn = indexer_jnum; return usrmotWriteEmcmotCommand(&emcmotCommand); } int emcTrajCircularMove(const EmcPose& end, const PM_CARTESIAN& center, - const PM_CARTESIAN& normal, int turn, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk) + const PM_CARTESIAN& normal, int turn, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk, + double vlimit_scale) { #ifdef ISNAN_TRAP if (std::isnan(end.tran.x) || std::isnan(end.tran.y) || std::isnan(end.tran.z) || @@ -1631,6 +1633,7 @@ int emcTrajCircularMove(const EmcPose& end, const PM_CARTESIAN& center, emcmotCommand.ini_maxvel = ini_maxvel; emcmotCommand.acc = acc; emcmotCommand.ini_maxjerk = ini_maxjerk; + emcmotCommand.vlimit_scale = vlimit_scale; return usrmotWriteEmcmotCommand(&emcmotCommand); } @@ -1642,7 +1645,7 @@ int emcTrajClearProbeTrippedFlag() return usrmotWriteEmcmotCommand(&emcmotCommand); } -int emcTrajProbe(const EmcPose& pos, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk, unsigned char probe_type) +int emcTrajProbe(const EmcPose& pos, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk, double vlimit_scale, unsigned char probe_type) { #ifdef ISNAN_TRAP if (std::isnan(pos.tran.x) || std::isnan(pos.tran.y) || std::isnan(pos.tran.z) || @@ -1662,6 +1665,7 @@ int emcTrajProbe(const EmcPose& pos, int type, double vel, double ini_maxvel, do emcmotCommand.ini_maxvel = ini_maxvel; emcmotCommand.acc = acc; emcmotCommand.ini_maxjerk = ini_maxjerk; + emcmotCommand.vlimit_scale = vlimit_scale; emcmotCommand.probe_type = probe_type; return usrmotWriteEmcmotCommand(&emcmotCommand); diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index f6267ab056b..68f21925b18 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -1065,15 +1065,6 @@ double pmRigidTapTarget(PmRigidTap * const tap, double uu_per_rev) return target; } -/** Returns true if segment has ONLY rotary motion, false otherwise. */ -int tcPureRotaryCheck(TC_STRUCT const * const tc) -{ - return (tc->motion_type == TC_LINEAR) && - (tc->coords.line.xyz.tmag_zero) && - (tc->coords.line.uvw.tmag_zero); -} - - /** * Given a PmCircle and a circular segment, copy the circle in as the XYZ portion of the segment, then update the motion parameters. * NOTE: does not yet support ABC or UVW motion! diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index 5558a55e280..dcafd088e31 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -114,7 +114,6 @@ int tcFinalizeLength(TC_STRUCT * const tc); int tcClampVelocityByLength(TC_STRUCT * const tc); -int tcPureRotaryCheck(TC_STRUCT const * const tc); int tcSetCircleXYZ(TC_STRUCT * const tc, PmCircle const * const circ); diff --git a/src/emc/tp/tc_types.h b/src/emc/tp/tc_types.h index 135b34586ae..47018b24885 100644 --- a/src/emc/tp/tc_types.h +++ b/src/emc/tp/tc_types.h @@ -128,6 +128,7 @@ typedef struct { double reqvel; // vel requested by F word, calc'd by task double target_vel; // velocity to actually track, limited by other factors double maxvel; // max possible vel (feed override stops here) + double vlimit_scale; // vLimit scale for this move, 0 no limit double currentvel; // keep track of current step (vel * cycle_time) double last_move_length;// last move length double finalvel; // velocity to aim for at end of segment diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index ef9c96501b5..c75c795d08b 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -300,16 +300,15 @@ STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT con } double v_max_target = tcGetMaxTargetVel(tc, max_scale); - /* Check if the cartesian velocity limit applies and clip the maximum + /* Check if the velocity limit applies and clip the maximum * velocity. The vLimit is from the max velocity slider, and should * restrict the maximum velocity during non-synced moves and velocity * synchronization. However, position-synced moves have the target velocity * computed in the TP, so it would disrupt position tracking to apply this - * limit here. + * limit here. Canon scales vLimit per move, 0 for no limit. */ - if (!tcPureRotaryCheck(tc) && (tc->synchronized != TC_SYNC_POSITION)){ - /*tc_debug_print("Cartesian velocity limit active\n");*/ - v_max_target = fmin(v_max_target, tp->vLimit); + if (tc->vlimit_scale > 0.0 && tc->synchronized != TC_SYNC_POSITION) { + v_max_target = fmin(v_max_target, tp->vLimit * tc->vlimit_scale); } return v_max_target; @@ -860,6 +859,7 @@ STATIC int tpInitBlendArcFromPrev(TP_STRUCT const * const tp, // Skip syncdio setup since this blend extends the previous line blend_tc->syncdio = // enqueue the list of DIOs prev_tc->syncdio; // that need toggling + blend_tc->vlimit_scale = prev_tc->vlimit_scale; // find "helix" length for target double length; @@ -2118,7 +2118,8 @@ tc_blend_type_t tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) { */ int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, - double vel, double ini_maxvel, double acc, double ini_maxjerk, unsigned char enables, + double vel, double ini_maxvel, double acc, double ini_maxjerk, + double vlimit_scale, unsigned char enables, char atspeed, int indexer_jnum, struct state_tag_t tag) { if (tpErrorCheck(tp) < 0) { @@ -2148,6 +2149,7 @@ int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, ini_maxvel, acc, ini_maxjerk); + tc.vlimit_scale = vlimit_scale; // Setup line geometry pmLine9Init(&tc.coords.line, &tp->goalPos, @@ -2201,6 +2203,7 @@ int tpAddCircle(TP_STRUCT * const tp, double ini_maxvel, double acc, double ini_maxjerk, + double vlimit_scale, unsigned char enables, char atspeed, struct state_tag_t tag) @@ -2251,6 +2254,7 @@ int tpAddCircle(TP_STRUCT * const tp, ini_maxvel, acc, ini_maxjerk); + tc.vlimit_scale = vlimit_scale; //Reduce max velocity to match sample rate tcClampVelocityByLength(&tc); diff --git a/src/emc/tp/tp.h b/src/emc/tp/tp.h index e00b457ad23..7d2eff173b6 100644 --- a/src/emc/tp/tp.h +++ b/src/emc/tp/tp.h @@ -56,11 +56,13 @@ int tpAddRigidTap(TP_STRUCT * const tp, double scale, struct state_tag_t tag); int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, - double vel, double ini_maxvel, double acc, double ini_maxjerk, unsigned char enables, + double vel, double ini_maxvel, double acc, double ini_maxjerk, + double vlimit_scale, unsigned char enables, char atspeed, int indexrotary, struct state_tag_t tag); 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 ini_maxjerk, unsigned char enables, + double ini_maxvel, double acc, double ini_maxjerk, + double vlimit_scale, unsigned char enables, char atspeed, struct state_tag_t tag); int tpGetPos(TP_STRUCT const * const tp, EmcPose * const pos); int tpIsDone(TP_STRUCT * const tp); diff --git a/tests/axis-type/README b/tests/axis-type/README new file mode 100644 index 00000000000..ff3906cb806 --- /dev/null +++ b/tests/axis-type/README @@ -0,0 +1,5 @@ +Axis type through task, canon and motion: a mm machine with A configured +LINEAR and V ANGULAR (and wrapped). Under G20 an A word is inches and a V +word degrees; F is measured along A when A is the linear axis that moves, +and along V in degrees per minute when V moves alone, the max velocity +slider caps the same length; V wraps. diff --git a/tests/axis-type/axis-type.hal b/tests/axis-type/axis-type.hal new file mode 100644 index 00000000000..0406391056f --- /dev/null +++ b/tests/axis-type/axis-type.hal @@ -0,0 +1,17 @@ +# HAL file for the axis type test + +loadrt [KINS]KINEMATICS coordinates=xyzav +loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +net j0pos joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net j1pos joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net j2pos joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net j3pos joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net j4pos joint.4.motor-pos-cmd => joint.4.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +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/axis-type/axis-type.ini b/tests/axis-type/axis-type.ini new file mode 100644 index 00000000000..4851966fd61 --- /dev/null +++ b/tests/axis-type/axis-type.ini @@ -0,0 +1,124 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0x0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +PARAMETER_FILE = sim.var + +[EMCMOT] +EMCMOT = motmod +COMM_TIMEOUT = 4.0 +BASE_PERIOD = 0 +SERVO_PERIOD = 1000000 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[HAL] +HALFILE = axis-type.hal + +[TRAJ] +NO_FORCE_HOMING = 1 +COORDINATES = X Y Z A V +LINEAR_UNITS = mm +ANGULAR_UNITS = degree +DEFAULT_LINEAR_VELOCITY = 10 +MAX_LINEAR_VELOCITY = 400 +MAX_LINEAR_ACCELERATION = 50000 + +[EMCIO] +CYCLE_TIME = 0.100 + +[KINS] +KINEMATICS = trivkins +JOINTS = 5 + +[AXIS_X] +TYPE = LINEAR +MAX_VELOCITY = 100 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 + +[JOINT_0] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 1 +MIN_FERROR = 1 + +[AXIS_Y] +TYPE = LINEAR +MAX_VELOCITY = 100 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 + +[JOINT_1] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 1 +MIN_FERROR = 1 + +[AXIS_Z] +TYPE = LINEAR +MAX_VELOCITY = 100 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 + +[JOINT_2] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 1 +MIN_FERROR = 1 + +[AXIS_A] +TYPE = LINEAR +MAX_VELOCITY = 100 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 + +[JOINT_3] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 100 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1000 +MAX_LIMIT = 1000 +FERROR = 1 +MIN_FERROR = 1 + +[AXIS_V] +TYPE = ANGULAR +MAX_VELOCITY = 360 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1e+09 +MAX_LIMIT = 1e+09 +WRAPPED_ROTARY = 1 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 360 +MAX_ACCELERATION = 50000 +MIN_LIMIT = -1e+09 +MAX_LIMIT = 1e+09 +FERROR = 1 +MIN_FERROR = 1 diff --git a/tests/axis-type/checkresult b/tests/axis-type/checkresult new file mode 100755 index 00000000000..acfd40f1b32 --- /dev/null +++ b/tests/axis-type/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# Test passes if test-ui.py returns successfully +exit 0 diff --git a/tests/axis-type/test-ui.py b/tests/axis-type/test-ui.py new file mode 100755 index 00000000000..84efee09928 --- /dev/null +++ b/tests/axis-type/test-ui.py @@ -0,0 +1,120 @@ +#!/usr/bin/env python3 +# A LINEAR and V ANGULAR on a mm machine: units, feed and wrap through +# task, canon and motion. + +import linuxcnc +import sys +import time + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +A, V = 3, 7 + + +def fail(msg): + print("FAIL: " + msg) + c.state(linuxcnc.STATE_ESTOP) + sys.exit(1) + + +def wait_ready(timeout=20.0): + end = time.time() + timeout + while time.time() < end: + s.poll() + if s.task_state == linuxcnc.STATE_ESTOP and s.interp_state == linuxcnc.INTERP_IDLE \ + and s.axis_mask != 0 and s.linear_units != 0.0: + return + time.sleep(0.1) + fail("linuxcnc did not come up") + + +def errors(): + err = e.poll() + if err: + fail("error: %s" % err[1]) + + +def mdi(cmd, timeout=20.0): + """Run one MDI line to the end; return the largest current_vel seen + and how long the machine moved.""" + c.mdi(cmd) + peak = 0.0 + first = last = None + end = time.time() + timeout + time.sleep(0.05) + while time.time() < end: + s.poll() + errors() + if s.current_vel > 1e-6: + now = time.time() + first = first or now + last = now + peak = max(peak, s.current_vel) + if s.interp_state == linuxcnc.INTERP_IDLE and s.queue == 0 and s.inpos \ + and s.state == linuxcnc.RCS_DONE: + break + time.sleep(0.001) + else: + fail("%s did not finish" % cmd) + errors() + return peak, (last - first) if first else 0.0 + + +def near(what, got, want, tol): + print("%s: %.6g (want %.6g)" % (what, got, want)) + if abs(got - want) > tol: + fail("%s is %.6g, not %.6g" % (what, got, want)) + + +wait_ready() +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete() + +# G20: an A word is inches, a V word degrees +mdi("G20 G90 G94 G0 A1 V10") +s.poll() +near("A after G20 A1 (mm)", s.position[A], 25.4, 1e-6) +near("V after G20 V10 (deg)", s.position[V], 10.0, 1e-6) + +# V alone: F is degrees per minute, G20 or not +peak, _ = mdi("G20 G1 V90 F3600") +near("V alone at F3600, deg/s", peak, 60.0, 0.6) + +# X with A: F along X, the feed axis (G21 on its own line: F comes first) +mdi("G21") +peak, _ = mdi("G1 X10 A35.4 F600") +near("X with A at F600, mm/s", peak, 10.0, 0.1) + +# A with V: F along A; motion measures along V, 8 times longer, so it +# runs 8 times faster and the move still takes 1 s +peak, secs = mdi("G1 A45.4 V170 F600") +near("A with V at F600, motion rate", peak, 80.0, 0.8) +near("A with V at F600, seconds", secs, 1.0, 0.1) +s.poll() +near("A after the move", s.position[A], 45.4, 1e-6) +near("V after the move", s.position[V], 170.0, 1e-6) + +# the max velocity slider caps A alone and A with V (along A), not V alone +c.maxvel(2.0) +peak, _ = mdi("G1 V230 F3600") +near("V alone at F3600 under the slider, deg/s", peak, 60.0, 0.6) +peak, _ = mdi("G1 A47.4 F600") +near("A alone at F600 under the slider, mm/s", peak, 2.0, 0.02) +peak, secs = mdi("G1 A49.4 V246 F600") +near("A with V at F600 under the slider, motion rate", peak, 16.0, 0.16) +near("A with V at F600 under the slider, seconds", secs, 1.0, 0.1) +c.maxvel(400.0) + +# V wraps: 350 then 10 goes on to 370 +mdi("G0 V350") +mdi("G0 V10") +s.poll() +near("V wrapped", s.position[V], 370.0, 1e-6) + +c.state(linuxcnc.STATE_ESTOP) +print("PASS") +sys.exit(0) diff --git a/tests/axis-type/test.sh b/tests/axis-type/test.sh new file mode 100755 index 00000000000..42663324ad4 --- /dev/null +++ b/tests/axis-type/test.sh @@ -0,0 +1,3 @@ +#!/bin/bash -e + +linuxcnc -r axis-type.ini diff --git a/tests/interp/axis-type/expected b/tests/interp/axis-type/expected new file mode 100644 index 00000000000..635f5054d94 --- /dev/null +++ b/tests/interp/axis-type/expected @@ -0,0 +1,51 @@ + N..... USE_LENGTH_UNITS(CANON_UNITS_MM) + N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + N..... SET_XY_ROTATION(0.0000) + N..... SET_FEED_REFERENCE(CANON_XYZ) + N..... ON_RESET() + N..... COMMENT("A is a length, V an angle, on a mm machine") + N..... COMMENT("interpreter: feed mode set to units per minute") + N..... SET_FEED_MODE(0, 0) + N..... SET_FEED_RATE(0.0000) + N..... SELECT_PLANE(CANON_PLANE_XY) + N..... USE_LENGTH_UNITS(CANON_UNITS_MM) + N..... SET_FEED_RATE(100.0000) + N..... STRAIGHT_FEED(0.0000, 0.0000, 0.0000, 10.0000, 0.0000, 0.0000) + N..... COMMENT("G20: A is 10 mm, 0.3937 inch; V stays 10 degrees") + N..... USE_LENGTH_UNITS(CANON_UNITS_INCHES) + N..... STRAIGHT_FEED(1.0000, 0.0000, 0.0000, 0.3937, 0.0000, 0.0000) + N..... MESSAGE(" G20 A=0.393701 V=10.000000") + N..... USE_LENGTH_UNITS(CANON_UNITS_MM) + N..... MESSAGE(" G21 A=10.000000 V=10.000000") + N..... COMMENT("G93 feeds along A, the linear axis that moves, else along V") + N..... COMMENT("interpreter: feed mode set to inverse time") + N..... SET_FEED_MODE(0, 0) + N..... SET_FEED_RATE(20.0000) + N..... STRAIGHT_FEED(25.4000, 0.0000, 0.0000, 10.0000, 0.0000, 0.0000) + N..... SET_FEED_RATE(20.0000) + N..... STRAIGHT_FEED(25.4000, 0.0000, 0.0000, 20.0000, 0.0000, 0.0000) + N..... COMMENT("interpreter: feed mode set to units per minute") + N..... SET_FEED_MODE(0, 0) + N..... SET_FEED_RATE(0.0000) + N..... COMMENT("V wraps: 350 then 10 goes on to 370") + N..... STRAIGHT_TRAVERSE(25.4000, 0.0000, 0.0000, 20.0000, 0.0000, 0.0000) + N..... STRAIGHT_TRAVERSE(25.4000, 0.0000, 0.0000, 20.0000, 0.0000, 0.0000) + N..... MESSAGE(" wrapped V=370.000000") + N..... COMMENT("W is modulo: 400 reads 40; U, a length, ignores it") + N..... STRAIGHT_TRAVERSE(25.4000, 0.0000, 0.0000, 20.0000, 0.0000, 0.0000) + N..... MESSAGE(" modulo W=40.000000 U=400.000000") + N..... COMMENT("offsets: A in program units, V in degrees") + N..... USE_LENGTH_UNITS(CANON_UNITS_INCHES) + N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000) + N..... SET_XY_ROTATION(0.0000) + N..... USE_LENGTH_UNITS(CANON_UNITS_MM) + N..... MESSAGE(" G54 A=25.400000 V=5.000000") + N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 25.4000, 0.0000, 0.0000) + N..... SET_XY_ROTATION(0.0000) + N..... SET_FEED_MODE(0, 0) + N..... SET_FEED_RATE(0.0000) + N..... STOP_SPINDLE_TURNING(0) + N..... SET_SPINDLE_MODE(0 0.0000) + N..... PROGRAM_END() + N..... ON_RESET() diff --git a/tests/interp/axis-type/test.ini b/tests/interp/axis-type/test.ini new file mode 100644 index 00000000000..f8ad4e58e8c --- /dev/null +++ b/tests/interp/axis-type/test.ini @@ -0,0 +1,18 @@ +[TRAJ] +COORDINATES = X Y Z A B C U V W +LINEAR_UNITS = mm +ANGULAR_UNITS = degree + +[AXIS_A] +TYPE = LINEAR + +[AXIS_V] +TYPE = ANGULAR +WRAPPED_ROTARY = 1 + +[AXIS_U] +ROTARY_MODULO = 1 + +[AXIS_W] +TYPE = ANGULAR +ROTARY_MODULO = 1 diff --git a/tests/interp/axis-type/test.ngc b/tests/interp/axis-type/test.ngc new file mode 100644 index 00000000000..b917175d9aa --- /dev/null +++ b/tests/interp/axis-type/test.ngc @@ -0,0 +1,25 @@ +(A is a length, V an angle, on a mm machine) +G21 G90 G94 G17 +G1 F100 A10 V10 +(G20: A is 10 mm, 0.3937 inch; V stays 10 degrees) +G20 +G1 X1 +(debug, G20 A=#5423 V=#5427) +G21 +(debug, G21 A=#5423 V=#5427) +(G93 feeds along A, the linear axis that moves, else along V) +G93 G1 V20 F2 +G1 A20 V40 F2 +G94 +(V wraps: 350 then 10 goes on to 370) +G0 V350 +G0 V10 +(debug, wrapped V=#5427) +(W is modulo: 400 reads 40; U, a length, ignores it) +G0 W400 U400 +(debug, modulo W=#5428 U=#5426) +(offsets: A in program units, V in degrees) +G20 G10 L2 P1 A1 V5 +G21 +(debug, G54 A=#5224 V=#5228) +M2 diff --git a/tests/interp/axis-type/test.sh b/tests/interp/axis-type/test.sh new file mode 100755 index 00000000000..65d2a707e9c --- /dev/null +++ b/tests/interp/axis-type/test.sh @@ -0,0 +1,3 @@ +#!/bin/bash +rs274 -g -i test.ini test.ngc | awk '{$1=""; print}' +exit "${PIPESTATUS[0]}" From e6963329c07aa2bb3f252ceaa9877355a1a9674e Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 24 Sep 2026 23:14:19 +1000 Subject: [PATCH 2/2] Name the axis numbers: AXIS_X to AXIS_W The interpreter and canon index the per axis tables and conversions by number, 0 X to 8 W. An enum in axis_kinds.hh names them, so axis_wrapped[AXIS_U] reads without counting. --- src/emc/ini/axis_kinds.hh | 7 + src/emc/rs274ngc/interp_convert.cc | 356 ++++++++++++------------- src/emc/rs274ngc/interp_find.cc | 120 ++++----- src/emc/rs274ngc/interp_internal.cc | 12 +- src/emc/rs274ngc/interp_internal.hh | 2 +- src/emc/rs274ngc/interp_namedparams.cc | 24 +- src/emc/rs274ngc/interp_queue.cc | 2 +- src/emc/rs274ngc/interpmodule.cc | 24 +- src/emc/rs274ngc/rs274ngc_pre.cc | 36 +-- src/emc/rs274ngc/units.h | 2 +- src/emc/task/emccanon.cc | 178 ++++++------- 11 files changed, 385 insertions(+), 378 deletions(-) diff --git a/src/emc/ini/axis_kinds.hh b/src/emc/ini/axis_kinds.hh index 86b8b673aa2..3b756029347 100644 --- a/src/emc/ini/axis_kinds.hh +++ b/src/emc/ini/axis_kinds.hh @@ -16,6 +16,13 @@ #include #include +/* Axis numbers, the bit of each axis in the masks below */ +enum AxisIndex { + AXIS_X, AXIS_Y, AXIS_Z, + AXIS_A, AXIS_B, AXIS_C, + AXIS_U, AXIS_V, AXIS_W, +}; + #define AXIS_KINDS_ALL 0x1ffu /* X Y Z A B C U V W, bit 0 is X */ #define AXIS_KINDS_ABC 0x038u /* angular unless the INI says otherwise */ #define AXIS_KINDS_XYZ 0x007u /* the feed group unless the INI says otherwise */ diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 552625420f5..35e9d50027e 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -1641,27 +1641,27 @@ int Interp::convert_axis_offsets(int g_code, //!< g_code being executed (mus CHKS((settings->cutter_comp_side != CUTTER_COMP::OFF), /* not "== true" */ NCE_CANNOT_CHANGE_AXIS_OFFSETS_WITH_CUTTER_RADIUS_COMP); - CHKS((block->a_flag && settings->axis_wrapped[3] && + CHKS((block->a_flag && settings->axis_wrapped[AXIS_A] && (block->a_number <= -360.0 || block->a_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->a_number, 'A'); - CHKS((block->b_flag && settings->axis_wrapped[4] && + CHKS((block->b_flag && settings->axis_wrapped[AXIS_B] && (block->b_number <= -360.0 || block->b_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->b_number, 'B'); - CHKS((block->c_flag && settings->axis_wrapped[5] && + CHKS((block->c_flag && settings->axis_wrapped[AXIS_C] && (block->c_number <= -360.0 || block->c_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->c_number, 'C'); - CHKS((block->u_flag && settings->axis_wrapped[6] && + CHKS((block->u_flag && settings->axis_wrapped[AXIS_U] && (block->u_number <= -360.0 || block->u_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->u_number, 'U'); - CHKS((block->v_flag && settings->axis_wrapped[7] && + CHKS((block->v_flag && settings->axis_wrapped[AXIS_V] && (block->v_number <= -360.0 || block->v_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->v_number, 'V'); - CHKS((block->w_flag && settings->axis_wrapped[8] && + CHKS((block->w_flag && settings->axis_wrapped[AXIS_W] && (block->w_number <= -360.0 || block->w_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->w_number, 'W'); @@ -1772,12 +1772,12 @@ int Interp::convert_axis_offsets(int g_code, //!< g_code being executed (mus pars[5211] = PROGRAM_TO_USER_LEN(settings->axis_offset_x); pars[5212] = PROGRAM_TO_USER_LEN(settings->axis_offset_y); pars[5213] = PROGRAM_TO_USER_LEN(settings->axis_offset_z); - pars[5214] = PROGRAM_TO_USER_AX(3, settings->AA_axis_offset); - pars[5215] = PROGRAM_TO_USER_AX(4, settings->BB_axis_offset); - pars[5216] = PROGRAM_TO_USER_AX(5, settings->CC_axis_offset); - pars[5217] = PROGRAM_TO_USER_AX(6, settings->u_axis_offset); - pars[5218] = PROGRAM_TO_USER_AX(7, settings->v_axis_offset); - pars[5219] = PROGRAM_TO_USER_AX(8, settings->w_axis_offset); + pars[5214] = PROGRAM_TO_USER_AX(AXIS_A, settings->AA_axis_offset); + pars[5215] = PROGRAM_TO_USER_AX(AXIS_B, settings->BB_axis_offset); + pars[5216] = PROGRAM_TO_USER_AX(AXIS_C, settings->CC_axis_offset); + pars[5217] = PROGRAM_TO_USER_AX(AXIS_U, settings->u_axis_offset); + pars[5218] = PROGRAM_TO_USER_AX(AXIS_V, settings->v_axis_offset); + pars[5219] = PROGRAM_TO_USER_AX(AXIS_W, settings->w_axis_offset); } else if ((g_code == G_92_1) || (g_code == G_92_2)) { pars[5210] = 0.0; @@ -1822,27 +1822,27 @@ int Interp::convert_axis_offsets(int g_code, //!< g_code being executed (mus settings->current_z = settings->current_z + settings->axis_offset_z - USER_TO_PROGRAM_LEN(pars[5213]); settings->AA_current = - settings->AA_current + settings->AA_axis_offset - USER_TO_PROGRAM_AX(3, pars[5214]); + settings->AA_current + settings->AA_axis_offset - USER_TO_PROGRAM_AX(AXIS_A, pars[5214]); settings->BB_current = - settings->BB_current + settings->BB_axis_offset - USER_TO_PROGRAM_AX(4, pars[5215]); + settings->BB_current + settings->BB_axis_offset - USER_TO_PROGRAM_AX(AXIS_B, pars[5215]); settings->CC_current = - settings->CC_current + settings->CC_axis_offset - USER_TO_PROGRAM_AX(5, pars[5216]); + settings->CC_current + settings->CC_axis_offset - USER_TO_PROGRAM_AX(AXIS_C, pars[5216]); settings->u_current = - settings->u_current + settings->u_axis_offset - USER_TO_PROGRAM_AX(6, pars[5217]); + settings->u_current + settings->u_axis_offset - USER_TO_PROGRAM_AX(AXIS_U, pars[5217]); settings->v_current = - settings->v_current + settings->v_axis_offset - USER_TO_PROGRAM_AX(7, pars[5218]); + settings->v_current + settings->v_axis_offset - USER_TO_PROGRAM_AX(AXIS_V, pars[5218]); settings->w_current = - settings->w_current + settings->w_axis_offset - USER_TO_PROGRAM_AX(8, pars[5219]); + settings->w_current + settings->w_axis_offset - USER_TO_PROGRAM_AX(AXIS_W, pars[5219]); settings->axis_offset_x = USER_TO_PROGRAM_LEN(pars[5211]); settings->axis_offset_y = USER_TO_PROGRAM_LEN(pars[5212]); settings->axis_offset_z = USER_TO_PROGRAM_LEN(pars[5213]); - settings->AA_axis_offset = USER_TO_PROGRAM_AX(3, pars[5214]); - settings->BB_axis_offset = USER_TO_PROGRAM_AX(4, pars[5215]); - settings->CC_axis_offset = USER_TO_PROGRAM_AX(5, pars[5216]); - settings->u_axis_offset = USER_TO_PROGRAM_AX(6, pars[5217]); - settings->v_axis_offset = USER_TO_PROGRAM_AX(7, pars[5218]); - settings->w_axis_offset = USER_TO_PROGRAM_AX(8, pars[5219]); + settings->AA_axis_offset = USER_TO_PROGRAM_AX(AXIS_A, pars[5214]); + settings->BB_axis_offset = USER_TO_PROGRAM_AX(AXIS_B, pars[5215]); + settings->CC_axis_offset = USER_TO_PROGRAM_AX(AXIS_C, pars[5216]); + settings->u_axis_offset = USER_TO_PROGRAM_AX(AXIS_U, pars[5217]); + settings->v_axis_offset = USER_TO_PROGRAM_AX(AXIS_V, pars[5218]); + settings->w_axis_offset = USER_TO_PROGRAM_AX(AXIS_W, pars[5219]); SET_G92_OFFSET(settings->axis_offset_x, settings->axis_offset_y, @@ -2430,12 +2430,12 @@ int Interp::convert_coordinate_system(int g_code, //!< g_code called (mus settings->origin_offset_x = USER_TO_PROGRAM_LEN(parameters[5201 + (origin * 20)]); settings->origin_offset_y = USER_TO_PROGRAM_LEN(parameters[5202 + (origin * 20)]); settings->origin_offset_z = USER_TO_PROGRAM_LEN(parameters[5203 + (origin * 20)]); - settings->AA_origin_offset = USER_TO_PROGRAM_AX(3, parameters[5204 + (origin * 20)]); - settings->BB_origin_offset = USER_TO_PROGRAM_AX(4, parameters[5205 + (origin * 20)]); - settings->CC_origin_offset = USER_TO_PROGRAM_AX(5, parameters[5206 + (origin * 20)]); - settings->u_origin_offset = USER_TO_PROGRAM_AX(6, parameters[5207 + (origin * 20)]); - settings->v_origin_offset = USER_TO_PROGRAM_AX(7, parameters[5208 + (origin * 20)]); - settings->w_origin_offset = USER_TO_PROGRAM_AX(8, parameters[5209 + (origin * 20)]); + settings->AA_origin_offset = USER_TO_PROGRAM_AX(AXIS_A, parameters[5204 + (origin * 20)]); + settings->BB_origin_offset = USER_TO_PROGRAM_AX(AXIS_B, parameters[5205 + (origin * 20)]); + settings->CC_origin_offset = USER_TO_PROGRAM_AX(AXIS_C, parameters[5206 + (origin * 20)]); + settings->u_origin_offset = USER_TO_PROGRAM_AX(AXIS_U, parameters[5207 + (origin * 20)]); + settings->v_origin_offset = USER_TO_PROGRAM_AX(AXIS_V, parameters[5208 + (origin * 20)]); + settings->w_origin_offset = USER_TO_PROGRAM_AX(AXIS_W, parameters[5209 + (origin * 20)]); settings->rotation_xy = parameters[5210 + (origin * 20)]; SET_G5X_OFFSET(origin, @@ -3095,39 +3095,39 @@ int Interp::convert_savehome(int code, block_pointer /*block*/, setup_pointer s) x = PROGRAM_TO_USER_LEN(x + s->tool_offset.tran.x + s->origin_offset_x); y = PROGRAM_TO_USER_LEN(y + s->tool_offset.tran.y + s->origin_offset_y); double z = PROGRAM_TO_USER_LEN(s->current_z + s->tool_offset.tran.z + s->origin_offset_z + s->axis_offset_z); - double a = PROGRAM_TO_USER_AX(3, s->AA_current + s->tool_offset.a + s->AA_origin_offset + s->AA_axis_offset); - double b = PROGRAM_TO_USER_AX(4, s->BB_current + s->tool_offset.b + s->BB_origin_offset + s->BB_axis_offset); - double c = PROGRAM_TO_USER_AX(5, s->CC_current + s->tool_offset.c + s->CC_origin_offset + s->CC_axis_offset); - double u = PROGRAM_TO_USER_AX(6, s->u_current + s->tool_offset.u + s->u_origin_offset + s->u_axis_offset); - double v = PROGRAM_TO_USER_AX(7, s->v_current + s->tool_offset.v + s->v_origin_offset + s->v_axis_offset); - double w = PROGRAM_TO_USER_AX(8, s->w_current + s->tool_offset.w + s->w_origin_offset + s->w_axis_offset); - - if(s->axis_wrapped[3]) { + double a = PROGRAM_TO_USER_AX(AXIS_A, s->AA_current + s->tool_offset.a + s->AA_origin_offset + s->AA_axis_offset); + double b = PROGRAM_TO_USER_AX(AXIS_B, s->BB_current + s->tool_offset.b + s->BB_origin_offset + s->BB_axis_offset); + double c = PROGRAM_TO_USER_AX(AXIS_C, s->CC_current + s->tool_offset.c + s->CC_origin_offset + s->CC_axis_offset); + double u = PROGRAM_TO_USER_AX(AXIS_U, s->u_current + s->tool_offset.u + s->u_origin_offset + s->u_axis_offset); + double v = PROGRAM_TO_USER_AX(AXIS_V, s->v_current + s->tool_offset.v + s->v_origin_offset + s->v_axis_offset); + double w = PROGRAM_TO_USER_AX(AXIS_W, s->w_current + s->tool_offset.w + s->w_origin_offset + s->w_axis_offset); + + if(s->axis_wrapped[AXIS_A]) { a = fmod(a, 360.0); if(a<0) a += 360.0; } - if(s->axis_wrapped[4]) { + if(s->axis_wrapped[AXIS_B]) { b = fmod(b, 360.0); if(b<0) b += 360.0; } - if(s->axis_wrapped[5]) { + if(s->axis_wrapped[AXIS_C]) { c = fmod(c, 360.0); if(c<0) c += 360.0; } - if(s->axis_wrapped[6]) { + if(s->axis_wrapped[AXIS_U]) { u = fmod(u, 360.0); if(u<0) u += 360.0; } - if(s->axis_wrapped[7]) { + if(s->axis_wrapped[AXIS_V]) { v = fmod(v, 360.0); if(v<0) v += 360.0; } - if(s->axis_wrapped[8]) { + if(s->axis_wrapped[AXIS_W]) { w = fmod(w, 360.0); if(w<0) w += 360.0; } @@ -3317,18 +3317,18 @@ int Interp::convert_home(int move, //!< G-code, must be G_28 or G_30 // move indexers first, one at a time // JOINTS_AXES settings->*_indexer_jnum == -1 means notused - if (AA_end != settings->AA_current && (-1 != settings->axis_indexer_jnum[3]) ) - issue_straight_index(3,settings->axis_indexer_jnum[3], AA_end, block->line_number, settings); - if (BB_end != settings->BB_current && (-1 != settings->axis_indexer_jnum[4]) ) - issue_straight_index(4,settings->axis_indexer_jnum[4], BB_end, block->line_number, settings); - if (CC_end != settings->CC_current && (-1 != settings->axis_indexer_jnum[5]) ) - issue_straight_index(5,settings->axis_indexer_jnum[5], CC_end, block->line_number, settings); - if (u_end != settings->u_current && (-1 != settings->axis_indexer_jnum[6]) ) - issue_straight_index(6,settings->axis_indexer_jnum[6], u_end, block->line_number, settings); - if (v_end != settings->v_current && (-1 != settings->axis_indexer_jnum[7]) ) - issue_straight_index(7,settings->axis_indexer_jnum[7], v_end, block->line_number, settings); - if (w_end != settings->w_current && (-1 != settings->axis_indexer_jnum[8]) ) - issue_straight_index(8,settings->axis_indexer_jnum[8], w_end, block->line_number, settings); + if (AA_end != settings->AA_current && (-1 != settings->axis_indexer_jnum[AXIS_A]) ) + issue_straight_index(AXIS_A,settings->axis_indexer_jnum[AXIS_A], AA_end, block->line_number, settings); + if (BB_end != settings->BB_current && (-1 != settings->axis_indexer_jnum[AXIS_B]) ) + issue_straight_index(AXIS_B,settings->axis_indexer_jnum[AXIS_B], BB_end, block->line_number, settings); + if (CC_end != settings->CC_current && (-1 != settings->axis_indexer_jnum[AXIS_C]) ) + issue_straight_index(AXIS_C,settings->axis_indexer_jnum[AXIS_C], CC_end, block->line_number, settings); + if (u_end != settings->u_current && (-1 != settings->axis_indexer_jnum[AXIS_U]) ) + issue_straight_index(AXIS_U,settings->axis_indexer_jnum[AXIS_U], u_end, block->line_number, settings); + if (v_end != settings->v_current && (-1 != settings->axis_indexer_jnum[AXIS_V]) ) + issue_straight_index(AXIS_V,settings->axis_indexer_jnum[AXIS_V], v_end, block->line_number, settings); + if (w_end != settings->w_current && (-1 != settings->axis_indexer_jnum[AXIS_W]) ) + issue_straight_index(AXIS_W,settings->axis_indexer_jnum[AXIS_W], w_end, block->line_number, settings); // Create a state tag and dump it to canon write_canon_state_tag(block, settings); @@ -3351,12 +3351,12 @@ int Interp::convert_home(int move, //!< G-code, must be G_28 or G_30 find_relative(USER_TO_PROGRAM_LEN(parameters[5161]), USER_TO_PROGRAM_LEN(parameters[5162]), USER_TO_PROGRAM_LEN(parameters[5163]), - USER_TO_PROGRAM_AX(3, parameters[5164]), - USER_TO_PROGRAM_AX(4, parameters[5165]), - USER_TO_PROGRAM_AX(5, parameters[5166]), - USER_TO_PROGRAM_AX(6, parameters[5167]), - USER_TO_PROGRAM_AX(7, parameters[5168]), - USER_TO_PROGRAM_AX(8, parameters[5169]), + USER_TO_PROGRAM_AX(AXIS_A, parameters[5164]), + USER_TO_PROGRAM_AX(AXIS_B, parameters[5165]), + USER_TO_PROGRAM_AX(AXIS_C, parameters[5166]), + USER_TO_PROGRAM_AX(AXIS_U, parameters[5167]), + USER_TO_PROGRAM_AX(AXIS_V, parameters[5168]), + USER_TO_PROGRAM_AX(AXIS_W, parameters[5169]), &end_x_home, &end_y_home, &end_z_home, &AA_end_home, &BB_end_home, &CC_end_home, &u_end_home, &v_end_home, &w_end_home, settings); @@ -3364,12 +3364,12 @@ int Interp::convert_home(int move, //!< G-code, must be G_28 or G_30 find_relative(USER_TO_PROGRAM_LEN(parameters[5181]), USER_TO_PROGRAM_LEN(parameters[5182]), USER_TO_PROGRAM_LEN(parameters[5183]), - USER_TO_PROGRAM_AX(3, parameters[5184]), - USER_TO_PROGRAM_AX(4, parameters[5185]), - USER_TO_PROGRAM_AX(5, parameters[5186]), - USER_TO_PROGRAM_AX(6, parameters[5187]), - USER_TO_PROGRAM_AX(7, parameters[5188]), - USER_TO_PROGRAM_AX(8, parameters[5189]), + USER_TO_PROGRAM_AX(AXIS_A, parameters[5184]), + USER_TO_PROGRAM_AX(AXIS_B, parameters[5185]), + USER_TO_PROGRAM_AX(AXIS_C, parameters[5186]), + USER_TO_PROGRAM_AX(AXIS_U, parameters[5187]), + USER_TO_PROGRAM_AX(AXIS_V, parameters[5188]), + USER_TO_PROGRAM_AX(AXIS_W, parameters[5189]), &end_x_home, &end_y_home, &end_z_home, &AA_end_home, &BB_end_home, &CC_end_home, &u_end_home, &v_end_home, &w_end_home, settings); @@ -3408,18 +3408,18 @@ int Interp::convert_home(int move, //!< G-code, must be G_28 or G_30 // move indexers first, one at a time // JOINTS_AXES settings->*_indexer_jnum == -1 means notused - if (AA_end != settings->AA_current && (-1 != settings->axis_indexer_jnum[3]) ) - issue_straight_index(3,settings->axis_indexer_jnum[3], AA_end, block->line_number, settings); - if (BB_end != settings->BB_current && (-1 != settings->axis_indexer_jnum[4]) ) - issue_straight_index(4,settings->axis_indexer_jnum[4], BB_end, block->line_number, settings); - if (CC_end != settings->CC_current && (-1 != settings->axis_indexer_jnum[5]) ) - issue_straight_index(5,settings->axis_indexer_jnum[5], CC_end, block->line_number, settings); - if (u_end != settings->u_current && (-1 != settings->axis_indexer_jnum[6]) ) - issue_straight_index(6,settings->axis_indexer_jnum[6], u_end, block->line_number, settings); - if (v_end != settings->v_current && (-1 != settings->axis_indexer_jnum[7]) ) - issue_straight_index(7,settings->axis_indexer_jnum[7], v_end, block->line_number, settings); - if (w_end != settings->w_current && (-1 != settings->axis_indexer_jnum[8]) ) - issue_straight_index(8,settings->axis_indexer_jnum[8], w_end, block->line_number, settings); + if (AA_end != settings->AA_current && (-1 != settings->axis_indexer_jnum[AXIS_A]) ) + issue_straight_index(AXIS_A,settings->axis_indexer_jnum[AXIS_A], AA_end, block->line_number, settings); + if (BB_end != settings->BB_current && (-1 != settings->axis_indexer_jnum[AXIS_B]) ) + issue_straight_index(AXIS_B,settings->axis_indexer_jnum[AXIS_B], BB_end, block->line_number, settings); + if (CC_end != settings->CC_current && (-1 != settings->axis_indexer_jnum[AXIS_C]) ) + issue_straight_index(AXIS_C,settings->axis_indexer_jnum[AXIS_C], CC_end, block->line_number, settings); + if (u_end != settings->u_current && (-1 != settings->axis_indexer_jnum[AXIS_U]) ) + issue_straight_index(AXIS_U,settings->axis_indexer_jnum[AXIS_U], u_end, block->line_number, settings); + if (v_end != settings->v_current && (-1 != settings->axis_indexer_jnum[AXIS_V]) ) + issue_straight_index(AXIS_V,settings->axis_indexer_jnum[AXIS_V], v_end, block->line_number, settings); + if (w_end != settings->w_current && (-1 != settings->axis_indexer_jnum[AXIS_W]) ) + issue_straight_index(AXIS_W,settings->axis_indexer_jnum[AXIS_W], w_end, block->line_number, settings); STRAIGHT_TRAVERSE(block->line_number, end_x, end_y, end_z, AA_end, BB_end, CC_end, @@ -4545,14 +4545,14 @@ int Interp::convert_motion(int motion, //!< g_code for a line, arc, canned cyc block->u_flag, block->v_flag, block->w_flag}; int indexed = -1; // the first axis word on a locking indexer - for (int n = 8; n >= 3; n--) { + for (int n = AXIS_W; n >= AXIS_A; n--) { if (axis_flag[n] && -1 != settings->axis_indexer_jnum[n]) { indexed = n; } } - for (int n = 3; n < 9 && motion != G_0; n++) { + for (int n = AXIS_A; n <= AXIS_W && motion != G_0; n++) { CHKS((axis_flag[n] && -1 != settings->axis_indexer_jnum[n]), (_("Indexing axis %c can only be moved with G0")), "XYZABCUVW"[n]); } - for (int n = 3; n < 9; n++) { + for (int n = AXIS_A; n <= AXIS_W; n++) { if (!axis_flag[n] || -1 == settings->axis_indexer_jnum[n]) { continue; } for (int other = 0; other < 9; other++) { CHKS((other != n && axis_flag[other]), @@ -4752,17 +4752,17 @@ int Interp::convert_setup_tool(block_pointer block, setup_pointer settings) { if(block->z_flag) settings->tool_table[idx].offset.tran.z = PROGRAM_TO_USER_LEN(block->z_number); if(block->a_flag) - settings->tool_table[idx].offset.a = PROGRAM_TO_USER_AX(3, block->a_number); + settings->tool_table[idx].offset.a = PROGRAM_TO_USER_AX(AXIS_A, block->a_number); if(block->b_flag) - settings->tool_table[idx].offset.b = PROGRAM_TO_USER_AX(4, block->b_number); + settings->tool_table[idx].offset.b = PROGRAM_TO_USER_AX(AXIS_B, block->b_number); if(block->c_flag) - settings->tool_table[idx].offset.c = PROGRAM_TO_USER_AX(5, block->c_number); + settings->tool_table[idx].offset.c = PROGRAM_TO_USER_AX(AXIS_C, block->c_number); if(block->u_flag) - settings->tool_table[idx].offset.u = PROGRAM_TO_USER_AX(6, block->u_number); + settings->tool_table[idx].offset.u = PROGRAM_TO_USER_AX(AXIS_U, block->u_number); if(block->v_flag) - settings->tool_table[idx].offset.v = PROGRAM_TO_USER_AX(7, block->v_number); + settings->tool_table[idx].offset.v = PROGRAM_TO_USER_AX(AXIS_V, block->v_number); if(block->w_flag) - settings->tool_table[idx].offset.w = PROGRAM_TO_USER_AX(8, block->w_number); + settings->tool_table[idx].offset.w = PROGRAM_TO_USER_AX(AXIS_W, block->w_number); } else { int to_fixture = block->l_number == 11; int destination_system = to_fixture? 9 : settings->origin_index; // maybe 9 (g59.3) should be user configurable? @@ -4779,12 +4779,12 @@ int Interp::convert_setup_tool(block_pointer block, setup_pointer settings) { tx += USER_TO_PROGRAM_LEN(settings->parameters[5211]); ty += USER_TO_PROGRAM_LEN(settings->parameters[5212]); tz += USER_TO_PROGRAM_LEN(settings->parameters[5213]); - ta += USER_TO_PROGRAM_AX(3, settings->parameters[5214]); - tb += USER_TO_PROGRAM_AX(4, settings->parameters[5215]); - tc += USER_TO_PROGRAM_AX(5, settings->parameters[5216]); - tu += USER_TO_PROGRAM_AX(6, settings->parameters[5217]); - tv += USER_TO_PROGRAM_AX(7, settings->parameters[5218]); - tw += USER_TO_PROGRAM_AX(8, settings->parameters[5219]); + ta += USER_TO_PROGRAM_AX(AXIS_A, settings->parameters[5214]); + tb += USER_TO_PROGRAM_AX(AXIS_B, settings->parameters[5215]); + tc += USER_TO_PROGRAM_AX(AXIS_C, settings->parameters[5216]); + tu += USER_TO_PROGRAM_AX(AXIS_U, settings->parameters[5217]); + tv += USER_TO_PROGRAM_AX(AXIS_V, settings->parameters[5218]); + tw += USER_TO_PROGRAM_AX(AXIS_W, settings->parameters[5219]); } @@ -4831,17 +4831,17 @@ int Interp::convert_setup_tool(block_pointer block, setup_pointer settings) { if(block->z_flag) settings->tool_table[idx].offset.tran.z = PROGRAM_TO_USER_LEN(tz - block->z_number); if(block->a_flag) - settings->tool_table[idx].offset.a = PROGRAM_TO_USER_AX(3, ta - block->a_number); + settings->tool_table[idx].offset.a = PROGRAM_TO_USER_AX(AXIS_A, ta - block->a_number); if(block->b_flag) - settings->tool_table[idx].offset.b = PROGRAM_TO_USER_AX(4, tb - block->b_number); + settings->tool_table[idx].offset.b = PROGRAM_TO_USER_AX(AXIS_B, tb - block->b_number); if(block->c_flag) - settings->tool_table[idx].offset.c = PROGRAM_TO_USER_AX(5, tc - block->c_number); + settings->tool_table[idx].offset.c = PROGRAM_TO_USER_AX(AXIS_C, tc - block->c_number); if(block->u_flag) - settings->tool_table[idx].offset.u = PROGRAM_TO_USER_AX(6, tu - block->u_number); + settings->tool_table[idx].offset.u = PROGRAM_TO_USER_AX(AXIS_U, tu - block->u_number); if(block->v_flag) - settings->tool_table[idx].offset.v = PROGRAM_TO_USER_AX(7, tv - block->v_number); + settings->tool_table[idx].offset.v = PROGRAM_TO_USER_AX(AXIS_V, tv - block->v_number); if(block->w_flag) - settings->tool_table[idx].offset.w = PROGRAM_TO_USER_AX(8, tw - block->w_number); + settings->tool_table[idx].offset.w = PROGRAM_TO_USER_AX(AXIS_W, tw - block->w_number); } if(block->r_flag) settings->tool_table[idx].diameter = PROGRAM_TO_USER_LEN(block->r_number) * 2.; @@ -4987,22 +4987,22 @@ int Interp::convert_setup(block_pointer block, //!< pointer to a block of RS27 p_int = settings->origin_index; } - CHKS((block->l_number == 20 && block->a_flag && settings->axis_wrapped[3] && + CHKS((block->l_number == 20 && block->a_flag && settings->axis_wrapped[AXIS_A] && (block->a_number <= -360.0 || block->a_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->a_number, 'A'); - CHKS((block->l_number == 20 && block->b_flag && settings->axis_wrapped[4] && + CHKS((block->l_number == 20 && block->b_flag && settings->axis_wrapped[AXIS_B] && (block->b_number <= -360.0 || block->b_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->b_number, 'B'); - CHKS((block->l_number == 20 && block->c_flag && settings->axis_wrapped[5] && + CHKS((block->l_number == 20 && block->c_flag && settings->axis_wrapped[AXIS_C] && (block->c_number <= -360.0 || block->c_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->c_number, 'C'); - CHKS((block->l_number == 20 && block->u_flag && settings->axis_wrapped[6] && + CHKS((block->l_number == 20 && block->u_flag && settings->axis_wrapped[AXIS_U] && (block->u_number <= -360.0 || block->u_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->u_number, 'U'); - CHKS((block->l_number == 20 && block->v_flag && settings->axis_wrapped[7] && + CHKS((block->l_number == 20 && block->v_flag && settings->axis_wrapped[AXIS_V] && (block->v_number <= -360.0 || block->v_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->v_number, 'V'); - CHKS((block->l_number == 20 && block->w_flag && settings->axis_wrapped[8] && + CHKS((block->l_number == 20 && block->w_flag && settings->axis_wrapped[AXIS_W] && (block->w_number <= -360.0 || block->w_number >= 360.0)), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), block->w_number, 'W'); @@ -5078,45 +5078,45 @@ int Interp::convert_setup(block_pointer block, //!< pointer to a block of RS27 if (block->a_flag) { a = block->a_number; - if (block->l_number == 20) a = ca + USER_TO_PROGRAM_AX(3, parameters[5204 + (p_int * 20)]) - a; - parameters[5204 + (p_int * 20)] = PROGRAM_TO_USER_AX(3, a); + if (block->l_number == 20) a = ca + USER_TO_PROGRAM_AX(AXIS_A, parameters[5204 + (p_int * 20)]) - a; + parameters[5204 + (p_int * 20)] = PROGRAM_TO_USER_AX(AXIS_A, a); } else - a = USER_TO_PROGRAM_AX(3, parameters[5204 + (p_int * 20)]); + a = USER_TO_PROGRAM_AX(AXIS_A, parameters[5204 + (p_int * 20)]); if (block->b_flag) { b = block->b_number; - if (block->l_number == 20) b = cb + USER_TO_PROGRAM_AX(4, parameters[5205 + (p_int * 20)]) - b; - parameters[5205 + (p_int * 20)] = PROGRAM_TO_USER_AX(4, b); + if (block->l_number == 20) b = cb + USER_TO_PROGRAM_AX(AXIS_B, parameters[5205 + (p_int * 20)]) - b; + parameters[5205 + (p_int * 20)] = PROGRAM_TO_USER_AX(AXIS_B, b); } else - b = USER_TO_PROGRAM_AX(4, parameters[5205 + (p_int * 20)]); + b = USER_TO_PROGRAM_AX(AXIS_B, parameters[5205 + (p_int * 20)]); if (block->c_flag) { c = block->c_number; - if (block->l_number == 20) c = cc + USER_TO_PROGRAM_AX(5, parameters[5206 + (p_int * 20)]) - c; - parameters[5206 + (p_int * 20)] = PROGRAM_TO_USER_AX(5, c); + if (block->l_number == 20) c = cc + USER_TO_PROGRAM_AX(AXIS_C, parameters[5206 + (p_int * 20)]) - c; + parameters[5206 + (p_int * 20)] = PROGRAM_TO_USER_AX(AXIS_C, c); } else - c = USER_TO_PROGRAM_AX(5, parameters[5206 + (p_int * 20)]); + c = USER_TO_PROGRAM_AX(AXIS_C, parameters[5206 + (p_int * 20)]); if (block->u_flag) { u = block->u_number; - if (block->l_number == 20) u = cu + USER_TO_PROGRAM_AX(6, parameters[5207 + (p_int * 20)]) - u; - parameters[5207 + (p_int * 20)] = PROGRAM_TO_USER_AX(6, u); + if (block->l_number == 20) u = cu + USER_TO_PROGRAM_AX(AXIS_U, parameters[5207 + (p_int * 20)]) - u; + parameters[5207 + (p_int * 20)] = PROGRAM_TO_USER_AX(AXIS_U, u); } else - u = USER_TO_PROGRAM_AX(6, parameters[5207 + (p_int * 20)]); + u = USER_TO_PROGRAM_AX(AXIS_U, parameters[5207 + (p_int * 20)]); if (block->v_flag) { v = block->v_number; - if (block->l_number == 20) v = cv + USER_TO_PROGRAM_AX(7, parameters[5208 + (p_int * 20)]) - v; - parameters[5208 + (p_int * 20)] = PROGRAM_TO_USER_AX(7, v); + if (block->l_number == 20) v = cv + USER_TO_PROGRAM_AX(AXIS_V, parameters[5208 + (p_int * 20)]) - v; + parameters[5208 + (p_int * 20)] = PROGRAM_TO_USER_AX(AXIS_V, v); } else - v = USER_TO_PROGRAM_AX(7, parameters[5208 + (p_int * 20)]); + v = USER_TO_PROGRAM_AX(AXIS_V, parameters[5208 + (p_int * 20)]); if (block->w_flag) { w = block->w_number; - if (block->l_number == 20) w = cw + USER_TO_PROGRAM_AX(8, parameters[5209 + (p_int * 20)]) - w; - parameters[5209 + (p_int * 20)] = PROGRAM_TO_USER_AX(8, w); + if (block->l_number == 20) w = cw + USER_TO_PROGRAM_AX(AXIS_W, parameters[5209 + (p_int * 20)]) - w; + parameters[5209 + (p_int * 20)] = PROGRAM_TO_USER_AX(AXIS_W, w); } else - w = USER_TO_PROGRAM_AX(8, parameters[5209 + (p_int * 20)]); + w = USER_TO_PROGRAM_AX(AXIS_W, parameters[5209 + (p_int * 20)]); if (p_int == settings->origin_index) { /* system is currently used */ @@ -5472,12 +5472,12 @@ int Interp::convert_stop(block_pointer block, //!< pointer to a block of RS27 settings->origin_offset_x = USER_TO_PROGRAM_LEN(settings->parameters[5221]); settings->origin_offset_y = USER_TO_PROGRAM_LEN(settings->parameters[5222]); settings->origin_offset_z = USER_TO_PROGRAM_LEN(settings->parameters[5223]); - settings->AA_origin_offset = USER_TO_PROGRAM_AX(3, settings->parameters[5224]); - settings->BB_origin_offset = USER_TO_PROGRAM_AX(4, settings->parameters[5225]); - settings->CC_origin_offset = USER_TO_PROGRAM_AX(5, settings->parameters[5226]); - settings->u_origin_offset = USER_TO_PROGRAM_AX(6, settings->parameters[5227]); - settings->v_origin_offset = USER_TO_PROGRAM_AX(7, settings->parameters[5228]); - settings->w_origin_offset = USER_TO_PROGRAM_AX(8, settings->parameters[5229]); + settings->AA_origin_offset = USER_TO_PROGRAM_AX(AXIS_A, settings->parameters[5224]); + settings->BB_origin_offset = USER_TO_PROGRAM_AX(AXIS_B, settings->parameters[5225]); + settings->CC_origin_offset = USER_TO_PROGRAM_AX(AXIS_C, settings->parameters[5226]); + settings->u_origin_offset = USER_TO_PROGRAM_AX(AXIS_U, settings->parameters[5227]); + settings->v_origin_offset = USER_TO_PROGRAM_AX(AXIS_V, settings->parameters[5228]); + settings->w_origin_offset = USER_TO_PROGRAM_AX(AXIS_W, settings->parameters[5229]); settings->rotation_xy = settings->parameters[5230]; settings->current_x -= settings->origin_offset_x; @@ -5826,13 +5826,13 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 int Interp::convert_straight_indexer(int axis, int jnum, block_pointer block, setup_pointer settings) { double end[9]; - find_ends(block, settings, &end[0], &end[1], &end[2], - &end[3], &end[4], &end[5], &end[6], &end[7], &end[8]); + find_ends(block, settings, &end[AXIS_X], &end[AXIS_Y], &end[AXIS_Z], + &end[AXIS_A], &end[AXIS_B], &end[AXIS_C], &end[AXIS_U], &end[AXIS_V], &end[AXIS_W]); const double current[9] = {settings->current_x, settings->current_y, settings->current_z, settings->AA_current, settings->BB_current, settings->CC_current, settings->u_current, settings->v_current, settings->w_current}; - CHKS((axis < 3 || axis > 8), (_("BUG: trying to index incorrect axis"))); + CHKS((axis < AXIS_A || axis > AXIS_W), (_("BUG: trying to index incorrect axis"))); for (int n = 0; n < 9; n++) { CHKS((n != axis && end[n] != current[n]), _("BUG: An axis incorrectly moved along with an indexer")); @@ -5858,9 +5858,9 @@ int Interp::issue_straight_index(int axis, int jnum, double target, int lineno, // tell canon that this is a special indexing move UNLOCK_ROTARY(lineno, jnum); - STRAIGHT_TRAVERSE(lineno, end[0], end[1], end[2], - end[3], end[4], end[5], - end[6], end[7], end[8]); + STRAIGHT_TRAVERSE(lineno, end[AXIS_X], end[AXIS_Y], end[AXIS_Z], + end[AXIS_A], end[AXIS_B], end[AXIS_C], + end[AXIS_U], end[AXIS_V], end[AXIS_W]); LOCK_ROTARY(lineno, jnum); // restore path mode @@ -5869,12 +5869,12 @@ int Interp::issue_straight_index(int axis, int jnum, double target, int lineno, SET_NAIVECAM_TOLERANCE(save_cam_tolerance); } - settings->AA_current = end[3]; - settings->BB_current = end[4]; - settings->CC_current = end[5]; - settings->u_current = end[6]; - settings->v_current = end[7]; - settings->w_current = end[8]; + settings->AA_current = end[AXIS_A]; + settings->BB_current = end[AXIS_B]; + settings->CC_current = end[AXIS_C]; + settings->u_current = end[AXIS_U]; + settings->v_current = end[AXIS_V]; + settings->w_current = end[AXIS_W]; return INTERP_OK; } @@ -6491,12 +6491,12 @@ int Interp::convert_tool_change(setup_pointer settings) //!< pointer to machine find_relative(USER_TO_PROGRAM_LEN(settings->parameters[5181]), USER_TO_PROGRAM_LEN(settings->parameters[5182]), USER_TO_PROGRAM_LEN(settings->parameters[5183]), - USER_TO_PROGRAM_AX(3, settings->parameters[5184]), - USER_TO_PROGRAM_AX(4, settings->parameters[5185]), - USER_TO_PROGRAM_AX(5, settings->parameters[5186]), - USER_TO_PROGRAM_AX(6, settings->parameters[5187]), - USER_TO_PROGRAM_AX(7, settings->parameters[5188]), - USER_TO_PROGRAM_AX(8, settings->parameters[5189]), + USER_TO_PROGRAM_AX(AXIS_A, settings->parameters[5184]), + USER_TO_PROGRAM_AX(AXIS_B, settings->parameters[5185]), + USER_TO_PROGRAM_AX(AXIS_C, settings->parameters[5186]), + USER_TO_PROGRAM_AX(AXIS_U, settings->parameters[5187]), + USER_TO_PROGRAM_AX(AXIS_V, settings->parameters[5188]), + USER_TO_PROGRAM_AX(AXIS_W, settings->parameters[5189]), &end_x, &end_y, &end_z, &AA_end, &BB_end, &CC_end, &u_end, &v_end, &w_end, settings); @@ -6504,18 +6504,18 @@ int Interp::convert_tool_change(setup_pointer settings) //!< pointer to machine // move indexers first, one at a time // JOINTS_AXES settings->*_indexer_jnum == -1 means notused - if (AA_end != settings->AA_current && (-1 != settings->axis_indexer_jnum[3]) ) - issue_straight_index(3,settings->axis_indexer_jnum[3], AA_end, -1, settings); - if (BB_end != settings->BB_current && (-1 != settings->axis_indexer_jnum[4]) ) - issue_straight_index(4,settings->axis_indexer_jnum[4], BB_end, -1, settings); - if (CC_end != settings->CC_current && (-1 != settings->axis_indexer_jnum[5]) ) - issue_straight_index(5,settings->axis_indexer_jnum[5], CC_end, -1, settings); - if (u_end != settings->u_current && (-1 != settings->axis_indexer_jnum[6]) ) - issue_straight_index(6,settings->axis_indexer_jnum[6], u_end, -1, settings); - if (v_end != settings->v_current && (-1 != settings->axis_indexer_jnum[7]) ) - issue_straight_index(7,settings->axis_indexer_jnum[7], v_end, -1, settings); - if (w_end != settings->w_current && (-1 != settings->axis_indexer_jnum[8]) ) - issue_straight_index(8,settings->axis_indexer_jnum[8], w_end, -1, settings); + if (AA_end != settings->AA_current && (-1 != settings->axis_indexer_jnum[AXIS_A]) ) + issue_straight_index(AXIS_A,settings->axis_indexer_jnum[AXIS_A], AA_end, -1, settings); + if (BB_end != settings->BB_current && (-1 != settings->axis_indexer_jnum[AXIS_B]) ) + issue_straight_index(AXIS_B,settings->axis_indexer_jnum[AXIS_B], BB_end, -1, settings); + if (CC_end != settings->CC_current && (-1 != settings->axis_indexer_jnum[AXIS_C]) ) + issue_straight_index(AXIS_C,settings->axis_indexer_jnum[AXIS_C], CC_end, -1, settings); + if (u_end != settings->u_current && (-1 != settings->axis_indexer_jnum[AXIS_U]) ) + issue_straight_index(AXIS_U,settings->axis_indexer_jnum[AXIS_U], u_end, -1, settings); + if (v_end != settings->v_current && (-1 != settings->axis_indexer_jnum[AXIS_V]) ) + issue_straight_index(AXIS_V,settings->axis_indexer_jnum[AXIS_V], v_end, -1, settings); + if (w_end != settings->w_current && (-1 != settings->axis_indexer_jnum[AXIS_W]) ) + issue_straight_index(AXIS_W,settings->axis_indexer_jnum[AXIS_W], w_end, -1, settings); STRAIGHT_TRAVERSE(-1, end_x, end_y, end_z, AA_end, BB_end, CC_end, @@ -6663,12 +6663,12 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu tool_offset.tran.x = USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.tran.x); tool_offset.tran.y = USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.tran.y); tool_offset.tran.z = USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.tran.z); - tool_offset.a = USER_TO_PROGRAM_AX(3, settings->tool_table[idx].offset.a); - tool_offset.b = USER_TO_PROGRAM_AX(4, settings->tool_table[idx].offset.b); - tool_offset.c = USER_TO_PROGRAM_AX(5, settings->tool_table[idx].offset.c); - tool_offset.u = USER_TO_PROGRAM_AX(6, settings->tool_table[idx].offset.u); - tool_offset.v = USER_TO_PROGRAM_AX(7, settings->tool_table[idx].offset.v); - tool_offset.w = USER_TO_PROGRAM_AX(8, settings->tool_table[idx].offset.w); + tool_offset.a = USER_TO_PROGRAM_AX(AXIS_A, settings->tool_table[idx].offset.a); + tool_offset.b = USER_TO_PROGRAM_AX(AXIS_B, settings->tool_table[idx].offset.b); + tool_offset.c = USER_TO_PROGRAM_AX(AXIS_C, settings->tool_table[idx].offset.c); + tool_offset.u = USER_TO_PROGRAM_AX(AXIS_U, settings->tool_table[idx].offset.u); + tool_offset.v = USER_TO_PROGRAM_AX(AXIS_V, settings->tool_table[idx].offset.v); + tool_offset.w = USER_TO_PROGRAM_AX(AXIS_W, settings->tool_table[idx].offset.w); settings->g43_with_zero_offset = !(tool_offset.tran.x || tool_offset.tran.y || tool_offset.tran.z || tool_offset.a || tool_offset.b || tool_offset.c || @@ -6698,12 +6698,12 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu tool_offset.tran.x += USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.tran.x); tool_offset.tran.y += USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.tran.y); tool_offset.tran.z += USER_TO_PROGRAM_LEN(settings->tool_table[idx].offset.tran.z); - tool_offset.a += USER_TO_PROGRAM_AX(3, settings->tool_table[idx].offset.a); - tool_offset.b += USER_TO_PROGRAM_AX(4, settings->tool_table[idx].offset.b); - tool_offset.c += USER_TO_PROGRAM_AX(5, settings->tool_table[idx].offset.c); - tool_offset.u += USER_TO_PROGRAM_AX(6, settings->tool_table[idx].offset.u); - tool_offset.v += USER_TO_PROGRAM_AX(7, settings->tool_table[idx].offset.v); - tool_offset.w += USER_TO_PROGRAM_AX(8, settings->tool_table[idx].offset.w); + tool_offset.a += USER_TO_PROGRAM_AX(AXIS_A, settings->tool_table[idx].offset.a); + tool_offset.b += USER_TO_PROGRAM_AX(AXIS_B, settings->tool_table[idx].offset.b); + tool_offset.c += USER_TO_PROGRAM_AX(AXIS_C, settings->tool_table[idx].offset.c); + tool_offset.u += USER_TO_PROGRAM_AX(AXIS_U, settings->tool_table[idx].offset.u); + tool_offset.v += USER_TO_PROGRAM_AX(AXIS_V, settings->tool_table[idx].offset.v); + tool_offset.w += USER_TO_PROGRAM_AX(AXIS_W, settings->tool_table[idx].offset.w); } else { if(block->x_flag) tool_offset.tran.x += block->x_number; if(block->y_flag) tool_offset.tran.y += block->y_number; @@ -6750,12 +6750,12 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu settings->parameters[5081] = PROGRAM_TO_USER_LEN(tool_offset.tran.x); settings->parameters[5082] = PROGRAM_TO_USER_LEN(tool_offset.tran.y); settings->parameters[5083] = PROGRAM_TO_USER_LEN(tool_offset.tran.z); - settings->parameters[5084] = PROGRAM_TO_USER_AX(3, tool_offset.a); - settings->parameters[5085] = PROGRAM_TO_USER_AX(4, tool_offset.b); - settings->parameters[5086] = PROGRAM_TO_USER_AX(5, tool_offset.c); - settings->parameters[5087] = PROGRAM_TO_USER_AX(6, tool_offset.u); - settings->parameters[5088] = PROGRAM_TO_USER_AX(7, tool_offset.v); - settings->parameters[5089] = PROGRAM_TO_USER_AX(8, tool_offset.w); + settings->parameters[5084] = PROGRAM_TO_USER_AX(AXIS_A, tool_offset.a); + settings->parameters[5085] = PROGRAM_TO_USER_AX(AXIS_B, tool_offset.b); + settings->parameters[5086] = PROGRAM_TO_USER_AX(AXIS_C, tool_offset.c); + settings->parameters[5087] = PROGRAM_TO_USER_AX(AXIS_U, tool_offset.u); + settings->parameters[5088] = PROGRAM_TO_USER_AX(AXIS_V, tool_offset.v); + settings->parameters[5089] = PROGRAM_TO_USER_AX(AXIS_W, tool_offset.w); if (g_code == G_49 && settings->kins_by_g43_4) { // G49 undoes what G43.4 did: after the cancel it drops the machine diff --git a/src/emc/rs274ngc/interp_find.cc b/src/emc/rs274ngc/interp_find.cc index bb420f1a4db..ed341d48049 100644 --- a/src/emc/rs274ngc/interp_find.cc +++ b/src/emc/rs274ngc/interp_find.cc @@ -217,11 +217,11 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->a_flag) { - if(s->axis_wrapped[3]) { + if(s->axis_wrapped[AXIS_A]) { CHP(unwrap_rotary(AA_p, block->a_number, block->a_number - s->AA_origin_offset - s->AA_axis_offset - s->tool_offset.a, s->AA_current, 'A')); - } else if (s->axis_rotary_modulo[3]) { + } else if (s->axis_rotary_modulo[AXIS_A]) { *AA_p = rotary_modulo_target(block->a_number, s->AA_origin_offset + s->AA_axis_offset + s->tool_offset.a, s->AA_current, s->rotary_modulo_literal); @@ -233,11 +233,11 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->b_flag) { - if(s->axis_wrapped[4]) { + if(s->axis_wrapped[AXIS_B]) { CHP(unwrap_rotary(BB_p, block->b_number, block->b_number - s->BB_origin_offset - s->BB_axis_offset - s->tool_offset.b, s->BB_current, 'B')); - } else if (s->axis_rotary_modulo[4]) { + } else if (s->axis_rotary_modulo[AXIS_B]) { *BB_p = rotary_modulo_target(block->b_number, s->BB_origin_offset + s->BB_axis_offset + s->tool_offset.b, s->BB_current, s->rotary_modulo_literal); @@ -249,11 +249,11 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->c_flag) { - if(s->axis_wrapped[5]) { + if(s->axis_wrapped[AXIS_C]) { CHP(unwrap_rotary(CC_p, block->c_number, block->c_number - s->CC_origin_offset - s->CC_axis_offset - s->tool_offset.c, s->CC_current, 'C')); - } else if (s->axis_rotary_modulo[5]) { + } else if (s->axis_rotary_modulo[AXIS_C]) { *CC_p = rotary_modulo_target(block->c_number, s->CC_origin_offset + s->CC_axis_offset + s->tool_offset.c, s->CC_current, s->rotary_modulo_literal); @@ -265,11 +265,11 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->u_flag) { - if(s->axis_wrapped[6]) { + if(s->axis_wrapped[AXIS_U]) { CHP(unwrap_rotary(u_p, block->u_number, block->u_number - s->u_origin_offset - s->u_axis_offset - s->tool_offset.u, s->u_current, 'U')); - } else if (s->axis_rotary_modulo[6]) { + } else if (s->axis_rotary_modulo[AXIS_U]) { *u_p = rotary_modulo_target(block->u_number, s->u_origin_offset + s->u_axis_offset + s->tool_offset.u, s->u_current, s->rotary_modulo_literal); @@ -281,11 +281,11 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->v_flag) { - if(s->axis_wrapped[7]) { + if(s->axis_wrapped[AXIS_V]) { CHP(unwrap_rotary(v_p, block->v_number, block->v_number - s->v_origin_offset - s->v_axis_offset - s->tool_offset.v, s->v_current, 'V')); - } else if (s->axis_rotary_modulo[7]) { + } else if (s->axis_rotary_modulo[AXIS_V]) { *v_p = rotary_modulo_target(block->v_number, s->v_origin_offset + s->v_axis_offset + s->tool_offset.v, s->v_current, s->rotary_modulo_literal); @@ -297,11 +297,11 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->w_flag) { - if(s->axis_wrapped[8]) { + if(s->axis_wrapped[AXIS_W]) { CHP(unwrap_rotary(w_p, block->w_number, block->w_number - s->w_origin_offset - s->w_axis_offset - s->tool_offset.w, s->w_current, 'W')); - } else if (s->axis_rotary_modulo[8]) { + } else if (s->axis_rotary_modulo[AXIS_W]) { *w_p = rotary_modulo_target(block->w_number, s->w_origin_offset + s->w_axis_offset + s->tool_offset.w, s->w_current, s->rotary_modulo_literal); @@ -354,9 +354,9 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->a_flag) { - if(s->axis_wrapped[3]) { + if(s->axis_wrapped[AXIS_A]) { CHP(unwrap_rotary(AA_p, block->a_number, block->a_number, s->AA_current, 'A')); - } else if (s->axis_rotary_modulo[3]) { + } else if (s->axis_rotary_modulo[AXIS_A]) { *AA_p = rotary_modulo_target(block->a_number, 0.0, s->AA_current, s->rotary_modulo_literal); } else { @@ -367,9 +367,9 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->b_flag) { - if(s->axis_wrapped[4]) { + if(s->axis_wrapped[AXIS_B]) { CHP(unwrap_rotary(BB_p, block->b_number, block->b_number, s->BB_current, 'B')); - } else if (s->axis_rotary_modulo[4]) { + } else if (s->axis_rotary_modulo[AXIS_B]) { *BB_p = rotary_modulo_target(block->b_number, 0.0, s->BB_current, s->rotary_modulo_literal); } else { @@ -380,9 +380,9 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->c_flag) { - if(s->axis_wrapped[5]) { + if(s->axis_wrapped[AXIS_C]) { CHP(unwrap_rotary(CC_p, block->c_number, block->c_number, s->CC_current, 'C')); - } else if (s->axis_rotary_modulo[5]) { + } else if (s->axis_rotary_modulo[AXIS_C]) { *CC_p = rotary_modulo_target(block->c_number, 0.0, s->CC_current, s->rotary_modulo_literal); } else { @@ -393,9 +393,9 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 } if(block->u_flag) { - if(s->axis_wrapped[6]) { + if(s->axis_wrapped[AXIS_U]) { CHP(unwrap_rotary(u_p, block->u_number, block->u_number, s->u_current, 'U')); - } else if (s->axis_rotary_modulo[6]) { + } else if (s->axis_rotary_modulo[AXIS_U]) { *u_p = rotary_modulo_target(block->u_number, 0.0, s->u_current, s->rotary_modulo_literal); } else { @@ -405,9 +405,9 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 *u_p = s->u_current; } if(block->v_flag) { - if(s->axis_wrapped[7]) { + if(s->axis_wrapped[AXIS_V]) { CHP(unwrap_rotary(v_p, block->v_number, block->v_number, s->v_current, 'V')); - } else if (s->axis_rotary_modulo[7]) { + } else if (s->axis_rotary_modulo[AXIS_V]) { *v_p = rotary_modulo_target(block->v_number, 0.0, s->v_current, s->rotary_modulo_literal); } else { @@ -417,9 +417,9 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 *v_p = s->v_current; } if(block->w_flag) { - if(s->axis_wrapped[8]) { + if(s->axis_wrapped[AXIS_W]) { CHP(unwrap_rotary(w_p, block->w_number, block->w_number, s->w_current, 'W')); - } else if (s->axis_rotary_modulo[8]) { + } else if (s->axis_rotary_modulo[AXIS_W]) { *w_p = rotary_modulo_target(block->w_number, 0.0, s->w_current, s->rotary_modulo_literal); } else { @@ -533,11 +533,11 @@ int Interp::find_relative(double x1, //!< absolute x position *y2 -= settings->axis_offset_y; *z2 = z1 - settings->origin_offset_z - settings->axis_offset_z - settings->tool_offset.tran.z; - if(settings->axis_wrapped[3]) { + if(settings->axis_wrapped[AXIS_A]) { CHP(unwrap_rotary(AA_2, AA_1, AA_1 - settings->AA_origin_offset - settings->AA_axis_offset - settings->tool_offset.a, settings->AA_current, 'A')); - } else if (settings->axis_rotary_modulo[3]) { + } else if (settings->axis_rotary_modulo[AXIS_A]) { // stored positions carry no programmed sign for M27 to read a direction // from, so G28/G30/tool change always take the shortest path *AA_2 = rotary_modulo_target(AA_1, @@ -547,11 +547,11 @@ int Interp::find_relative(double x1, //!< absolute x position *AA_2 = AA_1 - settings->AA_origin_offset - settings->AA_axis_offset - settings->tool_offset.a; } - if(settings->axis_wrapped[4]) { + if(settings->axis_wrapped[AXIS_B]) { CHP(unwrap_rotary(BB_2, BB_1, BB_1 - settings->BB_origin_offset - settings->BB_axis_offset - settings->tool_offset.b, settings->BB_current, 'B')); - } else if (settings->axis_rotary_modulo[4]) { + } else if (settings->axis_rotary_modulo[AXIS_B]) { *BB_2 = rotary_modulo_target(BB_1, settings->BB_origin_offset + settings->BB_axis_offset + settings->tool_offset.b, settings->BB_current, 0); @@ -559,11 +559,11 @@ int Interp::find_relative(double x1, //!< absolute x position *BB_2 = BB_1 - settings->BB_origin_offset - settings->BB_axis_offset - settings->tool_offset.b; } - if(settings->axis_wrapped[5]) { + if(settings->axis_wrapped[AXIS_C]) { CHP(unwrap_rotary(CC_2, CC_1, CC_1 - settings->CC_origin_offset - settings->CC_axis_offset - settings->tool_offset.c, settings->CC_current, 'C')); - } else if (settings->axis_rotary_modulo[5]) { + } else if (settings->axis_rotary_modulo[AXIS_C]) { *CC_2 = rotary_modulo_target(CC_1, settings->CC_origin_offset + settings->CC_axis_offset + settings->tool_offset.c, settings->CC_current, 0); @@ -571,11 +571,11 @@ int Interp::find_relative(double x1, //!< absolute x position *CC_2 = CC_1 - settings->CC_origin_offset - settings->CC_axis_offset - settings->tool_offset.c; } - if(settings->axis_wrapped[6]) { + if(settings->axis_wrapped[AXIS_U]) { CHP(unwrap_rotary(u_2, u_1, u_1 - settings->u_origin_offset - settings->u_axis_offset - settings->tool_offset.u, settings->u_current, 'U')); - } else if (settings->axis_rotary_modulo[6]) { + } else if (settings->axis_rotary_modulo[AXIS_U]) { *u_2 = rotary_modulo_target(u_1, settings->u_origin_offset + settings->u_axis_offset + settings->tool_offset.u, settings->u_current, 0); @@ -583,11 +583,11 @@ int Interp::find_relative(double x1, //!< absolute x position *u_2 = u_1 - settings->u_origin_offset - settings->u_axis_offset - settings->tool_offset.u; } - if(settings->axis_wrapped[7]) { + if(settings->axis_wrapped[AXIS_V]) { CHP(unwrap_rotary(v_2, v_1, v_1 - settings->v_origin_offset - settings->v_axis_offset - settings->tool_offset.v, settings->v_current, 'V')); - } else if (settings->axis_rotary_modulo[7]) { + } else if (settings->axis_rotary_modulo[AXIS_V]) { *v_2 = rotary_modulo_target(v_1, settings->v_origin_offset + settings->v_axis_offset + settings->tool_offset.v, settings->v_current, 0); @@ -595,11 +595,11 @@ int Interp::find_relative(double x1, //!< absolute x position *v_2 = v_1 - settings->v_origin_offset - settings->v_axis_offset - settings->tool_offset.v; } - if(settings->axis_wrapped[8]) { + if(settings->axis_wrapped[AXIS_W]) { CHP(unwrap_rotary(w_2, w_1, w_1 - settings->w_origin_offset - settings->w_axis_offset - settings->tool_offset.w, settings->w_current, 'W')); - } else if (settings->axis_rotary_modulo[8]) { + } else if (settings->axis_rotary_modulo[AXIS_W]) { *w_2 = rotary_modulo_target(w_1, settings->w_origin_offset + settings->w_axis_offset + settings->tool_offset.w, settings->w_current, 0); @@ -652,12 +652,12 @@ int Interp::find_current_in_system(setup_pointer s, int system, *x -= USER_TO_PROGRAM_LEN(p[5201 + system * 20]); *y -= USER_TO_PROGRAM_LEN(p[5202 + system * 20]); *z -= USER_TO_PROGRAM_LEN(p[5203 + system * 20]); - *a -= USER_TO_PROGRAM_AX(3, p[5204 + system * 20]); - *b -= USER_TO_PROGRAM_AX(4, p[5205 + system * 20]); - *c -= USER_TO_PROGRAM_AX(5, p[5206 + system * 20]); - *u -= USER_TO_PROGRAM_AX(6, p[5207 + system * 20]); - *v -= USER_TO_PROGRAM_AX(7, p[5208 + system * 20]); - *w -= USER_TO_PROGRAM_AX(8, p[5209 + system * 20]); + *a -= USER_TO_PROGRAM_AX(AXIS_A, p[5204 + system * 20]); + *b -= USER_TO_PROGRAM_AX(AXIS_B, p[5205 + system * 20]); + *c -= USER_TO_PROGRAM_AX(AXIS_C, p[5206 + system * 20]); + *u -= USER_TO_PROGRAM_AX(AXIS_U, p[5207 + system * 20]); + *v -= USER_TO_PROGRAM_AX(AXIS_V, p[5208 + system * 20]); + *w -= USER_TO_PROGRAM_AX(AXIS_W, p[5209 + system * 20]); rotate(x, y, -p[5210 + system * 20]); @@ -665,12 +665,12 @@ int Interp::find_current_in_system(setup_pointer s, int system, *x -= USER_TO_PROGRAM_LEN(p[5211]); *y -= USER_TO_PROGRAM_LEN(p[5212]); *z -= USER_TO_PROGRAM_LEN(p[5213]); - *a -= USER_TO_PROGRAM_AX(3, p[5214]); - *b -= USER_TO_PROGRAM_AX(4, p[5215]); - *c -= USER_TO_PROGRAM_AX(5, p[5216]); - *u -= USER_TO_PROGRAM_AX(6, p[5217]); - *v -= USER_TO_PROGRAM_AX(7, p[5218]); - *w -= USER_TO_PROGRAM_AX(8, p[5219]); + *a -= USER_TO_PROGRAM_AX(AXIS_A, p[5214]); + *b -= USER_TO_PROGRAM_AX(AXIS_B, p[5215]); + *c -= USER_TO_PROGRAM_AX(AXIS_C, p[5216]); + *u -= USER_TO_PROGRAM_AX(AXIS_U, p[5217]); + *v -= USER_TO_PROGRAM_AX(AXIS_V, p[5218]); + *w -= USER_TO_PROGRAM_AX(AXIS_W, p[5219]); } return INTERP_OK; @@ -731,12 +731,12 @@ int Interp::find_current_in_system_without_tlo(setup_pointer s, int system, *x -= USER_TO_PROGRAM_LEN(p[5201 + system * 20]); *y -= USER_TO_PROGRAM_LEN(p[5202 + system * 20]); *z -= USER_TO_PROGRAM_LEN(p[5203 + system * 20]); - *a -= USER_TO_PROGRAM_AX(3, p[5204 + system * 20]); - *b -= USER_TO_PROGRAM_AX(4, p[5205 + system * 20]); - *c -= USER_TO_PROGRAM_AX(5, p[5206 + system * 20]); - *u -= USER_TO_PROGRAM_AX(6, p[5207 + system * 20]); - *v -= USER_TO_PROGRAM_AX(7, p[5208 + system * 20]); - *w -= USER_TO_PROGRAM_AX(8, p[5209 + system * 20]); + *a -= USER_TO_PROGRAM_AX(AXIS_A, p[5204 + system * 20]); + *b -= USER_TO_PROGRAM_AX(AXIS_B, p[5205 + system * 20]); + *c -= USER_TO_PROGRAM_AX(AXIS_C, p[5206 + system * 20]); + *u -= USER_TO_PROGRAM_AX(AXIS_U, p[5207 + system * 20]); + *v -= USER_TO_PROGRAM_AX(AXIS_V, p[5208 + system * 20]); + *w -= USER_TO_PROGRAM_AX(AXIS_W, p[5209 + system * 20]); rotate(x, y, -p[5210 + system * 20]); @@ -744,12 +744,12 @@ int Interp::find_current_in_system_without_tlo(setup_pointer s, int system, *x -= USER_TO_PROGRAM_LEN(p[5211]); *y -= USER_TO_PROGRAM_LEN(p[5212]); *z -= USER_TO_PROGRAM_LEN(p[5213]); - *a -= USER_TO_PROGRAM_AX(3, p[5214]); - *b -= USER_TO_PROGRAM_AX(4, p[5215]); - *c -= USER_TO_PROGRAM_AX(5, p[5216]); - *u -= USER_TO_PROGRAM_AX(6, p[5217]); - *v -= USER_TO_PROGRAM_AX(7, p[5218]); - *w -= USER_TO_PROGRAM_AX(8, p[5219]); + *a -= USER_TO_PROGRAM_AX(AXIS_A, p[5214]); + *b -= USER_TO_PROGRAM_AX(AXIS_B, p[5215]); + *c -= USER_TO_PROGRAM_AX(AXIS_C, p[5216]); + *u -= USER_TO_PROGRAM_AX(AXIS_U, p[5217]); + *v -= USER_TO_PROGRAM_AX(AXIS_V, p[5218]); + *w -= USER_TO_PROGRAM_AX(AXIS_W, p[5219]); } return INTERP_OK; diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index 3b7749a62c4..4ec4f7aafb5 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -448,37 +448,37 @@ int Interp::set_probe_data(setup_pointer settings) //!< pointer to machine settings->parameters[5063] = GET_EXTERNAL_PROBE_POSITION_Z(); a = GET_EXTERNAL_PROBE_POSITION_A(); - if(settings->axis_wrapped[3] || settings->axis_rotary_modulo[3]) { + if(settings->axis_wrapped[AXIS_A] || settings->axis_rotary_modulo[AXIS_A]) { a = wrap_rotary_to_360(a); } settings->parameters[5064] = a; b = GET_EXTERNAL_PROBE_POSITION_B(); - if(settings->axis_wrapped[4] || settings->axis_rotary_modulo[4]) { + if(settings->axis_wrapped[AXIS_B] || settings->axis_rotary_modulo[AXIS_B]) { b = wrap_rotary_to_360(b); } settings->parameters[5065] = b; c = GET_EXTERNAL_PROBE_POSITION_C(); - if(settings->axis_wrapped[5] || settings->axis_rotary_modulo[5]) { + if(settings->axis_wrapped[AXIS_C] || settings->axis_rotary_modulo[AXIS_C]) { c = wrap_rotary_to_360(c); } settings->parameters[5066] = c; u = GET_EXTERNAL_PROBE_POSITION_U(); - if(settings->axis_wrapped[6] || settings->axis_rotary_modulo[6]) { + if(settings->axis_wrapped[AXIS_U] || settings->axis_rotary_modulo[AXIS_U]) { u = wrap_rotary_to_360(u); } settings->parameters[5067] = u; v = GET_EXTERNAL_PROBE_POSITION_V(); - if(settings->axis_wrapped[7] || settings->axis_rotary_modulo[7]) { + if(settings->axis_wrapped[AXIS_V] || settings->axis_rotary_modulo[AXIS_V]) { v = wrap_rotary_to_360(v); } settings->parameters[5068] = v; w = GET_EXTERNAL_PROBE_POSITION_W(); - if(settings->axis_wrapped[8] || settings->axis_rotary_modulo[8]) { + if(settings->axis_wrapped[AXIS_W] || settings->axis_rotary_modulo[AXIS_W]) { w = wrap_rotary_to_360(w); } settings->parameters[5069] = w; diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 55bfa138f4d..8066f0eac72 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -860,7 +860,7 @@ struct setup double parameter_g73_peck_clearance; double parameter_g83_peck_clearance; AxisKinds axis_kinds; // [AXIS_] TYPE, [TRAJ] FEED_AXES - int axis_wrapped[9]; // per axis, X 0 to W 8; angular axes only + int axis_wrapped[9]; // by AxisIndex; angular axes only int axis_rotary_modulo[9]; // angular axes only int rotary_modulo_literal; // M26 = shortest path (default), M27 = literal absolute int axis_indexer_jnum[9]; // -1 where the axis has no locking indexer diff --git a/src/emc/rs274ngc/interp_namedparams.cc b/src/emc/rs274ngc/interp_namedparams.cc index 131f4e2e3c8..6287d06dc93 100644 --- a/src/emc/rs274ngc/interp_namedparams.cc +++ b/src/emc/rs274ngc/interp_namedparams.cc @@ -732,32 +732,32 @@ int Interp::lookup_named_param(const char *nameBuf, break; case NP_A: // current position - *value = _setup.axis_rotary_modulo[3] + *value = _setup.axis_rotary_modulo[AXIS_A] ? wrap_rotary_to_360(_setup.AA_current) : _setup.AA_current; break; case NP_B: // current position - *value = _setup.axis_rotary_modulo[4] + *value = _setup.axis_rotary_modulo[AXIS_B] ? wrap_rotary_to_360(_setup.BB_current) : _setup.BB_current; break; case NP_C: // current position - *value = _setup.axis_rotary_modulo[5] + *value = _setup.axis_rotary_modulo[AXIS_C] ? wrap_rotary_to_360(_setup.CC_current) : _setup.CC_current; break; case NP_U: // current position - *value = _setup.axis_rotary_modulo[6] + *value = _setup.axis_rotary_modulo[AXIS_U] ? wrap_rotary_to_360(_setup.u_current) : _setup.u_current; break; case NP_V: // current position - *value = _setup.axis_rotary_modulo[7] + *value = _setup.axis_rotary_modulo[AXIS_V] ? wrap_rotary_to_360(_setup.v_current) : _setup.v_current; break; case NP_W: // current position - *value = _setup.axis_rotary_modulo[8] + *value = _setup.axis_rotary_modulo[AXIS_W] ? wrap_rotary_to_360(_setup.w_current) : _setup.w_current; break; @@ -789,7 +789,7 @@ int Interp::lookup_named_param(const char *nameBuf, { double v = _setup.AA_current + _setup.AA_axis_offset + _setup.AA_origin_offset + _setup.tool_offset.a; - *value = _setup.axis_rotary_modulo[3] ? wrap_rotary_to_360(v) : v; + *value = _setup.axis_rotary_modulo[AXIS_A] ? wrap_rotary_to_360(v) : v; } break; @@ -797,7 +797,7 @@ int Interp::lookup_named_param(const char *nameBuf, { double v = _setup.BB_current + _setup.BB_axis_offset + _setup.BB_origin_offset + _setup.tool_offset.b; - *value = _setup.axis_rotary_modulo[4] ? wrap_rotary_to_360(v) : v; + *value = _setup.axis_rotary_modulo[AXIS_B] ? wrap_rotary_to_360(v) : v; } break; @@ -805,7 +805,7 @@ int Interp::lookup_named_param(const char *nameBuf, { double v = _setup.CC_current + _setup.CC_axis_offset + _setup.CC_origin_offset + _setup.tool_offset.c; - *value = _setup.axis_rotary_modulo[5] ? wrap_rotary_to_360(v) : v; + *value = _setup.axis_rotary_modulo[AXIS_C] ? wrap_rotary_to_360(v) : v; } break; @@ -813,7 +813,7 @@ int Interp::lookup_named_param(const char *nameBuf, { double v = _setup.u_current + _setup.u_axis_offset + _setup.u_origin_offset + _setup.tool_offset.u; - *value = _setup.axis_rotary_modulo[6] ? wrap_rotary_to_360(v) : v; + *value = _setup.axis_rotary_modulo[AXIS_U] ? wrap_rotary_to_360(v) : v; } break; @@ -821,7 +821,7 @@ int Interp::lookup_named_param(const char *nameBuf, { double v = _setup.v_current + _setup.v_axis_offset + _setup.v_origin_offset + _setup.tool_offset.v; - *value = _setup.axis_rotary_modulo[7] ? wrap_rotary_to_360(v) : v; + *value = _setup.axis_rotary_modulo[AXIS_V] ? wrap_rotary_to_360(v) : v; } break; @@ -829,7 +829,7 @@ int Interp::lookup_named_param(const char *nameBuf, { double v = _setup.w_current + _setup.w_axis_offset + _setup.w_origin_offset + _setup.tool_offset.w; - *value = _setup.axis_rotary_modulo[8] ? wrap_rotary_to_360(v) : v; + *value = _setup.axis_rotary_modulo[AXIS_W] ? wrap_rotary_to_360(v) : v; } break; diff --git a/src/emc/rs274ngc/interp_queue.cc b/src/emc/rs274ngc/interp_queue.cc index ee4ccde31d6..87c23fa2757 100644 --- a/src/emc/rs274ngc/interp_queue.cc +++ b/src/emc/rs274ngc/interp_queue.cc @@ -390,7 +390,7 @@ static void scale_linear(const AxisKinds &kinds, double &a, double &b, double &c double &u, double &v, double &w, double scale) { double *axis[6] = {&a, &b, &c, &u, &v, &w}; for (int n = 0; n < 6; n++) { - if (!axisKindsAngular(kinds, n + 3)) { *axis[n] *= scale; } + if (!axisKindsAngular(kinds, AXIS_A + n)) { *axis[n] *= scale; } } } diff --git a/src/emc/rs274ngc/interpmodule.cc b/src/emc/rs274ngc/interpmodule.cc index 5c4172b9492..1597ec1be34 100644 --- a/src/emc/rs274ngc/interpmodule.cc +++ b/src/emc/rs274ngc/interpmodule.cc @@ -589,40 +589,40 @@ static inline void set_w_origin_offset(Interp &interp, double value) { interp._setup.w_origin_offset = value; } static inline int get_a_axis_wrapped (Interp &interp) { - return interp._setup.axis_wrapped[3]; + return interp._setup.axis_wrapped[AXIS_A]; } static inline void set_a_axis_wrapped(Interp &interp, int value) { - interp._setup.axis_wrapped[3] = value; + interp._setup.axis_wrapped[AXIS_A] = value; } static inline int get_a_indexer (Interp &interp) { - return interp._setup.axis_indexer_jnum[3]; + return interp._setup.axis_indexer_jnum[AXIS_A]; } static inline void set_a_indexer(Interp &interp, int value) { - interp._setup.axis_indexer_jnum[3] = value; + interp._setup.axis_indexer_jnum[AXIS_A] = value; } static inline int get_b_axis_wrapped (Interp &interp) { - return interp._setup.axis_wrapped[4]; + return interp._setup.axis_wrapped[AXIS_B]; } static inline void set_b_axis_wrapped(Interp &interp, int value) { - interp._setup.axis_wrapped[4] = value; + interp._setup.axis_wrapped[AXIS_B] = value; } static inline int get_b_indexer (Interp &interp) { - return interp._setup.axis_indexer_jnum[4]; + return interp._setup.axis_indexer_jnum[AXIS_B]; } static inline void set_b_indexer(Interp &interp, int value) { - interp._setup.axis_indexer_jnum[4] = value; + interp._setup.axis_indexer_jnum[AXIS_B] = value; } static inline int get_c_axis_wrapped (Interp &interp) { - return interp._setup.axis_wrapped[5]; + return interp._setup.axis_wrapped[AXIS_C]; } static inline void set_c_axis_wrapped(Interp &interp, int value) { - interp._setup.axis_wrapped[5] = value; + interp._setup.axis_wrapped[AXIS_C] = value; } static inline int get_c_indexer (Interp &interp) { - return interp._setup.axis_indexer_jnum[5]; + return interp._setup.axis_indexer_jnum[AXIS_C]; } static inline void set_c_indexer(Interp &interp, int value) { - interp._setup.axis_indexer_jnum[5] = value; + interp._setup.axis_indexer_jnum[AXIS_C] = value; } static inline int get_call_level (Interp &interp) { return interp._setup.call_level; diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index 8be056791cf..f594b4dc204 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -1121,12 +1121,12 @@ int Interp::init() _setup.origin_offset_x = USER_TO_PROGRAM_LEN(pars[k + 1]); _setup.origin_offset_y = USER_TO_PROGRAM_LEN(pars[k + 2]); _setup.origin_offset_z = USER_TO_PROGRAM_LEN(pars[k + 3]); - _setup.AA_origin_offset = USER_TO_PROGRAM_AX(3, pars[k + 4]); - _setup.BB_origin_offset = USER_TO_PROGRAM_AX(4, pars[k + 5]); - _setup.CC_origin_offset = USER_TO_PROGRAM_AX(5, pars[k + 6]); - _setup.u_origin_offset = USER_TO_PROGRAM_AX(6, pars[k + 7]); - _setup.v_origin_offset = USER_TO_PROGRAM_AX(7, pars[k + 8]); - _setup.w_origin_offset = USER_TO_PROGRAM_AX(8, pars[k + 9]); + _setup.AA_origin_offset = USER_TO_PROGRAM_AX(AXIS_A, pars[k + 4]); + _setup.BB_origin_offset = USER_TO_PROGRAM_AX(AXIS_B, pars[k + 5]); + _setup.CC_origin_offset = USER_TO_PROGRAM_AX(AXIS_C, pars[k + 6]); + _setup.u_origin_offset = USER_TO_PROGRAM_AX(AXIS_U, pars[k + 7]); + _setup.v_origin_offset = USER_TO_PROGRAM_AX(AXIS_V, pars[k + 8]); + _setup.w_origin_offset = USER_TO_PROGRAM_AX(AXIS_W, pars[k + 9]); SET_G5X_OFFSET(_setup.origin_index, _setup.origin_offset_x , @@ -1152,12 +1152,12 @@ int Interp::init() _setup.axis_offset_x = USER_TO_PROGRAM_LEN(pars[5211]); _setup.axis_offset_y = USER_TO_PROGRAM_LEN(pars[5212]); _setup.axis_offset_z = USER_TO_PROGRAM_LEN(pars[5213]); - _setup.AA_axis_offset = USER_TO_PROGRAM_AX(3, pars[5214]); - _setup.BB_axis_offset = USER_TO_PROGRAM_AX(4, pars[5215]); - _setup.CC_axis_offset = USER_TO_PROGRAM_AX(5, pars[5216]); - _setup.u_axis_offset = USER_TO_PROGRAM_AX(6, pars[5217]); - _setup.v_axis_offset = USER_TO_PROGRAM_AX(7, pars[5218]); - _setup.w_axis_offset = USER_TO_PROGRAM_AX(8, pars[5219]); + _setup.AA_axis_offset = USER_TO_PROGRAM_AX(AXIS_A, pars[5214]); + _setup.BB_axis_offset = USER_TO_PROGRAM_AX(AXIS_B, pars[5215]); + _setup.CC_axis_offset = USER_TO_PROGRAM_AX(AXIS_C, pars[5216]); + _setup.u_axis_offset = USER_TO_PROGRAM_AX(AXIS_U, pars[5217]); + _setup.v_axis_offset = USER_TO_PROGRAM_AX(AXIS_V, pars[5218]); + _setup.w_axis_offset = USER_TO_PROGRAM_AX(AXIS_W, pars[5219]); } else { _setup.axis_offset_x = 0.0; _setup.axis_offset_y = 0.0; @@ -1667,17 +1667,17 @@ int Interp::_read(const char *command) //!< may be NULL or a string to read _setup.parameters[5422] = _setup.current_z; // ROTARY_MODULO axes: present #5423-#5428 wrapped to [0,360); internal // positions stay accumulated to keep sync with motion.traj.position. - _setup.parameters[5423] = _setup.axis_rotary_modulo[3] + _setup.parameters[5423] = _setup.axis_rotary_modulo[AXIS_A] ? wrap_rotary_to_360(_setup.AA_current) : _setup.AA_current; - _setup.parameters[5424] = _setup.axis_rotary_modulo[4] + _setup.parameters[5424] = _setup.axis_rotary_modulo[AXIS_B] ? wrap_rotary_to_360(_setup.BB_current) : _setup.BB_current; - _setup.parameters[5425] = _setup.axis_rotary_modulo[5] + _setup.parameters[5425] = _setup.axis_rotary_modulo[AXIS_C] ? wrap_rotary_to_360(_setup.CC_current) : _setup.CC_current; - _setup.parameters[5426] = _setup.axis_rotary_modulo[6] + _setup.parameters[5426] = _setup.axis_rotary_modulo[AXIS_U] ? wrap_rotary_to_360(_setup.u_current) : _setup.u_current; - _setup.parameters[5427] = _setup.axis_rotary_modulo[7] + _setup.parameters[5427] = _setup.axis_rotary_modulo[AXIS_V] ? wrap_rotary_to_360(_setup.v_current) : _setup.v_current; - _setup.parameters[5428] = _setup.axis_rotary_modulo[8] + _setup.parameters[5428] = _setup.axis_rotary_modulo[AXIS_W] ? wrap_rotary_to_360(_setup.w_current) : _setup.w_current; double abs_pos[9]; diff --git a/src/emc/rs274ngc/units.h b/src/emc/rs274ngc/units.h index 2b1a3de8209..e004ac90764 100644 --- a/src/emc/rs274ngc/units.h +++ b/src/emc/rs274ngc/units.h @@ -35,7 +35,7 @@ #define PROGRAM_TO_USER_ANG(p) (TO_EXT_ANG(FROM_PROG_ANG(p))) -/* the same for axis n, 0 X to 8 W, a length or an angle as its +/* the same for axis n (AxisIndex), a length or an angle as its [AXIS_] TYPE says */ #define USER_TO_PROGRAM_AX(n, u) (axisKindsAngular(_setup.axis_kinds, (n)) ? USER_TO_PROGRAM_ANG(u) : USER_TO_PROGRAM_LEN(u)) #define PROGRAM_TO_USER_AX(n, p) (axisKindsAngular(_setup.axis_kinds, (n)) ? PROGRAM_TO_USER_ANG(p) : PROGRAM_TO_USER_LEN(p)) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index d3ef981de02..706a6e41fcd 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -115,7 +115,7 @@ void UPDATE_TAG(const StateTag& tag) { #define FROM_PROG_LEN(prog) ((prog) * (canon.lengthUnits == CANON_UNITS_INCHES ? 25.4 : canon.lengthUnits == CANON_UNITS_CM ? 10.0 : 1.0)) #define FROM_PROG_ANG(prog) (prog) -/* [AXIS_] TYPE and [TRAJ] FEED_AXES, axis n 0 X to 8 W */ +/* [AXIS_] TYPE and [TRAJ] FEED_AXES */ static AxisKinds kinds = axisKindsDefault(); #define AXIS_ANG(n) axisKindsAngular(kinds, (n)) @@ -289,24 +289,24 @@ static void from_prog(double &x, double &y, double &z, double &a, double &b, dou x = FROM_PROG_LEN(x); y = FROM_PROG_LEN(y); z = FROM_PROG_LEN(z); - a = FROM_PROG_AX(3, a); - b = FROM_PROG_AX(4, b); - c = FROM_PROG_AX(5, c); - u = FROM_PROG_AX(6, u); - v = FROM_PROG_AX(7, v); - w = FROM_PROG_AX(8, w); + a = FROM_PROG_AX(AXIS_A, a); + b = FROM_PROG_AX(AXIS_B, b); + c = FROM_PROG_AX(AXIS_C, c); + u = FROM_PROG_AX(AXIS_U, u); + v = FROM_PROG_AX(AXIS_V, v); + w = FROM_PROG_AX(AXIS_W, w); } static void from_prog(CANON_POSITION &pos) { pos.x = FROM_PROG_LEN(pos.x); pos.y = FROM_PROG_LEN(pos.y); pos.z = FROM_PROG_LEN(pos.z); - pos.a = FROM_PROG_AX(3, pos.a); - pos.b = FROM_PROG_AX(4, pos.b); - pos.c = FROM_PROG_AX(5, pos.c); - pos.u = FROM_PROG_AX(6, pos.u); - pos.v = FROM_PROG_AX(7, pos.v); - pos.w = FROM_PROG_AX(8, pos.w); + pos.a = FROM_PROG_AX(AXIS_A, pos.a); + pos.b = FROM_PROG_AX(AXIS_B, pos.b); + pos.c = FROM_PROG_AX(AXIS_C, pos.c); + pos.u = FROM_PROG_AX(AXIS_U, pos.u); + pos.v = FROM_PROG_AX(AXIS_V, pos.v); + pos.w = FROM_PROG_AX(AXIS_W, pos.w); } static void from_prog_len(PM_CARTESIAN &vec) { @@ -319,24 +319,24 @@ static void to_ext(double &x, double &y, double &z, double &a, double &b, double x = TO_EXT_LEN(x); y = TO_EXT_LEN(y); z = TO_EXT_LEN(z); - a = TO_EXT_AX(3, a); - b = TO_EXT_AX(4, b); - c = TO_EXT_AX(5, c); - u = TO_EXT_AX(6, u); - v = TO_EXT_AX(7, v); - w = TO_EXT_AX(8, w); + a = TO_EXT_AX(AXIS_A, a); + b = TO_EXT_AX(AXIS_B, b); + c = TO_EXT_AX(AXIS_C, c); + u = TO_EXT_AX(AXIS_U, u); + v = TO_EXT_AX(AXIS_V, v); + w = TO_EXT_AX(AXIS_W, w); } static void to_ext(CANON_POSITION & pos) { pos.x=TO_EXT_LEN(pos.x); pos.y=TO_EXT_LEN(pos.y); pos.z=TO_EXT_LEN(pos.z); - pos.a=TO_EXT_AX(3, pos.a); - pos.b=TO_EXT_AX(4, pos.b); - pos.c=TO_EXT_AX(5, pos.c); - pos.u=TO_EXT_AX(6, pos.u); - pos.v=TO_EXT_AX(7, pos.v); - pos.w=TO_EXT_AX(8, pos.w); + pos.a=TO_EXT_AX(AXIS_A, pos.a); + pos.b=TO_EXT_AX(AXIS_B, pos.b); + pos.c=TO_EXT_AX(AXIS_C, pos.c); + pos.u=TO_EXT_AX(AXIS_U, pos.u); + pos.v=TO_EXT_AX(AXIS_V, pos.v); + pos.w=TO_EXT_AX(AXIS_W, pos.w); } #endif @@ -353,12 +353,12 @@ static EmcPose to_ext_pose(double x, double y, double z, double a, double b, dou result.tran.x = TO_EXT_LEN(x); result.tran.y = TO_EXT_LEN(y); result.tran.z = TO_EXT_LEN(z); - result.a = TO_EXT_AX(3, a); - result.b = TO_EXT_AX(4, b); - result.c = TO_EXT_AX(5, c); - result.u = TO_EXT_AX(6, u); - result.v = TO_EXT_AX(7, v); - result.w = TO_EXT_AX(8, w); + result.a = TO_EXT_AX(AXIS_A, a); + result.b = TO_EXT_AX(AXIS_B, b); + result.c = TO_EXT_AX(AXIS_C, c); + result.u = TO_EXT_AX(AXIS_U, u); + result.v = TO_EXT_AX(AXIS_V, v); + result.w = TO_EXT_AX(AXIS_W, w); return result; } @@ -367,12 +367,12 @@ static EmcPose to_ext_pose(const CANON_POSITION & pos) { result.tran.x = TO_EXT_LEN(pos.x); result.tran.y = TO_EXT_LEN(pos.y); result.tran.z = TO_EXT_LEN(pos.z); - result.a = TO_EXT_AX(3, pos.a); - result.b = TO_EXT_AX(4, pos.b); - result.c = TO_EXT_AX(5, pos.c); - result.u = TO_EXT_AX(6, pos.u); - result.v = TO_EXT_AX(7, pos.v); - result.w = TO_EXT_AX(8, pos.w); + result.a = TO_EXT_AX(AXIS_A, pos.a); + result.b = TO_EXT_AX(AXIS_B, pos.b); + result.c = TO_EXT_AX(AXIS_C, pos.c); + result.u = TO_EXT_AX(AXIS_U, pos.u); + result.v = TO_EXT_AX(AXIS_V, pos.v); + result.w = TO_EXT_AX(AXIS_W, pos.w); return result; } @@ -380,12 +380,12 @@ static void to_prog(CANON_POSITION &e) { e.x = TO_PROG_LEN(e.x); e.y = TO_PROG_LEN(e.y); e.z = TO_PROG_LEN(e.z); - e.a = TO_PROG_AX(3, e.a); - e.b = TO_PROG_AX(4, e.b); - e.c = TO_PROG_AX(5, e.c); - e.u = TO_PROG_AX(6, e.u); - e.v = TO_PROG_AX(7, e.v); - e.w = TO_PROG_AX(8, e.w); + e.a = TO_PROG_AX(AXIS_A, e.a); + e.b = TO_PROG_AX(AXIS_B, e.b); + e.c = TO_PROG_AX(AXIS_C, e.c); + e.u = TO_PROG_AX(AXIS_U, e.u); + e.v = TO_PROG_AX(AXIS_V, e.v); + e.w = TO_PROG_AX(AXIS_W, e.w); } static int axis_valid(int n) { @@ -421,8 +421,8 @@ void CANON_UPDATE_END_POINT(double x, double y, double z, double u, double v, double w) { canonUpdateEndPoint(FROM_PROG_LEN(x),FROM_PROG_LEN(y),FROM_PROG_LEN(z), - FROM_PROG_AX(3, a),FROM_PROG_AX(4, b),FROM_PROG_AX(5, c), - FROM_PROG_AX(6, u),FROM_PROG_AX(7, v),FROM_PROG_AX(8, w)); + FROM_PROG_AX(AXIS_A, a),FROM_PROG_AX(AXIS_B, b),FROM_PROG_AX(AXIS_C, c), + FROM_PROG_AX(AXIS_U, u),FROM_PROG_AX(AXIS_V, v),FROM_PROG_AX(AXIS_W, w)); } static double toExtVel(double vel) { @@ -921,14 +921,14 @@ static void flush_segments(void) { linearMoveMsg->end.tran.y = TO_EXT_LEN(y); linearMoveMsg->end.tran.z = TO_EXT_LEN(z); - linearMoveMsg->end.u = TO_EXT_AX(6, u); - linearMoveMsg->end.v = TO_EXT_AX(7, v); - linearMoveMsg->end.w = TO_EXT_AX(8, w); + linearMoveMsg->end.u = TO_EXT_AX(AXIS_U, u); + linearMoveMsg->end.v = TO_EXT_AX(AXIS_V, v); + linearMoveMsg->end.w = TO_EXT_AX(AXIS_W, w); // fill in the orientation - linearMoveMsg->end.a = TO_EXT_AX(3, a); - linearMoveMsg->end.b = TO_EXT_AX(4, b); - linearMoveMsg->end.c = TO_EXT_AX(5, c); + linearMoveMsg->end.a = TO_EXT_AX(AXIS_A, a); + linearMoveMsg->end.b = TO_EXT_AX(AXIS_B, b); + linearMoveMsg->end.c = TO_EXT_AX(AXIS_C, c); linearMoveMsg->vel = toExtVel(vel); linearMoveMsg->ini_maxvel = toExtVel(linedata.vel); @@ -2439,12 +2439,12 @@ void ARC_FEED(int line_number, rotate_and_offset_pos(fe, se, ae, unused, unused, unused, unused, unused, unused); rotate_and_offset_pos(fa, sa, unused, unused, unused, unused, unused, unused, unused); if (chord_deviation(lx, ly, fe, se, fa, sa, rotation, mx, my) < canon.naivecamTolerance) { - a = FROM_PROG_AX(3, a); - b = FROM_PROG_AX(4, b); - c = FROM_PROG_AX(5, c); - u = FROM_PROG_AX(6, u); - v = FROM_PROG_AX(7, v); - w = FROM_PROG_AX(8, w); + a = FROM_PROG_AX(AXIS_A, a); + b = FROM_PROG_AX(AXIS_B, b); + c = FROM_PROG_AX(AXIS_C, c); + u = FROM_PROG_AX(AXIS_U, u); + v = FROM_PROG_AX(AXIS_V, v); + w = FROM_PROG_AX(AXIS_W, w); rotate_and_offset_pos(unused, unused, unused, a, b, c, u, v, w); see_segment(line_number, _tag, mx, my, @@ -2942,24 +2942,24 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose& offset) canon.toolOffset.tran.x = FROM_PROG_LEN(offset.tran.x); canon.toolOffset.tran.y = FROM_PROG_LEN(offset.tran.y); canon.toolOffset.tran.z = FROM_PROG_LEN(offset.tran.z); - canon.toolOffset.a = FROM_PROG_AX(3, offset.a); - canon.toolOffset.b = FROM_PROG_AX(4, offset.b); - canon.toolOffset.c = FROM_PROG_AX(5, offset.c); - canon.toolOffset.u = FROM_PROG_AX(6, offset.u); - canon.toolOffset.v = FROM_PROG_AX(7, offset.v); - canon.toolOffset.w = FROM_PROG_AX(8, offset.w); + canon.toolOffset.a = FROM_PROG_AX(AXIS_A, offset.a); + canon.toolOffset.b = FROM_PROG_AX(AXIS_B, offset.b); + canon.toolOffset.c = FROM_PROG_AX(AXIS_C, offset.c); + canon.toolOffset.u = FROM_PROG_AX(AXIS_U, offset.u); + canon.toolOffset.v = FROM_PROG_AX(AXIS_V, offset.v); + canon.toolOffset.w = FROM_PROG_AX(AXIS_W, offset.w); /* append it to interp list so it gets updated at the right time, not at read-ahead time */ set_offset_msg->offset.tran.x = TO_EXT_LEN(canon.toolOffset.tran.x); set_offset_msg->offset.tran.y = TO_EXT_LEN(canon.toolOffset.tran.y); set_offset_msg->offset.tran.z = TO_EXT_LEN(canon.toolOffset.tran.z); - set_offset_msg->offset.a = TO_EXT_AX(3, canon.toolOffset.a); - set_offset_msg->offset.b = TO_EXT_AX(4, canon.toolOffset.b); - set_offset_msg->offset.c = TO_EXT_AX(5, canon.toolOffset.c); - set_offset_msg->offset.u = TO_EXT_AX(6, canon.toolOffset.u); - set_offset_msg->offset.v = TO_EXT_AX(7, canon.toolOffset.v); - set_offset_msg->offset.w = TO_EXT_AX(8, canon.toolOffset.w); + set_offset_msg->offset.a = TO_EXT_AX(AXIS_A, canon.toolOffset.a); + set_offset_msg->offset.b = TO_EXT_AX(AXIS_B, canon.toolOffset.b); + set_offset_msg->offset.c = TO_EXT_AX(AXIS_C, canon.toolOffset.c); + set_offset_msg->offset.u = TO_EXT_AX(AXIS_U, canon.toolOffset.u); + set_offset_msg->offset.v = TO_EXT_AX(AXIS_V, canon.toolOffset.v); + set_offset_msg->offset.w = TO_EXT_AX(AXIS_W, canon.toolOffset.w); for (int s = 0; s < emcStatus->motion.traj.spindles; s++){ if(canon.spindle[s].css_maximum) { @@ -2998,15 +2998,15 @@ void CHANGE_TOOL() w = canon.endPoint.w; if (have_tool_change_position > 3) { - a = FROM_EXT_AX(3, tool_change_position.a); - b = FROM_EXT_AX(4, tool_change_position.b); - c = FROM_EXT_AX(5, tool_change_position.c); + a = FROM_EXT_AX(AXIS_A, tool_change_position.a); + b = FROM_EXT_AX(AXIS_B, tool_change_position.b); + c = FROM_EXT_AX(AXIS_C, tool_change_position.c); } if (have_tool_change_position > 6) { - u = FROM_EXT_AX(6, tool_change_position.u); - v = FROM_EXT_AX(7, tool_change_position.v); - w = FROM_EXT_AX(8, tool_change_position.w); + u = FROM_EXT_AX(AXIS_U, tool_change_position.u); + v = FROM_EXT_AX(AXIS_V, tool_change_position.v); + w = FROM_EXT_AX(AXIS_W, tool_change_position.w); } VelData veldata = getStraightVelocity(x, y, z, a, b, c, u, v, w); @@ -3402,32 +3402,32 @@ double GET_EXTERNAL_TOOL_LENGTH_ZOFFSET() double GET_EXTERNAL_TOOL_LENGTH_AOFFSET() { - return TO_PROG_AX(3, canon.toolOffset.a); + return TO_PROG_AX(AXIS_A, canon.toolOffset.a); } double GET_EXTERNAL_TOOL_LENGTH_BOFFSET() { - return TO_PROG_AX(4, canon.toolOffset.b); + return TO_PROG_AX(AXIS_B, canon.toolOffset.b); } double GET_EXTERNAL_TOOL_LENGTH_COFFSET() { - return TO_PROG_AX(5, canon.toolOffset.c); + return TO_PROG_AX(AXIS_C, canon.toolOffset.c); } double GET_EXTERNAL_TOOL_LENGTH_UOFFSET() { - return TO_PROG_AX(6, canon.toolOffset.u); + return TO_PROG_AX(AXIS_U, canon.toolOffset.u); } double GET_EXTERNAL_TOOL_LENGTH_VOFFSET() { - return TO_PROG_AX(7, canon.toolOffset.v); + return TO_PROG_AX(AXIS_V, canon.toolOffset.v); } double GET_EXTERNAL_TOOL_LENGTH_WOFFSET() { - return TO_PROG_AX(8, canon.toolOffset.w); + return TO_PROG_AX(AXIS_W, canon.toolOffset.w); } /* @@ -3581,8 +3581,8 @@ CANON_POSITION GET_EXTERNAL_POSITION() // first update internal record of last position canonUpdateEndPoint(FROM_EXT_LEN(pos.tran.x), FROM_EXT_LEN(pos.tran.y), FROM_EXT_LEN(pos.tran.z), - FROM_EXT_AX(3, pos.a), FROM_EXT_AX(4, pos.b), FROM_EXT_AX(5, pos.c), - FROM_EXT_AX(6, pos.u), FROM_EXT_AX(7, pos.v), FROM_EXT_AX(8, pos.w)); + FROM_EXT_AX(AXIS_A, pos.a), FROM_EXT_AX(AXIS_B, pos.b), FROM_EXT_AX(AXIS_C, pos.c), + FROM_EXT_AX(AXIS_U, pos.u), FROM_EXT_AX(AXIS_V, pos.v), FROM_EXT_AX(AXIS_W, pos.w)); // now calculate position in program units, for interpreter position = unoffset_and_unrotate_pos(canon.endPoint); @@ -3606,13 +3606,13 @@ CANON_POSITION GET_EXTERNAL_PROBE_POSITION() pos.tran.y = FROM_EXT_LEN(pos.tran.y); pos.tran.z = FROM_EXT_LEN(pos.tran.z); - pos.a = FROM_EXT_AX(3, pos.a); - pos.b = FROM_EXT_AX(4, pos.b); - pos.c = FROM_EXT_AX(5, pos.c); + pos.a = FROM_EXT_AX(AXIS_A, pos.a); + pos.b = FROM_EXT_AX(AXIS_B, pos.b); + pos.c = FROM_EXT_AX(AXIS_C, pos.c); - pos.u = FROM_EXT_AX(6, pos.u); - pos.v = FROM_EXT_AX(7, pos.v); - pos.w = FROM_EXT_AX(8, pos.w); + pos.u = FROM_EXT_AX(AXIS_U, pos.u); + pos.v = FROM_EXT_AX(AXIS_V, pos.v); + pos.w = FROM_EXT_AX(AXIS_W, pos.w); // now calculate position in program units, for interpreter position = unoffset_and_unrotate_pos(pos);