Menu
▾
▴
[Emc-commit] dgarr/external_offsets: external axis-offset hal pins (v25)
|
From: Dewey G. <gi...@gi...> - 2017-02-04 04:36:21
|
external axis-offset hal pins (v25)
(rebased to master at: 2377a87 2017-01-31 12:52:01 -0700)
Docs: docs/src/motion/external-offsets.txt
New hal pins (similar to wheel jog pins):
axis.L.eoffset-enable Input (bit): enable
axis.L.eoffset-scale Input (float): scale factor
axis.L.eoffset-counts Input (s32): request is counts*scale
axis.L.eoffset Output (float): current external offset
motion.eoffset-limited Output (bit): limited by soft limit
Sim config:
configs/sim/axis/external_offsets/eoffset_demo.ini
Signed-off-by: Dewey Garrett <dga...@pa...>
http://git.linuxcnc.org/?p=linuxcnc.git;a=commitdiff;h=e45787b
---
configs/sim/axis/external_offsets/README | 23 ++
configs/sim/axis/external_offsets/eoffsets.hal | 19 +
.../sim/axis/external_offsets/eoffsets_demo.ini | 91 ++++
.../sim/axis/external_offsets/eoffsets_monitor.tcl | 27 ++
.../sim/axis/external_offsets/eoffsets_panel.hal | 12 +
.../sim/axis/external_offsets/eoffsets_panel.xml | 127 ++++++
configs/sim/axis/external_offsets/rect.ngc | 21 +
docs/man/man9/motion.9 | 31 ++
docs/src/Master_Documentation.txt | 2 +
docs/src/Submakefile | 1 +
docs/src/config/ini-config.txt | 10 +
docs/src/index.tmpl | 1 +
docs/src/motion/external-offsets.txt | 96 +++++
src/emc/ini/iniaxis.cc | 34 +-
src/emc/ini/iniaxis.hh | 2 +
src/emc/ini/inihal.cc | 9 +-
src/emc/motion/command.c | 9 +
src/emc/motion/control.c | 460 ++++++++++++++++++---
src/emc/motion/mot_priv.h | 6 +
src/emc/motion/motion.c | 11 +
src/emc/motion/motion.h | 7 +
src/emc/nml_intf/emc.hh | 4 +-
src/emc/task/taskintf.cc | 6 +-
23 files changed, 951 insertions(+), 58 deletions(-)
diff --git a/configs/sim/axis/external_offsets/README b/configs/sim/axis/external_offsets/README
new file mode 100644
index 0000000..646dfd8
--- /dev/null
+++ b/configs/sim/axis/external_offsets/README
@@ -0,0 +1,23 @@
+eoffset_demo.ini
+
+Usage:
+ 1) Estop OFF (F1)
+ 2) Machine ON (F2)
+ 3) HOME All (Ctrl-Home)
+ 4) Explore external offset functionality
+ using the sim_pin gui to alter the
+ per-axis hal pins:
+
+ axis.<*>.eoffset-scale
+ e:<*>counts (=> axis.<*>eoffset-counts)
+
+-------------------------------------------
+Note: The external offset interface is
+similar to the interface for wheel jogging:
+
+Input hal pins:
+ axis.L.enable (bit)
+ axis.L.scale (float)
+ axis.L.counts (s32)
+
+Requested offset == counts*scale
diff --git a/configs/sim/axis/external_offsets/eoffsets.hal b/configs/sim/axis/external_offsets/eoffsets.hal
new file mode 100644
index 0000000..5f4f195
--- /dev/null
+++ b/configs/sim/axis/external_offsets/eoffsets.hal
@@ -0,0 +1,19 @@
+#---------------------------------------------------------------
+# external offset input control pins:
+setp axis.x.eoffset-enable 1
+setp axis.y.eoffset-enable 1
+setp axis.z.eoffset-enable 1
+
+setp axis.x.eoffset-scale 0.1
+setp axis.y.eoffset-scale 0.1
+setp axis.z.eoffset-scale 0.1
+
+#---------------------------------------------------------------
+# convenience signals for use with eoffsets_monitor.tcl:
+net e:xcounts => axis.x.eoffset-counts
+net e:ycounts => axis.y.eoffset-counts
+net e:zcounts => axis.z.eoffset-counts
+
+#---------------------------------------------------------------
+# for pyvcp panel
+net e:limited <= motion.eoffset-limited
diff --git a/configs/sim/axis/external_offsets/eoffsets_demo.ini b/configs/sim/axis/external_offsets/eoffsets_demo.ini
new file mode 100644
index 0000000..b402916
--- /dev/null
+++ b/configs/sim/axis/external_offsets/eoffsets_demo.ini
@@ -0,0 +1,91 @@
+# Notes:
+# 1) Immediate homing herein
+# ([JOINT_*]HOME_SEQUENCE=0, other HOME_* items omitted)
+# 2) [AXIS_n]OFFSET_AV_RATIO= controls external offsets a,v
+# Allowed values are 0.0 <= OFFSET_AV_RATIO <= 0.9
+# Value of 0.0 disables external offsets
+# Disallowed values are superseded with msg to stdout
+
+[APPLICATIONS]
+APP = sim_pin \
+ axis.x.eoffset-scale \
+ e:xcounts \
+ axis.y.eoffset-scale \
+ e:ycounts \
+ axis.z.eoffset-scale \
+ e:zcounts
+
+# make sure [xyz]:ecounts are zero if machine off:
+APP = eoffsets_monitor.tcl
+
+[HAL]
+HALUI = halui
+HALFILE = LIB:basic_sim.tcl
+HALFILE = eoffsets.hal
+POSTGUI_HALFILE = eoffsets_panel.hal
+
+[EMC]
+VERSION = 1.0
+
+[DISPLAY]
+PYVCP = eoffsets_panel.xml
+DISPLAY = axis
+MAX_LINEAR_VELOCITY = 111
+OPEN_FILE = ./rect.ngc
+
+[TASK]
+TASK = milltask
+CYCLE_TIME = 0.001
+
+[RS274NGC]
+PARAMETER_FILE = sim.var
+
+[EMCIO]
+EMCIO = io
+CYCLE_TIME = 0.100
+
+[EMCMOT]
+EMCMOT = motmod
+SERVO_PERIOD = 1000000
+
+[TRAJ]
+COORDINATES = XYZ
+LINEAR_UNITS = inch
+ANGULAR_UNITS = degree
+
+[KINS]
+JOINTS = 3
+KINEMATICS = trivkins coordinates=XYZ kinstype=1
+
+[AXIS_X]
+OFFSET_AV_RATIO = 0.2
+MAX_VELOCITY = 99
+MAX_ACCELERATION = 9.9
+MIN_LIMIT = -11
+MAX_LIMIT = 11
+
+[AXIS_Y]
+OFFSET_AV_RATIO = 0.2
+MAX_VELOCITY = 98
+MAX_ACCELERATION = 9.8
+MIN_LIMIT = -10
+MAX_LIMIT = 10
+
+[AXIS_Z]
+OFFSET_AV_RATIO = 0.2
+MAX_VELOCITY = 97
+MAX_ACCELERATION = 9.7
+MIN_LIMIT = -1
+MAX_LIMIT = 1
+
+[JOINT_0]
+TYPE = LINEAR
+HOME_SEQUENCE = 0
+
+[JOINT_1]
+TYPE = LINEAR
+HOME_SEQUENCE = 0
+
+[JOINT_2]
+TYPE = LINEAR
+HOME_SEQUENCE = 0
diff --git a/configs/sim/axis/external_offsets/eoffsets_monitor.tcl b/configs/sim/axis/external_offsets/eoffsets_monitor.tcl
new file mode 100755
index 0000000..898f55d
--- /dev/null
+++ b/configs/sim/axis/external_offsets/eoffsets_monitor.tcl
@@ -0,0 +1,27 @@
+#!/usr/bin/tclsh
+# script to ensure [xyz]:ecounts are zero if machine off
+
+package require Hal
+set ::prog [file tail [info script]]
+set ::periodic_delay_ms 200
+
+proc monitor_ecounts {} {
+ catch {after cancel $::a_id}
+ if { ![hal getp halui.machine.is-on]
+ && ( 0 != [hal gets e:xcounts]
+ || 0 != [hal gets e:ycounts]
+ || 0 != [hal gets e:zcounts]) } {
+ # handle deferred connection to signal
+ if [catch { hal sets e:xcounts 0
+ hal sets e:ycounts 0
+ hal sets e:zcounts 0
+ } msg] {
+ puts stderr msg=$msg
+ puts stderr "$::prog exiting"
+ exit 1
+ }
+ }
+ set ::a_id [after $::periodic_delay_ms monitor_ecounts]
+}
+monitor_ecounts
+vwait ::forever
diff --git a/configs/sim/axis/external_offsets/eoffsets_panel.hal b/configs/sim/axis/external_offsets/eoffsets_panel.hal
new file mode 100644
index 0000000..1062137
--- /dev/null
+++ b/configs/sim/axis/external_offsets/eoffsets_panel.hal
@@ -0,0 +1,12 @@
+# pyvcp led:
+net e:limited <= motion.eoffset-limited => pyvcp.limited
+
+# controls for demo (from sim_pin setp, no pin source):
+net e:panelx => axis.x.eoffset => pyvcp.x-offset-f
+net e:panely => axis.y.eoffset => pyvcp.y-offset-f
+net e:panelz => axis.z.eoffset => pyvcp.z-offset-f
+
+# existing (basic_sim.tcl) signals:
+net J0:pos-cmd => pyvcp.motor.0-f
+net J1:pos-cmd => pyvcp.motor.1-f
+net J2:pos-cmd => pyvcp.motor.2-f
diff --git a/configs/sim/axis/external_offsets/eoffsets_panel.xml b/configs/sim/axis/external_offsets/eoffsets_panel.xml
new file mode 100644
index 0000000..ee1e67f
--- /dev/null
+++ b/configs/sim/axis/external_offsets/eoffsets_panel.xml
@@ -0,0 +1,127 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<!-- Example display of external offsets
+-->
+<pyvcp>
+ <vbox>
+ <label>
+ <text>"EOffsets"</text>
+ <font>"bold"</font>
+ </label>
+
+ <vbox>
+ <relief>"sunken"</relief>
+ <bd>3</bd>
+ <hbox>
+ <label>
+ <text>"X:"</text>
+ <anchor>"w"</anchor>
+ </label>
+ <number>
+ <halpin>"x-offset-f"</halpin>
+ <format>"+10.4f"</format>
+ <width>"10"</width>
+ <bg>"black"</bg>
+ <fg>"gold"</fg>
+ <font>"bold"</font>
+ </number>
+ </hbox>
+ <hbox>
+ <label>
+ <text>"Y:"</text>
+ <anchor>"w"</anchor>
+ </label>
+ <number>
+ <halpin>"y-offset-f"</halpin>
+ <format>"+10.4f"</format>
+ <width>"10"</width>
+ <bg>"black"</bg>
+ <fg>"gold"</fg>
+ <font>"bold"</font>
+ </number>
+ </hbox>
+ <hbox>
+ <label>
+ <text>"Z:"</text>
+ <anchor>"w"</anchor>
+ </label>
+ <number>
+ <halpin>"z-offset-f"</halpin>
+ <format>"+10.4f"</format>
+ <width>"10"</width>
+ <bg>"black"</bg>
+ <fg>"gold"</fg>
+ <font>"bold"</font>
+ </number>
+ </hbox>
+
+ <hbox>
+ <led>
+ <halpin>"limited"</halpin>
+ <size>20</size>
+ <on_color>"red"</on_color>
+ <off_color>"green"</off_color>
+ </led>
+ <label>
+ <text>"Limited"</text>
+ <anchor>"w"</anchor>
+ </label>
+ </hbox>
+
+ </vbox>
+
+ <vbox>
+ <label>
+ <text>"Motor"</text>
+ <font>"bold"</font>
+ </label>
+ </vbox>
+
+ <vbox>
+ <relief>"sunken"</relief>
+ <bd>3</bd>
+ <hbox>
+ <label>
+ <text>"J0:"</text>
+ <anchor>"w"</anchor>
+ </label>
+ <number>
+ <halpin>"motor.0-f"</halpin>
+ <format>"+10.4f"</format>
+ <width>"10"</width>
+ <bg>"black"</bg>
+ <fg>"cyan"</fg>
+ <font>"bold"</font>
+ </number>
+ </hbox>
+ <hbox>
+ <label>
+ <text>"J1:"</text>
+ <anchor>"w"</anchor>
+ </label>
+ <number>
+ <halpin>"motor.1-f"</halpin>
+ <format>"+10.4f"</format>
+ <width>"10"</width>
+ <bg>"black"</bg>
+ <fg>"cyan"</fg>
+ <font>"bold"</font>
+ </number>
+ </hbox>
+ <hbox>
+ <label>
+ <text>"J2:"</text>
+ <anchor>"w"</anchor>
+ </label>
+ <number>
+ <halpin>"motor.2-f"</halpin>
+ <format>"+10.4f"</format>
+ <width>"10"</width>
+ <bg>"black"</bg>
+ <fg>"cyan"</fg>
+ <font>"bold"</font>
+ </number>
+ </hbox>
+ </vbox>
+
+ </vbox>
+</pyvcp>
diff --git a/configs/sim/axis/external_offsets/rect.ngc b/configs/sim/axis/external_offsets/rect.ngc
new file mode 100644
index 0000000..1fc9453
--- /dev/null
+++ b/configs/sim/axis/external_offsets/rect.ngc
@@ -0,0 +1,21 @@
+ #<xmax> = 10
+ #<ymax> = 5
+#<zdepth> = 0
+ #<feed> = 10
+
+g61
+g0x0y0z0
+(debug, pause: s to continue)
+m0
+
+f #<feed>
+g1 x #<xmax> y #<ymax>
+;g1 z #<zdepth>
+;g1 x #<xmax> y #<ymax>
+;g1 x-#<xmax> y #<ymax>
+;g1 x-#<xmax> y-#<ymax>
+;g1 x #<xmax> y-#<ymax>
+;g1 x #<xmax> y 0
+;g0 z 0
+
+m2
diff --git a/docs/man/man9/motion.9 b/docs/man/man9/motion.9
index c964d80..58ee09a 100644
--- a/docs/man/man9/motion.9
+++ b/docs/man/man9/motion.9
@@ -70,6 +70,30 @@ The axis's commanded velocity
.TP
\fBaxis.\fIL\fB.wheel-jog-active\fR OUT BIT
+.TP
+\fBaxis.\fIL\fB.eoffset-enable\fR IN BIT
+Enable for external offset (also requires ini file
+setting for [AXIS_L]OFFSET_AV_RATIO)
+
+.TP
+\fBaxis.\fIL\fB.eoffset-counts\fR IN s32
+Counts input for external offset.
+The eoffset-counts are transferred to an internal
+register. The applied external offset is the
+product of the register counts and the eoffset-scale
+value. The register is \fBreset to zero at each machine
+startup\fR. If the machine is turned off with an external
+offset active, the eoffset-counts pin should be set
+to zero before restarting.
+
+.TP
+\fBaxis.\fIL\fB.eoffset-scale\fR IN FLOAT
+Scale for external offset.
+
+.TP
+\fBaxis.\fIL\fB.eoffset\fR OUT FLOAT
+Current external offset.
+
.SH JOINT PINS
(\fBN\fR is the joint number (\fB0\fR ... \fBnum_joints-1\fR))
@@ -141,6 +165,8 @@ Should be driven TRUE if the positive limit switch for this joint is tripped.
\fBjoint.\fIN\fB.unlock\fR OUT BIT
TRUE if the axis is a locked rotary and a move is commanded.
+.SH OTHER PINS
+
.TP
\fBmotion.adaptive-feed\fR IN FLOAT
When adaptive feed is enabled with M52 P1, the commanded velocity is multiplied by this value. This effect is multiplicative with the NML-level feed override value and motion.feed-hold.
@@ -328,6 +354,11 @@ Spindle orient complete pin. Cleared by any of M3,M4,M5.
\fBmotion.tooloffset.w\fR OUT FLOAT
Current tool offset in all 9 axes.
+.TQ
+\fBmotion.eoffset-limited\fR OUT BIT
+Indicates motion with external offsets was limited by
+a soft limit constraint ([AXIS_L]MIN_LIMIT,MAX_LIMIT).
+
.SH UNLOCK PINS
The pins for unlocking a joint are enabled with
the \fBunlock_joints_mask=\fRjointmask parameter for motmod.
diff --git a/docs/src/Master_Documentation.txt b/docs/src/Master_Documentation.txt
index 726ab39..6ac74e2 100644
--- a/docs/src/Master_Documentation.txt
+++ b/docs/src/Master_Documentation.txt
@@ -278,6 +278,8 @@ include::motion/5-axis-kinematics.txt[]
include::motion/pid-theory.txt[]
+include::motion/external-offsets.txt[]
+
include::code/rs274.txt[]
:leveloffset: 0
diff --git a/docs/src/Submakefile b/docs/src/Submakefile
index c1e2cd4..a9ad3bf 100644
--- a/docs/src/Submakefile
+++ b/docs/src/Submakefile
@@ -125,6 +125,7 @@ DOC_SRCS_EN := \
motion/pid-theory.txt \
motion/tweaking-steppers.txt \
motion/5-axis-kinematics.txt \
+ motion/external-offsets.txt \
remap/remap.txt \
user/starting-linuxcnc.txt \
user/user-concepts.txt \
diff --git a/docs/src/config/ini-config.txt b/docs/src/config/ini-config.txt
index e1380eb..52d7e56 100644
--- a/docs/src/config/ini-config.txt
+++ b/docs/src/config/ini-config.txt
@@ -938,6 +938,16 @@ The <letter> specifies one of: X Y Z A B C U V W
Example: loadrt motmod ... unlock_joints_mask=0x38
creates unlock pins for joints 3,4,5
+* 'OFFSET_AV_RATIO = 0.1' - If nonzero, this item enables the use of
+ hal input pins for external axis offsets:
+
+ 'axis.<letter>.eoffset-enable'
+ 'axis.<letter>.eoffset-counts'
+ 'axis.<letter>.eoffset-scale'
+
+See the Chapter: <<cha:external-offsets, 'External Offsets'>> for
+external offset usage.
+
[[sec:joint-section]](((INI File, JOINT Section)))
=== [JOINT_<num>] Section
diff --git a/docs/src/index.tmpl b/docs/src/index.tmpl
index 1193fc9..4bcebdf 100644
--- a/docs/src/index.tmpl
+++ b/docs/src/index.tmpl
@@ -266,6 +266,7 @@ into LinuxCNC.
<li><a href="remap/remap.html">Remap: Extending LinuxCNC</a></li>
<li><a href="config/moveoff.html">Moveoff Component</a></li>
<li><a href="code/rs274.html">Stand Alone Interperter</a></li>
+ <li><a href="motion/external-offsets.html">External Offsets</a></li>
</ul>
</div>
diff --git a/docs/src/motion/external-offsets.txt b/docs/src/motion/external-offsets.txt
new file mode 100644
index 0000000..2f01e45
--- /dev/null
+++ b/docs/src/motion/external-offsets.txt
@@ -0,0 +1,96 @@
+[[cha:external-offsets]]
+
+= External axis offsets
+
+Hal pins are provided to offset axis positions during teleop
+(world) jogs and coordinated (gcode) motion. All
+joints must be homed before external offsets are honored.
+
+== Ini file Settings
+
+For each axis letter (*L* in xyzabcuvw):
+
+----
+[AXIS_L]OFFSET_AV_RATIO = value (controls accel/vel for external offsets)
+----
+
+. Allowed values: 0 <= value <= 0.9
+. Disallowed values are replaced with 0.1 with message to stdout
+. Default value: 0 (disables external offset).
+ Consequence: omitted [AXIS_L]OFFSET_AV_RATIO disables external offset for the axis.
+. If nonzero, the OFFSET_AV_RATIO (*r*), adjusts the normal max vel to preserve [AXIS_L] constraints:
+
+ normal max vel = (1-r) * MAX_VELOCITY
+ external offset vel = ( r) * MAX_VELOCITY
+
+ normal max accel = (1-r) * MAX_ACCELERATIOIN
+ external offset accel = ( r) * MAX_ACCELERATION
+
+== Axis Hal Pins
+
+For each axis letter (L in xyzabcuvw)
+
+. *axis.L.eoffset-enable* Input(bit): enable
+. *axis.L.eoffset-scale* Input(float): scale factor
+. *axis.L.eoffset-counts* Input(s32): input to counts register
+. *axis.L.eoffset* Output(float): current external offset
+
+
+== Motion Hal Pins
+
+. *motion.eoffset-limited* Output(bit): motion inhibited due to soft limit
+
+== Usage
+
+The axis input hal pins (enable,scale,counts) are similar to the
+pins used for wheel jogging. The value of the
+'axis.L.eoffset-counts' pin is transferred to an internal
+counts register that is reset to zero at each machine
+startup. The requested external offset is the product of
+the register counts and the value of the
+'axis.L.eoffset-scale' pin.
+
+For testing, it is convenient to simulate a jog wheel using the
+sim_pin gui. For example:
+
+----
+$ sim_pin axis.z.eoffset-enable axis.z.eoffset-scale axis.z.eoffset-counts
+----
+
+=== Soft Limits
+
+During teleop (world) jogging and coordinated (gcode) motion,
+axis soft limits constrain travel. When operating at an axis
+limit, the hal pin 'motion.eoffset-limited' is asserted.
+
+Note: When a soft limit is encountered, motion is stopped
+and axis acceleration limits may be violated.
+
+=== Machine-off/Machine-on
+
+When the machine is turned off, the *current position with
+external offsets is maintained* so that there is no
+unexpected motion at turn off or turn on.
+
+At each startup (machine on), the internal counts register for
+each hal pin 'axis.L.eoffset-counts' is zeroed and the
+corresponding hal output pin 'axis.L.eoffset' is reset to zero.
+
+In other words, external offsets are *defined as ZERO at
+each startup* (machine on) regardless of the value of
+the 'axis.L.eoffset-counts' pins. To avoid confusion, it is
+recommended that all 'axis.L.eoffset-counts' pins are set to
+zero when the machine is off.
+
+== Examples
+
+The sim config 'sim/configs/axis/exernal_offsets/eoffsets_demo.ini'
+demonstrates:
+
+. a cartesian XYZ machine (identity kinematics x=joint0,y=joint1,z=joint2)
+. sim_pin buttons for the axis control pins eoffset-scale & eoffset-counts
+. pyvcp panel LED for the pin: motion.eoffset-limited
+. pyvcp panel numeric displays for the external offsets: axis.L.eoffset
+. pyvcp panel numeric displays for the motor (joint) commands: joint.N.pos-cm
+. hal logic to ensure that the axis.L.counts pins are set to zero at machine off
+
diff --git a/src/emc/ini/iniaxis.cc b/src/emc/ini/iniaxis.cc
index 2246ada..2f09025 100644
--- a/src/emc/ini/iniaxis.cc
+++ b/src/emc/ini/iniaxis.cc
@@ -31,6 +31,10 @@
#include "inihal.hh"
extern value_inihal_data old_inihal_data;
+double ext_offset_a_or_v_ratio[EMCMOT_MAX_AXIS]; // all zero
+
+// default ratio or external offset velocity,acceleration
+#define DEFAULT_A_OR_V_RATIO 0
/*
loadAxis(int axis)
@@ -47,8 +51,8 @@ extern value_inihal_data old_inihal_data;
emcAxisSetMinPositionLimit(int axis, double limit);
emcAxisSetMaxPositionLimit(int axis, double limit);
- emcAxisSetMaxVelocity(int axis, double vel);
- emcAxisSetMaxAcceleration(int axis, double acc);
+ emcAxisSetMaxVelocity(int axis, double vel, double ext_offset_vel);
+ emcAxisSetMaxAcceleration(int axis, double acc, double ext_offset_acc);
*/
static int loadAxis(int axis, EmcIniFile *axisIniFile)
@@ -97,10 +101,27 @@ static int loadAxis(int axis, EmcIniFile *axisIniFile)
}
old_inihal_data.axis_max_limit[axis] = limit;
- // set maximum velocity
+ ext_offset_a_or_v_ratio[axis] = DEFAULT_A_OR_V_RATIO;
+ axisIniFile->Find(&ext_offset_a_or_v_ratio[axis], "OFFSET_AV_RATIO", axisString);
+
+#define REPLACE_AV_RATIO 0.1
+#define MAX_AV_RATIO 0.9
+ if ( (ext_offset_a_or_v_ratio[axis] < 0)
+ || (ext_offset_a_or_v_ratio[axis] > MAX_AV_RATIO)
+ ) {
+ rcs_print_error("\n Invalid:[%s]OFFSET_AV_RATIO= %8.5f\n"
+ " Using: [%s]OFFSET_AV_RATIO= %8.5f\n",
+ axisString,ext_offset_a_or_v_ratio[axis],
+ axisString,REPLACE_AV_RATIO);
+ ext_offset_a_or_v_ratio[axis] = REPLACE_AV_RATIO;
+ }
+
+ // set maximum velocities for axis: vel,ext_offset_vel
maxVelocity = DEFAULT_AXIS_MAX_VELOCITY;
axisIniFile->Find(&maxVelocity, "MAX_VELOCITY", axisString);
- if (0 != emcAxisSetMaxVelocity(axis, maxVelocity)) {
+ if (0 != emcAxisSetMaxVelocity(axis,
+ (1 - ext_offset_a_or_v_ratio[axis]) * maxVelocity,
+ ( ext_offset_a_or_v_ratio[axis]) * maxVelocity)) {
if (emc_debug & EMC_DEBUG_CONFIG) {
rcs_print_error("bad return from emcAxisSetMaxVelocity\n");
}
@@ -108,9 +129,12 @@ static int loadAxis(int axis, EmcIniFile *axisIniFile)
}
old_inihal_data.axis_max_velocity[axis] = maxVelocity;
+ // set maximum accels for axis: acc,ext_offset_acc
maxAcceleration = DEFAULT_AXIS_MAX_ACCELERATION;
axisIniFile->Find(&maxAcceleration, "MAX_ACCELERATION", axisString);
- if (0 != emcAxisSetMaxAcceleration(axis, maxAcceleration)) {
+ if (0 != emcAxisSetMaxAcceleration(axis,
+ (1 - ext_offset_a_or_v_ratio[axis]) * maxAcceleration,
+ ( ext_offset_a_or_v_ratio[axis]) * maxAcceleration)) {
if (emc_debug & EMC_DEBUG_CONFIG) {
rcs_print_error("bad return from emcAxisSetMaxAcceleration\n");
}
diff --git a/src/emc/ini/iniaxis.hh b/src/emc/ini/iniaxis.hh
index c870b51..4a53fa0 100644
--- a/src/emc/ini/iniaxis.hh
+++ b/src/emc/ini/iniaxis.hh
@@ -19,4 +19,6 @@
/* initializes axis modules from ini file */
extern int iniAxis(int axis, const char *filename);
+extern double ext_offset_a_or_v_ratio[];
+
#endif
diff --git a/src/emc/ini/inihal.cc b/src/emc/ini/inihal.cc
index 2e8829f..e8f2c52 100644
--- a/src/emc/ini/inihal.cc
+++ b/src/emc/ini/inihal.cc
@@ -25,6 +25,7 @@ Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#include "hal.h"
#include "rtapi.h"
#include "inihal.hh"
+#include "iniaxis.hh"
static int debug=0;
static int comp_id;
@@ -389,7 +390,9 @@ int check_ini_hal_items(int numjoints)
if (CHANGED_IDX(axis_max_velocity,idx) ) {
if (debug) SHOW_CHANGE_IDX(axis_max_velocity,idx);
UPDATE_IDX(axis_max_velocity,idx);
- if (0 != emcAxisSetMaxVelocity(idx,NEW(axis_max_velocity[idx]))) {
+ if (0 != emcAxisSetMaxVelocity(idx,
+ (1 - ext_offset_a_or_v_ratio[idx]) * NEW(axis_max_velocity[idx]),
+ ( ext_offset_a_or_v_ratio[idx]) * NEW(axis_max_velocity[idx]))) {
if (emc_debug & EMC_DEBUG_CONFIG) {
rcs_print_error("check_ini_hal_items:bad return from emcAxisSetMaxVelocity\n");
}
@@ -398,7 +401,9 @@ int check_ini_hal_items(int numjoints)
if (CHANGED_IDX(axis_max_acceleration,idx) ) {
if (debug) SHOW_CHANGE_IDX(axis_max_acceleration,idx);
UPDATE_IDX(axis_max_acceleration,idx);
- if (0 != emcAxisSetMaxAcceleration(idx,NEW(axis_max_acceleration[idx]))) {
+ if (0 != emcAxisSetMaxAcceleration(idx,
+ (1 - ext_offset_a_or_v_ratio[idx]) * NEW(axis_max_acceleration[idx]),
+ ( ext_offset_a_or_v_ratio[idx]) * NEW(axis_max_acceleration[idx]))) {
if (emc_debug & EMC_DEBUG_CONFIG) {
rcs_print_error("check_ini_hal_items:bad return from emcAxisSetMaxAcceleration\n");
}
diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c
index da081d4..cdb7a9e 100644
--- a/src/emc/motion/command.c
+++ b/src/emc/motion/command.c
@@ -193,6 +193,12 @@ static int check_axis_constraint(double target, int id, char *move_type,
double nl = axes[axis_no].min_pos_limit;
double pl = axes[axis_no].max_pos_limit;
+ double eps = 1e-308;
+
+ if ( (fabs(target) < eps)
+ && (fabs(axes[axis_no].min_pos_limit) < eps)
+ && (fabs(axes[axis_no].max_pos_limit) < eps) ) { return 1;}
+
if(target < nl) {
in_range = 0;
reportError(_("%s move on line %d would exceed %c's %s limit"),
@@ -1007,6 +1013,7 @@ void emcmotCommandHandler(void *arg, long period)
SET_MOTION_ERROR_FLAG(1);
break;
} else if (!inRange(emcmotCommand->pos, emcmotCommand->id, "Linear")) {
+ reportError(_("invalid params in linear command"));
emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
tpAbort(&emcmotDebug->coord_tp);
SET_MOTION_ERROR_FLAG(1);
@@ -1753,6 +1760,7 @@ void emcmotCommandHandler(void *arg, long period)
break;
}
axis->vel_limit = emcmotCommand->vel;
+ axis->ext_offset_vel_limit = emcmotCommand->ext_offset_vel;
break;
case EMCMOT_SET_AXIS_ACC_LIMIT:
@@ -1765,6 +1773,7 @@ void emcmotCommandHandler(void *arg, long period)
break;
}
axis->acc_limit = emcmotCommand->acc;
+ axis->ext_offset_acc_limit = emcmotCommand->ext_offset_acc;
break;
case EMCMOT_SET_AXIS_LOCKING_JOINT:
diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c
index ad34f4e..3e37a69 100644
--- a/src/emc/motion/control.c
+++ b/src/emc/motion/control.c
@@ -28,7 +28,8 @@
// Mark strings for translation, but defer translation to userspace
#define _(s) (s)
-
+static int ext_offset_teleop_limit = 0;
+static int ext_offset_coord_limit = 0;
/* kinematics flags */
KINEMATICS_FORWARD_FLAGS fflags = 0;
KINEMATICS_INVERSE_FLAGS iflags = 0;
@@ -51,6 +52,38 @@ static unsigned long last_period = 0;
/* servo cycle time */
static double servo_period;
+#define dprint(format, ...) rtapi_print_msg(RTAPI_MSG_INFO,format, ##__VA_ARGS__)
+
+
+#undef EDEBUG
+#ifdef EDEBUG
+/* TEMPORARY debug items *******************************************/
+#define TMP_LEVEL RTAPI_MSG_INFO
+static int dbg_ct;
+static int dbg_enable_ct;
+static int dbg_disable_ct;
+
+static void dbg_show(char*txt) {
+ int ano;
+ emcmot_axis_t *a;
+ char afmt[]= "%6d %4s A%d T%d C%d I%d E(cmd=%7.4f curr=%7.4f) T(cmd=%7.4f curr=%7.4f) V:%7.4f\n";
+ dprint("\n");
+ for (ano=2; ano<3; ano++) {
+ double v;
+ if (ano == 0) { v=emcmotStatus->carte_pos_cmd.tran.x;
+ } else if (ano == 1) { v=emcmotStatus->carte_pos_cmd.tran.y;
+ } else if (ano == 2) { v=emcmotStatus->carte_pos_cmd.tran.z;
+ } else { v=999; }
+ a = &axes[ano];
+ dprint(afmt,dbg_ct,txt,ano
+ ,GET_MOTION_TELEOP_FLAG(),GET_MOTION_COORD_FLAG(),GET_MOTION_INPOS_FLAG()
+ ,a->ext_offset_tp.pos_cmd, a->ext_offset_tp.curr_pos
+ ,v
+ );
+ }
+}
+#endif
+
/***********************************************************************
* LOCAL FUNCTION PROTOTYPES *
************************************************************************/
@@ -165,6 +198,14 @@ static void output_to_hal(void);
*/
static void update_status(void);
+static void initialize_external_offsets(void);
+static void plan_external_offsets(void);
+static void sync_teleop_tp_to_carte_pos(int);
+static void sync_carte_pos_to_teleop_tp(int);
+static void apply_ext_offsets_to_carte_pos(int);
+static int update_coord_with_bound(void);
+static int update_teleop_with_check(int,simple_tp_t*);
+
/***********************************************************************
* PUBLIC FUNCTION CODE *
************************************************************************/
@@ -189,9 +230,19 @@ void emcmotController(void *arg, long period)
#ifdef HAVE_CPU_KHZ
*(emcmot_hal_data->last_period_ns) = this_run * 1e6 / cpu_khz;
#endif
+
+#ifdef TMP_LEVEL
+static int do_once = 1;
+ if (do_once) {
+ rtapi_set_msg_level(TMP_LEVEL);
+ do_once = 0;
+ }
+#endif
+
// we need this for next time
last = now;
+
/* calculate servo period as a double - period is in integer nsec */
servo_period = period * 0.000000001;
@@ -207,6 +258,9 @@ void emcmotController(void *arg, long period)
emcmotStatus->head++;
/* here begins the core of the controller */
+#ifdef EDEBUG
+ dbg_ct++;
+#endif
process_inputs();
do_forward_kins();
process_probe_inputs();
@@ -218,6 +272,7 @@ void emcmotController(void *arg, long period)
do_homing();
get_pos_cmds(period);
compute_screw_comp();
+ plan_external_offsets();
output_to_hal();
update_status();
/* here ends the core of the controller */
@@ -230,7 +285,6 @@ void emcmotController(void *arg, long period)
/***********************************************************************
* LOCAL FUNCTION CODE *
************************************************************************/
-
/* The protoypes and documentation for these functions are located
at the top of the file in the section called "local function
prototypes"
@@ -673,6 +727,7 @@ static void set_operating_mode(void)
/* check for disabling */
if (!emcmotDebug->enabling && GET_MOTION_ENABLE_FLAG()) {
+ //dbg_show("dsbl");dbg_disable_ct=dbg_ct;
/* clear out the motion emcmotDebug->coord_tp and interpolators */
tpClear(&emcmotDebug->coord_tp);
for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) {
@@ -708,11 +763,16 @@ static void set_operating_mode(void)
/* check for emcmotDebug->enabling */
if (emcmotDebug->enabling && !GET_MOTION_ENABLE_FLAG()) {
- tpSetPos(&emcmotDebug->coord_tp, &emcmotStatus->carte_pos_cmd);
+ //dbg_show("enbl");dbg_enable_ct=dbg_ct;
+ if (*(emcmot_hal_data->eoffset_limited)) {
+ reportError("Starting beyond Soft Limits");
+ *(emcmot_hal_data->eoffset_limited) = 0;
+ }
+ initialize_external_offsets();
+ tpSetPos(&emcmotDebug->coord_tp, &emcmotStatus->carte_pos_cmd);
for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
-
joint->free_tp.curr_pos = joint->pos_cmd;
if (GET_JOINT_ACTIVE_FLAG(joint)) {
SET_JOINT_ENABLE_FLAG(joint, 1);
@@ -724,17 +784,9 @@ static void set_operating_mode(void)
SET_JOINT_ERROR_FLAG(joint, 0);
}
if ( !GET_MOTION_ENABLE_FLAG() ) {
- if (GET_MOTION_TELEOP_FLAG()) {
- (&axes[0])->teleop_tp.curr_pos = emcmotStatus->carte_pos_cmd.tran.x;
- (&axes[1])->teleop_tp.curr_pos = emcmotStatus->carte_pos_cmd.tran.y;
- (&axes[2])->teleop_tp.curr_pos = emcmotStatus->carte_pos_cmd.tran.z;
- (&axes[3])->teleop_tp.curr_pos = emcmotStatus->carte_pos_cmd.a;
- (&axes[4])->teleop_tp.curr_pos = emcmotStatus->carte_pos_cmd.b;
- (&axes[5])->teleop_tp.curr_pos = emcmotStatus->carte_pos_cmd.c;
- (&axes[6])->teleop_tp.curr_pos = emcmotStatus->carte_pos_cmd.u;
- (&axes[7])->teleop_tp.curr_pos = emcmotStatus->carte_pos_cmd.v;
- (&axes[8])->teleop_tp.curr_pos = emcmotStatus->carte_pos_cmd.w;
- }
+ if (GET_MOTION_TELEOP_FLAG()) {
+ sync_teleop_tp_to_carte_pos(0);
+ }
}
SET_MOTION_ENABLE_FLAG(1);
/* clear any outstanding motion errors when going into enabled state */
@@ -759,16 +811,8 @@ static void set_operating_mode(void)
SET_MOTION_ERROR_FLAG(0);
kinematicsForward(positions, &emcmotStatus->carte_pos_cmd, &fflags, &iflags);
-
- (&axes[0])->teleop_tp.curr_pos = emcmotStatus->carte_pos_cmd.tran.x;
- (&axes[1])->teleop_tp.curr_pos = emcmotStatus->carte_pos_cmd.tran.y;
- (&axes[2])->teleop_tp.curr_pos = emcmotStatus->carte_pos_cmd.tran.z;
- (&axes[3])->teleop_tp.curr_pos = emcmotStatus->carte_pos_cmd.a;
- (&axes[4])->teleop_tp.curr_pos = emcmotStatus->carte_pos_cmd.b;
- (&axes[5])->teleop_tp.curr_pos = emcmotStatus->carte_pos_cmd.c;
- (&axes[6])->teleop_tp.curr_pos = emcmotStatus->carte_pos_cmd.u;
- (&axes[7])->teleop_tp.curr_pos = emcmotStatus->carte_pos_cmd.v;
- (&axes[8])->teleop_tp.curr_pos = emcmotStatus->carte_pos_cmd.w;
+ // entering teleop (INPOS), remove ext offsets
+ sync_teleop_tp_to_carte_pos(-1);
} else {
/* not in position-- don't honor mode change */
emcmotDebug->teleoperating = 0;
@@ -792,6 +836,9 @@ static void set_operating_mode(void)
if (emcmotDebug->coordinating && !GET_MOTION_COORD_FLAG()) {
if (GET_MOTION_INPOS_FLAG()) {
/* preset traj planner to current position */
+
+ apply_ext_offsets_to_carte_pos(-1); // subtract at coord mode start
+
tpSetPos(&emcmotDebug->coord_tp, &emcmotStatus->carte_pos_cmd);
/* drain the cubics so they'll synch up */
for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) {
@@ -843,7 +890,7 @@ static void set_operating_mode(void)
} else {
emcmotStatus->motion_state = EMCMOT_MOTION_FREE;
}
-}
+} //set_operating_mode
static void handle_jjogwheels(void)
{
@@ -1024,7 +1071,7 @@ static void handle_ajogwheels(void)
axis->teleop_tp.pos_cmd = pos;
axis->teleop_tp.max_vel = axis->vel_limit;
axis->teleop_tp.max_acc = axis->acc_limit;
- axis->wheel_ajog_active = 1;
+ axis->wheel_ajog_active = 1;
axis->teleop_tp.enable = 1;
}
first_pass = 0;
@@ -1042,6 +1089,7 @@ static void get_pos_cmds(long period)
/* used in teleop mode to compute the max accell requested */
int onlimit = 0;
int joint_limit[EMCMOT_MAX_JOINTS][2];
|
