From 08ca67aa0efb5c32b2f17032810d406face2b7c3 Mon Sep 17 00:00:00 2001 From: Dewey Garrett Date: Fri, 24 Feb 2017 09:43:34 -0700 Subject: [PATCH] External Axis Offset hal pins (65: rebased to master at: 2603145 2017-05-20 16:02:37 -0700) Docs: docs/src/motion/external-offsets.txt Hal pins (input interface is similar to wheel jogging 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-clear Input (bit): clear request axis.L.eoffset Output (float): current external offset axis.L.eoffset-request Output (float): requested external offset (debug pin) motion.eoffset-active Output (bit): non-zero external offsets applied motion.eoffset-limited Output (bit): limited by soft limit Hal components: eoffset_per_angle.comp eoffset_pid.comp Sim configs (configs/sim/axis/external_offsets/) eoffset_demo.ini (XYZ ext offsets) jwp_z.ini (jog-while-pause Z) dynamic_offsets.ini (dynamic Z) opa.ini (X ext offset per C angle) hpid.ini (torch height control using eoffset_pid) Notes: 1) see docs/src/motion/external_offsets.txt for usage restrictions man updated: motion.9 new manpage: offset_per_angle.9 (auto by halcompile) new manpage: eoffset_pid.9 (auto by halcompile) 2) In canon.hh, option: #undef STOP_ON_SYNCH_IF_EXTERNAL_OFFSETS #define STOP_ON_SYNCH_IF_EXTERNAL_OFFSETS forces a stop on any synch() if non-zero external offsets. Use #undef for testing -- works for many cases but not for queue-busters within program loops (repeat,while,do-while) using gcodes (like g0,g1) that omit axis letters 3) In command.c, using ALT_EOFFSET_BEHAVIOR for cont/incr jogs Signed-off-by: Dewey Garrett --- configs/sim/axis/external_offsets/M111 | 5 + configs/sim/axis/external_offsets/README | 5 + configs/sim/axis/external_offsets/ccircle.ngc | 48 ++ .../sim/axis/external_offsets/dyn_demo.ngc | 33 ++ .../external_offsets/dynamic_offsets.halscope | 27 + .../axis/external_offsets/dynamic_offsets.ini | 81 +++ .../axis/external_offsets/dynamic_offsets.txt | 32 ++ .../dynamic_offsets_panel.hal | 46 ++ .../dynamic_offsets_panel.xml | 166 ++++++ configs/sim/axis/external_offsets/eoffset.tbl | 1 + .../sim/axis/external_offsets/eoffsets.hal | 16 + .../sim/axis/external_offsets/eoffsets.ini | 100 ++++ .../sim/axis/external_offsets/eoffsets.ngc | 35 ++ .../sim/axis/external_offsets/eoffsets.txt | 26 + .../external_offsets/eoffsets_monitor.tcl | 27 + .../axis/external_offsets/eoffsets_panel.hal | 49 ++ .../axis/external_offsets/eoffsets_panel.xml | 382 ++++++++++++++ configs/sim/axis/external_offsets/hpid.hal | 106 ++++ .../sim/axis/external_offsets/hpid.halscope | 47 ++ configs/sim/axis/external_offsets/hpid.ini | 104 ++++ configs/sim/axis/external_offsets/hpid.txt | 178 +++++++ .../sim/axis/external_offsets/hpid_demo.ngc | 3 + .../sim/axis/external_offsets/hpid_panel.hal | 79 +++ .../sim/axis/external_offsets/hpid_panel.xml | 229 +++++++++ .../sim/axis/external_offsets/hpid_sub.ngc | 76 +++ .../axis/external_offsets/hpid_touchoff.ngc | 47 ++ configs/sim/axis/external_offsets/jwp_z.ini | 84 ++++ configs/sim/axis/external_offsets/jwp_z.ngc | 35 ++ configs/sim/axis/external_offsets/jwp_z.txt | 16 + .../sim/axis/external_offsets/jwp_z_panel.hal | 17 + .../sim/axis/external_offsets/jwp_z_panel.xml | 112 +++++ configs/sim/axis/external_offsets/opa.hal | 47 ++ configs/sim/axis/external_offsets/opa.ini | 82 +++ configs/sim/axis/external_offsets/opa.txt | 40 ++ .../sim/axis/external_offsets/opa_demo.ngc | 13 + .../sim/axis/external_offsets/opa_panel.hal | 31 ++ .../sim/axis/external_offsets/opa_panel.xml | 231 +++++++++ .../sim/axis/external_offsets/queuebuster.ngc | 45 ++ .../sim/axis/external_offsets/sim_torch.hal | 64 +++ docs/man/man9/motion.9 | 46 +- 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 | 373 ++++++++++++++ src/emc/ini/iniaxis.cc | 34 +- src/emc/ini/iniaxis.hh | 2 + src/emc/ini/inihal.cc | 9 +- src/emc/motion/command.c | 90 +++- src/emc/motion/control.c | 450 ++++++++++++++--- src/emc/motion/mot_priv.h | 11 + src/emc/motion/motion.c | 19 + src/emc/motion/motion.h | 11 +- src/emc/nml_intf/canon.hh | 6 + src/emc/nml_intf/emc.hh | 6 +- src/emc/nml_intf/emc_nml.hh | 2 + src/emc/rs274ngc/canonmodule.cc | 3 + src/emc/rs274ngc/gcodemodule.cc | 16 +- src/emc/rs274ngc/rs274ngc_pre.cc | 37 +- src/emc/sai/saicanon.cc | 18 + src/emc/task/emccanon.cc | 8 + src/emc/task/emctask.cc | 15 +- src/emc/task/emctaskmain.cc | 40 +- src/emc/task/taskintf.cc | 15 +- src/hal/components/eoffset_per_angle.comp | 298 +++++++++++ src/hal/components/eoffset_pid.comp | 476 ++++++++++++++++++ tests/interp/compile/use-rs274.cc | 2 + 67 files changed, 4661 insertions(+), 105 deletions(-) create mode 100755 configs/sim/axis/external_offsets/M111 create mode 100644 configs/sim/axis/external_offsets/README create mode 100644 configs/sim/axis/external_offsets/ccircle.ngc create mode 100644 configs/sim/axis/external_offsets/dyn_demo.ngc create mode 100644 configs/sim/axis/external_offsets/dynamic_offsets.halscope create mode 100644 configs/sim/axis/external_offsets/dynamic_offsets.ini create mode 100644 configs/sim/axis/external_offsets/dynamic_offsets.txt create mode 100644 configs/sim/axis/external_offsets/dynamic_offsets_panel.hal create mode 100644 configs/sim/axis/external_offsets/dynamic_offsets_panel.xml create mode 100644 configs/sim/axis/external_offsets/eoffset.tbl create mode 100644 configs/sim/axis/external_offsets/eoffsets.hal create mode 100644 configs/sim/axis/external_offsets/eoffsets.ini create mode 100644 configs/sim/axis/external_offsets/eoffsets.ngc create mode 100644 configs/sim/axis/external_offsets/eoffsets.txt create mode 100755 configs/sim/axis/external_offsets/eoffsets_monitor.tcl create mode 100644 configs/sim/axis/external_offsets/eoffsets_panel.hal create mode 100644 configs/sim/axis/external_offsets/eoffsets_panel.xml create mode 100644 configs/sim/axis/external_offsets/hpid.hal create mode 100644 configs/sim/axis/external_offsets/hpid.halscope create mode 100644 configs/sim/axis/external_offsets/hpid.ini create mode 100644 configs/sim/axis/external_offsets/hpid.txt create mode 100644 configs/sim/axis/external_offsets/hpid_demo.ngc create mode 100644 configs/sim/axis/external_offsets/hpid_panel.hal create mode 100644 configs/sim/axis/external_offsets/hpid_panel.xml create mode 100644 configs/sim/axis/external_offsets/hpid_sub.ngc create mode 100644 configs/sim/axis/external_offsets/hpid_touchoff.ngc create mode 100644 configs/sim/axis/external_offsets/jwp_z.ini create mode 100644 configs/sim/axis/external_offsets/jwp_z.ngc create mode 100644 configs/sim/axis/external_offsets/jwp_z.txt create mode 100644 configs/sim/axis/external_offsets/jwp_z_panel.hal create mode 100644 configs/sim/axis/external_offsets/jwp_z_panel.xml create mode 100644 configs/sim/axis/external_offsets/opa.hal create mode 100644 configs/sim/axis/external_offsets/opa.ini create mode 100644 configs/sim/axis/external_offsets/opa.txt create mode 100644 configs/sim/axis/external_offsets/opa_demo.ngc create mode 100644 configs/sim/axis/external_offsets/opa_panel.hal create mode 100644 configs/sim/axis/external_offsets/opa_panel.xml create mode 100644 configs/sim/axis/external_offsets/queuebuster.ngc create mode 100644 configs/sim/axis/external_offsets/sim_torch.hal create mode 100644 docs/src/motion/external-offsets.txt create mode 100644 src/hal/components/eoffset_per_angle.comp create mode 100644 src/hal/components/eoffset_pid.comp diff --git a/configs/sim/axis/external_offsets/M111 b/configs/sim/axis/external_offsets/M111 new file mode 100755 index 00000000000..718681d78de --- /dev/null +++ b/configs/sim/axis/external_offsets/M111 @@ -0,0 +1,5 @@ +#!/bin/bash +halcmd setp axisui.notifications-clear-info 1 +sleep .1 +halcmd setp axisui.notifications-clear-info 0 +exit 0 diff --git a/configs/sim/axis/external_offsets/README b/configs/sim/axis/external_offsets/README new file mode 100644 index 00000000000..14284b78811 --- /dev/null +++ b/configs/sim/axis/external_offsets/README @@ -0,0 +1,5 @@ +eoffset_demo.ini ----- Basic demo (XYZ) +jwp_z.ini ------------ Jog while paused (Z) demo +dynamic_offsets.ini -- Z offset waveforms demo +opa.ini -------------- Offset per Angle (XZC) demo +hpid.ini ------------- Torch height PID demo diff --git a/configs/sim/axis/external_offsets/ccircle.ngc b/configs/sim/axis/external_offsets/ccircle.ngc new file mode 100644 index 00000000000..8d46ea54aaa --- /dev/null +++ b/configs/sim/axis/external_offsets/ccircle.ngc @@ -0,0 +1,48 @@ +o sub + # = #1 (= 1 radius) + # = #2 (= 2000 feedrate) + # = #3 (= 2 +/-Revs) + +# = [# * 360] +# = 0 +# = 0 + + +;------------------------------------ +g92 C 0 +# = #5216 +# = [# MOD 360] +o if [# lt 0] + # = [# -360] +o endif +g92.1 +g10l20p1 C # +;------------------------------------ + +(debug, one moment --> C#) +g0 c# +g0 x# z# +m68 e0 q# ; set radius for eoffsets +(debug, Enable offsets, s to continue) +m1 ; optional stop +m111 + +f# +g1 c[# * 360] +(debug, Disable offsets, s to continue) +m1 ; optional stop +g0 x0 + +;------------------------------------ +g92 C 0 +# = #5216 +# = [# MOD 360] +o if [# lt 0] + # = [# -360] +o endif +g92.1 +g10l20p1 C # +;------------------------------------ +m111 +o endsub + diff --git a/configs/sim/axis/external_offsets/dyn_demo.ngc b/configs/sim/axis/external_offsets/dyn_demo.ngc new file mode 100644 index 00000000000..621f825d332 --- /dev/null +++ b/configs/sim/axis/external_offsets/dyn_demo.ngc @@ -0,0 +1,33 @@ +# = 9 +# = 4 +# = 3 +# = 100 +# = 100 + +# = # +# = 0 +# = [# * 0.5] + +# = # +# = # +# = 0 + +# = 0 +# = # +# = # + +m111 ;clear notifications +g61 +g0 x0 y0 z0 + +o repeat [#] +f # +g0 x 0 y 0 z 0 +g1 x # y # z # +g1 x # y # z # +g1 x # y # z # +g1 x 0 y 0 z 0 +o endrepeat + +m111 ;clear notifications +m2 diff --git a/configs/sim/axis/external_offsets/dynamic_offsets.halscope b/configs/sim/axis/external_offsets/dynamic_offsets.halscope new file mode 100644 index 00000000000..35662663ca5 --- /dev/null +++ b/configs/sim/axis/external_offsets/dynamic_offsets.halscope @@ -0,0 +1,27 @@ +THREAD servo-thread +MAXCHAN 8 +HMULT 4 +HZOOM 1 +HPOS 5.186916e-01 +CHAN 1 +SIG E:z-offset +VSCALE -1 +VPOS 0.398564 +VOFF 0.000000e+00 +CHAN 2 +SIG G:waveform +VSCALE -1 +VPOS 0.398564 +VOFF 0.000000e+00 +CHAN 4 +SIG G:reset +VSCALE 2 +VPOS 0.961120 +VOFF 0.000000e+00 +CHAN 3 +PIN joint.2.motor-pos-cmd +VSCALE 0 +VPOS 0.900000 +VOFF 0.000000e+00 +TMODE 0 +RMODE 0 diff --git a/configs/sim/axis/external_offsets/dynamic_offsets.ini b/configs/sim/axis/external_offsets/dynamic_offsets.ini new file mode 100644 index 00000000000..b80996c3582 --- /dev/null +++ b/configs/sim/axis/external_offsets/dynamic_offsets.ini @@ -0,0 +1,81 @@ +[APPLICATIONS] +# delay because some items are done in postgui +DELAY = 5 +APP = halscope -i dynamic_offsets.halscope + +[HAL] +HALUI = halui +HALFILE = LIB:basic_sim.tcl +POSTGUI_HALFILE = dynamic_offsets_panel.hal + +[EMC] +MACHINE = Dynamic External Offsets +VERSION = 1.0 + +[DISPLAY] +PYVCP = dynamic_offsets_panel.xml +DISPLAY = axis +POSITION_OFFSET = RELATIVE +POSITION_FEEDBACK = ACTUAL +MAX_LINEAR_VELOCITY = 2 +OPEN_FILE = ./dyn_demo.ngc + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[RS274NGC] +USER_M_PATH = . +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 + +[AXIS_X] +MAX_VELOCITY = 2 +MAX_ACCELERATION = 20 +MIN_LIMIT = -1 +MAX_LIMIT = 10 + +[AXIS_Y] +MAX_VELOCITY = 2 +MAX_ACCELERATION = 20 +MIN_LIMIT = -1 +MAX_LIMIT = 10 + +[AXIS_Z] +# Note:give half to external_offsets: +OFFSET_AV_RATIO = 0.5 + +# Note: modified in pyvcp panel: +MAX_VELOCITY = 2 +MAX_ACCELERATION = 50 + +MIN_LIMIT = -5 +MAX_LIMIT = 5 + +[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/dynamic_offsets.txt b/configs/sim/axis/external_offsets/dynamic_offsets.txt new file mode 100644 index 00000000000..538ee46fd8d --- /dev/null +++ b/configs/sim/axis/external_offsets/dynamic_offsets.txt @@ -0,0 +1,32 @@ +dynamic_offsets + +A sinewave is applied to the Z coordinate external offset + +The waveform amplitdude and frequency are set by pyvcp panel spinboxes. + +The Z coordinate max acceleration and max velocity settings (from the ini file) can be modified with pyvcp panel spinboxes. + +Usage: + 1) Estop OFF (F1) + 2) Machine ON (F2) + 3) HOME All (Ctrl-Home) + 4) A halscope application is started to + show the offset waveforms and the + z coordinate response + 5) Enable the dynamic offset waveform + with the panel 'Z enable' checkbox + 5) Use the panel to adjust: + Waveform: amplitude, frequency + Z Coordinate: max accel, max vel + +Notes: + The dynamic waveform is applied with and without + running a program. + + MDI cannot be started with non-zero external + offsets -- disable the Z offsets and toggle + the machine On/Off to zero offsets. + + Changes to the Z max accel, max vel are not + made while a program or mdi move is in progress. + diff --git a/configs/sim/axis/external_offsets/dynamic_offsets_panel.hal b/configs/sim/axis/external_offsets/dynamic_offsets_panel.hal new file mode 100644 index 00000000000..2595ac3d9dd --- /dev/null +++ b/configs/sim/axis/external_offsets/dynamic_offsets_panel.hal @@ -0,0 +1,46 @@ +loadrt siggen names=gen +loadrt scale names=scalea +loadrt conv_float_s32 names=cvt +loadrt not names=nota +loadrt or2 names=or2a + +addf gen.update servo-thread +addf scalea servo-thread +addf cvt servo-thread +addf nota servo-thread +addf or2a servo-thread + +setp gen.offset 0.0 + +# scaling ----------------------------------------------------------- +# these should be reciprocals: +# 1 unit (inch) --> 10000 counts +setp axis.z.eoffset-scale 0.0001 +setp scalea.gain 10000 + +net E:amplitude <= pyvcp.gen-amplitude => gen.amplitude +net E:frequency <= pyvcp.gen-frequency => gen.frequency + +# can also use sawtooth (others inappropriate on a reset) +net G:waveform <= gen.sine => scalea.in +net G:swaveform <= scalea.out => cvt.in +net G:counts <= cvt.out => axis.z.eoffset-counts + +net E:enable <= pyvcp.z-enable => axis.z.eoffset-enable +net E:enable => nota.in + +net E:disable <= nota.out => or2a.in1 + +net E:z-offset <= axis.z.eoffset => pyvcp.z-offset-f + +net E:limited <= motion.eoffset-limited => pyvcp.eoffset-limited + +net E:active <= motion.eoffset-active => pyvcp.eoffset-active + +net E:reset <= pyvcp.reset => or2a.in0 + +net E:ini-accel <= pyvcp.ini-accel => ini.z.max_acceleration +net E:ini-vel <= pyvcp.ini-vel => ini.z.max_velocity + +net G:reset <= or2a.out => axis.z.eoffset-clear +net G:reset => gen.reset diff --git a/configs/sim/axis/external_offsets/dynamic_offsets_panel.xml b/configs/sim/axis/external_offsets/dynamic_offsets_panel.xml new file mode 100644 index 00000000000..0d6537568f2 --- /dev/null +++ b/configs/sim/axis/external_offsets/dynamic_offsets_panel.xml @@ -0,0 +1,166 @@ + + + + + + "sunken" + 3 + + + "w" + "z-enable" + "Z enable offsets" + 0 + + + + + + + "eoffset-limited" + 15 + "red" + "#d9d9d9" + + + + + + + "eoffset-active" + 15 + "gold" + "#d9d9d9" + + + + + + + "z-offset-f" + "+8.4f" + "13" + "black" + "gold" + "bold" + + + + + + "sunken" + 3 + + + + "ini-accel" + 6 + "bold" + "8.2f" + 386.0 + 1 + 1.0 + 40 + 1 + + + + + + "ini-vel" + 6 + "bold" + "8.2f" + 0.2 + 100 + .1 + 2 + 1 + + + + + + "sunken" + 3 + + + + "gen-frequency" + 6 + "bold" + "8.2f" + 10.0 + 0.0 + 0.1 + 1.0 + 1 + + + + + + "gen-amplitude" + 6 + "bold" + "8.2f" + 0.0 + 2.0 + 0.1 + 0.2 + 1 + + + + + + diff --git a/configs/sim/axis/external_offsets/eoffset.tbl b/configs/sim/axis/external_offsets/eoffset.tbl new file mode 100644 index 00000000000..c65768947d7 --- /dev/null +++ b/configs/sim/axis/external_offsets/eoffset.tbl @@ -0,0 +1 @@ +T1 P1 D0.125 Z0.5 diff --git a/configs/sim/axis/external_offsets/eoffsets.hal b/configs/sim/axis/external_offsets/eoffsets.hal new file mode 100644 index 00000000000..0a47910ba2c --- /dev/null +++ b/configs/sim/axis/external_offsets/eoffsets.hal @@ -0,0 +1,16 @@ +#--------------------------------------------------------------- +# note: axis.L.eoffset-enables are controlled by pyvcp panel +# external offset input scale: +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.ini b/configs/sim/axis/external_offsets/eoffsets.ini new file mode 100644 index 00000000000..9550d773498 --- /dev/null +++ b/configs/sim/axis/external_offsets/eoffsets.ini @@ -0,0 +1,100 @@ +# Notes: +# 1) [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 +# 2) Immediate homing herein +# ([JOINT_*]HOME_SEQUENCE=0, other HOME_* items omitted) +# 3) uncomment [JOINT_0]HOME_SEARCH_VEL,HOME_LATCH_VEL +# to demonstrate non-zero joint.0.motor-offset + +[APPLICATIONS] +APP = sim_pin --title "MPG Simulator" \ + e:xcounts \ + e:ycounts \ + e:zcounts + +# make sure [xyz]:ecounts are zero if machine off: +APP = eoffsets_monitor.tcl + +[HAL] +HALUI = halui +HALFILE = LIB:basic_sim.tcl -no_use_hal_manualtoolchange +HALFILE = eoffsets.hal +POSTGUI_HALFILE = eoffsets_panel.hal + +[EMC] +MACHINE = External Offsets Demo +VERSION = 1.0 + +[DISPLAY] +PYVCP = eoffsets_panel.xml +DISPLAY = axis +POSITION_OFFSET = RELATIVE +POSITION_FEEDBACK = ACTUAL +MAX_LINEAR_VELOCITY = 2 +OPEN_FILE = ./eoffsets.ngc + +TKPKG = Ngcgui 1.0 +NGCGUI_FONT = Helvetica -12 normal +NGCGUI_SUBFILE = queuebuster.ngc + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[RS274NGC] +SUBROUTINE_PATH = . +USER_M_PATH = . +PARAMETER_FILE = sim.var + +[EMCIO] +TOOL_TABLE = eoffset.tbl +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 + +[AXIS_X] +OFFSET_AV_RATIO = 0.2 +MAX_VELOCITY = 2 +MAX_ACCELERATION = 20 +MIN_LIMIT = -0.1 +MAX_LIMIT = 10 + +[AXIS_Y] +OFFSET_AV_RATIO = 0.2 +MAX_VELOCITY = 2 +MAX_ACCELERATION = 20 +MIN_LIMIT = -0.1 +MAX_LIMIT = 5 + +[AXIS_Z] +OFFSET_AV_RATIO = 0.2 +MAX_VELOCITY = 2 +MAX_ACCELERATION = 20 +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.ngc b/configs/sim/axis/external_offsets/eoffsets.ngc new file mode 100644 index 00000000000..251ee10c368 --- /dev/null +++ b/configs/sim/axis/external_offsets/eoffsets.ngc @@ -0,0 +1,35 @@ + # = 9 + # = 4 +# = 0 + # = 20 + +# = [0.25 * #] +# = [0.66 * #] +# = 0 + +# = [0.50 * #] +# = 0 +# = -.5 + +# = [0.75 * #] +# = 0 +# = 0 + +# = # +# = # +# = 0 + +m111 ;clear notifications +g61 +g0x0y0z0 + +f # +g1 x # y # z # +g1 x # y # z # +g1 x # y # z # +g1 x # y # z # +g1 x # y # +g0x0y0z0 + +m111 ;clear notifications +m2 diff --git a/configs/sim/axis/external_offsets/eoffsets.txt b/configs/sim/axis/external_offsets/eoffsets.txt new file mode 100644 index 00000000000..0af5ef6d7ed --- /dev/null +++ b/configs/sim/axis/external_offsets/eoffsets.txt @@ -0,0 +1,26 @@ +eoffsets.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: + + e:xcounts (sig to => axis.x.eoffset-counts) + e:ycounts (sig to => axis.y.eoffset-counts) + e:zcounts (sig to => axis.z.eoffset-counts) + e:clearall (sig to => axis.*.eoffset-clear) + +------------------------------------------- +Note: The external offset interface is +similar to the interface for wheel jogging: + +Input hal pins: + axis.L.eoffset-enable (bit) + axis.L.eoffset-clear (bit) + axis.L.eoffset-scale (float) + axis.L.eoffset-counts (s32) + +Requested offset == counts*scale 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 00000000000..b0a72d13f64 --- /dev/null +++ b/configs/sim/axis/external_offsets/eoffsets_monitor.tcl @@ -0,0 +1,27 @@ +#!/usr/bin/tclsh +# script to ensure e:[xyz]counts 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 00000000000..c3bb8080afb --- /dev/null +++ b/configs/sim/axis/external_offsets/eoffsets_panel.hal @@ -0,0 +1,49 @@ +# existing (basic_sim.tcl) signals for display: +net J0:pos-cmd => pyvcp.motor-pos-cmd.0-f +net J1:pos-cmd => pyvcp.motor-pos-cmd.1-f +net J2:pos-cmd => pyvcp.motor-pos-cmd.2-f + +# motor-offset (display only): +net E:motor-offset.0 <= joint.0.motor-offset => pyvcp.motor-offset.0-f +net E:motor-offset.1 <= joint.1.motor-offset => pyvcp.motor-offset.1-f +net E:motor-offset.2 <= joint.2.motor-offset => pyvcp.motor-offset.2-f + +# controls for demo (from sim_pin setp, no pin source): +# note these are teleop pos-cmds (not coord motion) +net E:x-pos-cmd => axis.x.pos-cmd => pyvcp.x-pos-cmd-f +net E:y-pos-cmd => axis.y.pos-cmd => pyvcp.y-pos-cmd-f +net E:z-pos-cmd => axis.z.pos-cmd => pyvcp.z-pos-cmd-f + +# eoffset (display only): +net E:x-offset <= pyvcp.x-offset-f => axis.x.eoffset +net E:y-offset <= pyvcp.y-offset-f => axis.y.eoffset +net E:z-offset <= pyvcp.z-offset-f => axis.z.eoffset + +# offset-request (display only): +net E:x-offset-request <= axis.x.eoffset-request +net E:x-offset-request => pyvcp.x-offset-request-f + +net E:y-offset-request <= axis.y.eoffset-request +net E:y-offset-request => pyvcp.y-offset-request-f + +net E:z-offset-request <= axis.z.eoffset-request +net E:z-offset-request => pyvcp.z-offset-request-f + +# enables +net E:x-enable <= pyvcp.x-enable => axis.x.eoffset-enable +net E:y-enable <= pyvcp.y-enable => axis.y.eoffset-enable +net E:z-enable <= pyvcp.z-enable => axis.z.eoffset-enable + +# scale factors +net E:x-scale <= pyvcp.x-scale => axis.x.eoffset-scale +net E:y-scale <= pyvcp.y-scale => axis.y.eoffset-scale +net E:z-scale <= pyvcp.z-scale => axis.z.eoffset-scale + +# clears +net E:clearall <= pyvcp.clearall +net E:clearall => axis.x.eoffset-clear +net E:clearall => axis.y.eoffset-clear +net E:clearall => axis.z.eoffset-clear + +# pyvcp led: +net e:limited <= motion.eoffset-limited => pyvcp.limited 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 00000000000..1d5851f5dcf --- /dev/null +++ b/configs/sim/axis/external_offsets/eoffsets_panel.xml @@ -0,0 +1,382 @@ + + + + + + + + + "sunken" + 3 + + + + "motor-pos-cmd.0-f" + "+10.4f" + "10" + "black" + "cyan" + "bold" + + + + + + "motor-pos-cmd.1-f" + "+10.4f" + "10" + "black" + "cyan" + "bold" + + + + + + "motor-pos-cmd.2-f" + "+10.4f" + "10" + "black" + "cyan" + "bold" + + + + + + + + + + "sunken" + 3 + + + + "motor-offset.0-f" + "+10.4f" + "10" + "black" + "white" + "bold" + + + + + + "motor-offset.1-f" + "+10.4f" + "10" + "black" + "white" + "bold" + + + + + + "motor-offset.2-f" + "+10.4f" + "10" + "black" + "white" + "bold" + + + + + + + + + + "sunken" + 3 + + + + "x-pos-cmd-f" + "+10.4f" + "10" + "black" + "greenyellow" + "bold" + + + + + + "y-pos-cmd-f" + "+10.4f" + "10" + "black" + "greenyellow" + "bold" + + + + + + "z-pos-cmd-f" + "+10.4f" + "10" + "black" + "greenyellow" + "bold" + + + + + + + + "sunken" + 3 + + + + "limited" + 15 + "red" + "#d9d9d9" + + + + + + + "x-offset-f" + "+10.4f" + "10" + "black" + "gold" + "bold" + + + + + + "y-offset-f" + "+10.4f" + "10" + "black" + "gold" + "bold" + + + + + + "z-offset-f" + "+10.4f" + "10" + "black" + "gold" + "bold" + + + + + + "x-offset-request-f" + "+10.4f" + "10" + "black" + "orange" + "bold" + + + + + + "y-offset-request-f" + "+10.4f" + "10" + "black" + "orange" + "bold" + + + + + + "z-offset-request-f" + "+10.4f" + "10" + "black" + "orange" + "bold" + + + + + + "w" + "x-enable" + "" + 0 + + + + "x-scale" + 0.1 + 10.0 + 4 + 0.1 + 0.1 + "4.1f" + "bold" + 1 + + + + + + "w" + "y-enable" + "" + 0 + 0 + + + + "y-scale" + 0.1 + 10.0 + 4 + 0.1 + 0.1 + "4.1f" + "bold" + 1 + + + + + + "w" + "z-enable" + "" + 0 + 0 + + + + "z-scale" + 0.1 + 10.0 + 4 + 0.1 + 0.1 + "4.1f" + "bold" + 1 + + + + + + diff --git a/configs/sim/axis/external_offsets/hpid.hal b/configs/sim/axis/external_offsets/hpid.hal new file mode 100644 index 00000000000..7ed15d9e6c8 --- /dev/null +++ b/configs/sim/axis/external_offsets/hpid.hal @@ -0,0 +1,106 @@ +loadrt eoffset_pid names=zo +loadrt sum2 names=summer + +# Initial PID gain settings +# Notes: (simulation) +# dgain mandatory +# igain not useful +setp zo.pgain 30 +setp zo.igain 0 +setp zo.dgain 0.4 + +setp zo.k 10000 +setp zo.fnum 0 + +# order so offset computation precedes motion-* threads +addf summer servo-thread 1 +addf zo servo-thread 2 + +#----------------------------------------------------------- +# required standard connections TO zo +net E:is-on <= halui.machine.is-on +net E:is-on => zo.is-on + +net E:active <= motion.eoffset-active +net E:active => zo.active + +#----------------------------------------------------------- +# required standard connections FROM zo +net E:eoffset-enable <= zo.enable-out +net E:eoffset-enable => axis.z.eoffset-enable + +net E:clear <= zo.clear +net E:clear => axis.z.eoffset-clear + +net E:scale <= zo.kreciprocal +net E:scale => axis.z.eoffset-scale + +net E:kcounts <= zo.kcounts +net E:kcounts => axis.z.eoffset-counts + +#----------------------------------------------------------- +# zo feedback (source elsewhere, mandatory) +net E:feedback => zo.feedback + +#----------------------------------------------------------- +# zo command & request (use M68 to set analog out pin 0 +net E:request <= motion.analog-out-00 +net E:request => summer.in0 + +net E:command <= summer.out +net E:command => zo.command + +#----------------------------------------------------------- +# current velocity (corner lockout) +net E:current-vel <= motion.current-vel +net E:current-vel => zo.current-vel + +#----------------------------------------------------------- +# Spindle commands are used to start/stop torch: +# M3 --> torch on +# M5 --> torch off +net E:enable-in-a <= motion.spindle-on +net E:enable-in-a => zo.enable-in-a + +#----------------------------------------------------------- +# GCODE must issue this final enable using M64 +# after verifying arc and asserting E:arc-ok: +net E:enable-in-b <= motion.digital-out-00 +net E:enable-in-b => zo.enable-in-b + +#----------------------------------------------------------- +# signals with source elsewhere + +# E:arc-ok is read by GCODE using M66 prior to final enable: +# NOTE: M66 is a queuebuster and must be used prior +# to enabling external offsets +# E:arc-ok <= torch controller sets somehow +net E:arc-ok => motion.digital-in-00 + +# extra zo enable (a,b,c are anded together): +# default to 1, use elsewhere as required: +setp zo.enable-in-c 1 +net E:enable-in-c => zo.enable-in-c + +# provision for a perturbation to pid input command +net E:perturb => summer.in1 + +# cornerlock minimum velocity (units/sec): +net E:minimum-vel => zo.minimum-vel + +#----------------------------------------------------------- +# signals with sink elsewhere +# convenience pin, use as required (sim:hpid_panel.hal): +net E:is-off <= zo.is-off + +#----------------------------------------------------------- +# signals for sim only: +net E:z-eoffset <= axis.z.eoffset + +#----------------------------------------------------------- +# signals for halscope usage: +net E:error <= zo.dbg-error +net E:holding <= zo.dbg-holding +net E:state <= zo.dbg-state +net E:hold-request => zo.hold-request +net E:minimum-vel => zo.minimum-vel diff --git a/configs/sim/axis/external_offsets/hpid.halscope b/configs/sim/axis/external_offsets/hpid.halscope new file mode 100644 index 00000000000..cf08554d7fa --- /dev/null +++ b/configs/sim/axis/external_offsets/hpid.halscope @@ -0,0 +1,47 @@ +THREAD servo-thread +MAXCHAN 8 +HMULT 5 +HZOOM 1 +HPOS 4.760870e-01 +CHAN 1 +SIG E:enable-in-a +VSCALE 1 +VPOS 0.100000 +VOFF 0.000000e+00 +CHAN 2 +SIG E:command +VSCALE 5 +VPOS 0.500000 +VOFF 0.000000e+00 +CHAN 3 +SIG E:feedback +VSCALE 5 +VPOS 0.500000 +VOFF 0.000000e+00 +CHAN 5 +SIG E:current-vel +VSCALE 1 +VPOS 0.800000 +VOFF 0.000000e+00 +CHAN 6 +SIG E:holding +VSCALE 1 +VPOS 0.600000 +VOFF 0.000000e+00 +CHAN 7 +SIG E:enable-in-b +VSCALE 1 +VPOS 0.200000 +VOFF 0.000000e+00 +CHAN 8 +SIG E:arc-ok +VSCALE 1 +VPOS 0.300000 +VOFF 0.000000e+00 +CHAN 4 +SIG E:error +VSCALE 4 +VPOS 0.900000 +VOFF 0.000000e+00 +TMODE 0 +RMODE 0 diff --git a/configs/sim/axis/external_offsets/hpid.ini b/configs/sim/axis/external_offsets/hpid.ini new file mode 100644 index 00000000000..896473a5493 --- /dev/null +++ b/configs/sim/axis/external_offsets/hpid.ini @@ -0,0 +1,104 @@ +[APPLICATIONS] +# delay because some pins are postgui +DELAY = 3 + +APP = sim_pin --title "INI A/V" \ + ini.z.max_velocity \ + ini.z.max_acceleration + +APP = sim_pin --title "PID TUNE" \ + zo.pgain \ + zo.igain \ + zo.dgain + +APP = sim_pin --title "SIM Torch" \ + sim:torch.gain \ + sim:arcwait.width + +APP = sim_pin --title Experiments \ + zo.fnum \ + zo.kptd \ + zo.nfilt \ + E:enable-in-c + +APP = halscope -i hpid.halscope + +[HAL] +HALUI = halui +# use -no_sim_spindle (avoid lowpass usage clash) +HALFILE = LIB:basic_sim.tcl -no_sim_spindle +HALFILE = hpid.hal +HALFILE = sim_torch.hal +POSTGUI_HALFILE = hpid_panel.hal + +[EMC] +MACHINE = Offset per Angle Demo +VERSION = 1.0 + +[DISPLAY] +PYVCP = hpid_panel.xml +DISPLAY = axis +OPEN_FILE = hpid_demo.ngc +GEOMETRY = XYZ +POSITION_OFFSET = RELATIVE +POSITION_FEEDBACK = ACTUAL +MAX_LINEAR_VELOCITY = 2 +MAX_ANGULAR_VELOCITY = 60 + +TKPKG = Ngcgui 1.0 +NGCGUI_FONT = Helvetica -12 normal +NGCGUI_SUBFILE = hpid_sub.ngc + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[RS274NGC] +SUBROUTINE_PATH = . +USER_M_PATH = . +PARAMETER_FILE = sim.var + +[EMCIO] +TOOL_TABLE = eoffset.tbl +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 + +[AXIS_X] +MAX_VELOCITY = 2 +MAX_ACCELERATION = 10 + +[AXIS_Y] +MAX_VELOCITY = 2 +MAX_ACCELERATION = 10 + +[AXIS_Z] +OFFSET_AV_RATIO = 0.2 +MAX_VELOCITY = 1 +MAX_ACCELERATION = 50 +MAX_LIMIT = 0.5 +MIN_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/hpid.txt b/configs/sim/axis/external_offsets/hpid.txt new file mode 100644 index 00000000000..f0c112abc9a --- /dev/null +++ b/configs/sim/axis/external_offsets/hpid.txt @@ -0,0 +1,178 @@ +hpid.ini - height (Z axis) PID control using external offsets (torch height control) + +For info on the compoent, see the man page ($ man eoffset_pid) + +Usage: + 1) Estop OFF (F1) + 2) Machine ON (F2) + 3) HOME All (Ctrl-Home) + 4) Run (R) + +The Minimum velocity (in inches/sec) for corner lockout may be specified with a spinbox. Use a value of zero to disable. The hold button requests momentary holding of the external offset. + +The Perturb On/Off buttons control a signal generator that dynamically alters the requested torch-volts. The signal generator's waveform, frequency and amplitude may be set with pyvcp widgets. The introduced perturbations are useful for tuning the PID settings and may be used when the program is running or paused. + +Testing while paused requires setting the hal pin zo.minimal-vel to zero to prevent min-velocity 'holding' of current offset value. Pausing with perturbations is useful in simulations but may not be practical for real hardware where non-interrupted motion is important. However, perturbations may be used in hardware testing dynamic response of the regulator loop (see below). + +For testing, sim_pin entry widgets are provided for numerous hal pins and signals using ini file settings like: + +[APPLICATIONS] +APP = sim_pin ... + +These ini file settings may be disabled by adding a initial comment character (#): + +#APP = sim_pin ... + +The hpid.ini (height control by pid) uses the hal component zo (an instance of eoffset_pid.comp) to control torch height by regulating torch voltage. + +The torch controller is simulated (sim_torch.hal) by connecting the z axis external offset output (axis.z.eoffset) to a scale component (sim:tovolts) and scaling by a factor of 1000 so that a 0.1 inch z offset height corresponds to a 100 volt torch voltage. The simulated torch voltage is passed through a lowpass filter component (sim:torch) to simulate the bandwidth-limited measurement of torch voltage. The bandwidth is set by the filter gain setting pin (sim:torch.gain). (See the lowpass filter man page for the relationship of lowpass gain and filter bandwidth). The delay to achieve arc-ok is simulated by a oneshot component (sim:arcwait), the delay time is set by the pin sim:arcwait.width. + +To test, the [DISPLAY]OPEN_FILE=hpid_demo.ngc gcode file calls an ngcgui subroutine hpid_sub.ngc. An ngcgui tab is also included to set parameters and make gcode files with other settings for testing. + +The hpid_sub.ngc subroutine accepts inputs to repeat an arbitrary number (#) of simple patterns with fixed width and height and horizontal separation. Before starting a pattern, a touchoff subroutine (hpid_touchoff.ngc) is called to start the simulated torch using a spindle (M3 S1) command. Settings can be altered for important items like torch_setpoint, torch_cut_hgt, pierce_hgt, pierce_dly, etc. + +The simulation hpid_touchoff.ngc subroutine omits probing to the surface -- a real touchoff routine would likely include this step to probe the surface and establish an appropriate z axis zero reference. + +The eoffset_pid component has three enable inputs (zo.enable-in-a,b,c) that are 'anded' together internally. The sim config uses the 'a' and 'b' enables, the 'c' input could be used for other enabling requirements. The disabling of an external offset will return all external offsets to zero (respecting velocity and acceleration constraints) and is NOT a suitable method for short-term disabling of offset motion. The zo.hold-request pin can be used to sustain the current offset for short time intervals. + +Provision is made to accomodate velocity reduction at corners (corner lockout) by using pins zo.minimal-vel and zo.current-vel. The pin zo.current-vel is connected to a pin measuring current velocity with signal E:current-vel (sourced by motion.current-vel). The pin zo.minimal-vel (signal E:minimal-vel) sets a threshold. The current eoffset is sustained with no additional corrections based on torch voltage when the zo.current-vel is less than the zo.minimal-vel. + +The required sequence of gcode commands that relate to hardware control and hal connections are: + +hpid_sub.ngc: + M68 -- read torch voltage setpoint + (pin motion.analog-out-00) + (signal: E:request) + M65 -- deassert secondary enable + (signal: E:enable-in-b) + + hpid_touchoff.ngc: + M3 --- Start torch + (pin: motion.spindle-on) + (signal: E:enable-in-a) + M66 -- Wait for arc-ok + (signal: E:arc-ok) + M2 --- Stop if no arc-ok within timeout + M64 -- Assert secondary enable + (pin: motion.digital-in-00) + (signal: E:enable-in-b) + G4 -- Dwell for pierce-delay + +hpid_sub.ngc: + (run the pattern making program to conclusion) + M68 -- return torch voltage setpoint to 0 + (pin motion.analog-out-00) + (signal: E:request) + M5 --- Stop torch (spindle command) + (pin: motion.spindle-on) + (signal E:enable-in-a) + +Note that a handshake is required to enable external offsets: + +Gcode commands start the torch, wait for arc-ok, and if succesful assert the secondary enable (signal: E:enable-in-b). Since M66 is a 'queuebuster' command, it must be used PRIOR to enabling the z external offset (See the Restrictions section in the Docs chapter on External Axis Offsets). + + +The eoffset_pid component (named 'zo' in the sim config) works to null the error between its command and feedback. The feedback loop uses: + + 1) zo.command --- torch voltage setpoint controlled by gcode + 2) zo.feedback -- from sim_torch.hal using a scaled, lowpass component + +The main issues with a real hardware implementation will be: + + 1) Electrical noise & filtering. + + Noise is always problematic for high voltage circuitry and requires good design practices for grounding and shielding. It is especially important to provide an analog anti-aliasing filter prior to the analog-to-digital conversion. The sampling of the voltage typically occurs at a 1KHz rate (1mS servo thread period). The analog anti-aliasing filter must attenuate noise components above the Nyquist frequency (500 Hz) but excessive filtering will compromise stability of the feedback loop. Oversampling (using smaller servo thread periods) may allow for simpler analog anti-aliasing filters but requires a system with very low-latency. Note that modifications of the servo thread period will usually require retuning of the servo implementations for any joint using PID control. + +An excellent and readable reference: http://www.stablesimulations.com/technotes/alias.html + + 2) Tuning the feedback loop. + + The LinuxCNC motion subsystem for z axis control is *inside* the servo loop that regulates torch voltage by control of the z axis external offset. The input to zo.command is subject to the z axis velocity and acceleration constraints. The transfer function properties of the entire Linuxcnc motion control for the z axis affect the stability of the outer torch-voltage control loop. + + 3) Height regulation at low velocities. + + At XY corners, velocity reduction causes an increase in torch voltage which is corrected by servo action to decrease the torch height. If this correction is unstable (due to non-linear voltage-height relationship as speed changes for example), the torch can crash to the surface. + + To address this behavior, a hal pin 'zo.current-vel' is provided to monitor the current velocity provided by the motion module output on 'motion.current-vel' pin. Offset correction is suspended when the zo.current-vel is below the threshold set by the 'zo.minimum-vel' hal pin. + + +TUNING + +The pid gain settings are made available on sim_pin gui entry boxes: zo.pgain, zo.igain, zo.dgain. + +The ini file settings: + + [AXIS_Z]MAX_ACCELERATION + [AXIS_Z]MAX_VELOCITY + [AXIS_Z]OFFSET_AV_RATIO + +control the LinuxCNC motion subsystem response. These should be set based on known limits of the hardware being simulated. The first two items (ini.z.max_acceleration, ini.z.max_velocity) are made available in sim_pin gui entry boxes for experimentation. + +For the sim config, the torch-controller bandwidth is set by the lowpass filter gain parameter (torch-height.gain) that is also available as a sim_pin entry box. The bandwidth limit represents both the frequency response of the z (height) coordinate and the measurement of torch voltage. Note that very small bandwidths (excessive filtering or slow z response) will make tuning difficult or impossible. Similarly, if a motor (not a stepper) is used for the z axis, its tuning may affect the tuning of the outer feedback loop that controls the z axis external offset. + +To examine/modify servo tuning, start the hpid_demo.ngc gcode program and observe the E:command and E:feedback signals with halscope (automatically started, use 'Roll' mode). The sequence -- torch-on, cut pattern, torch-off -- repeats, so it is possible to observe the response for different PID gain settings and change them with the sim_pin guis. + +With the sim config, some dgain is always required to prevent oscillation, igain is not useful. + +For the sim config, it is convenient to pause the program once the torch is on and perturb the command input to see the dynamic response when settings are changed. (Note: sets E:minimal-vel to zero is required). The convenience of pausing the program is not likely feasible with real hardware since the plasma torch needs to be kept in motion. + +The reason for perturbing the zo.command is that the height pid servo is a regulator whose job is to accept a nominally constant voltage command input and regulate the torch-voltage output. Since both input and output are expected to be constant, it is not always easy to observe or tune the loop response. In the real world, disturbances (height changes like a warped or corrugated material) will require good dynamic response. Perturbing the command input is an alternate and practical method to oberve and tune the system dynamic response. + +The perturbation signal generator controlled by pyvcp panel buttons (On/Off) and associated controls for amplitude, frequency, and waveform. Waveforms available are: + + sel1 sel0 + 0 0 sine wave + 0 1 square wave + 1 0 triangle wave + 1 1 sawtooth wave + +A sine wave perturbation is convenient to observe tracking of command and feedback inputs. + +The square wave is most commonly used to investigate system overshoot, undershoot, or oscillation for step changes in the command input. + +#---------------------------------------------------------- +Hardware (non-simulation) implementations + +The halfile LIB:basic_sim.tcl is for simulation configurations and should be replaced with an appropriate halfile for real hardware. + +Use [HAL]HALFILE=hpid.hal as-is to make most of the required connections. This file defines signals that need additional connections which should be made in a separate HALFILE similar in purpose to the sim_torch.hal file. + + 1) The signal E:arc-ok must be connected to a pin indicating satisfactory arc status. + + 2) The signal E:feedback must be connected to a pin representing measured torch voltage. + + 3) The signal E:minimum-vel should be set (sets -- setsignal) to the required minimum velocity for corner lockout. + + 4) The signal E:perturb is available for connection to a perturbation signal (as with the sim configuration) + + 5) The signal E:is-off (inversion of halui.machine.is-on) is available for conection elsewhere if needed. + +#---------------------------------------------------------- +The HALFILES hpid_panel.hal is not required but may be used to enable a perturbation signal generator or as a starting point for a custom pyvcp panel. + +#---------------------------------------------------------- +The HALFILE sim_torch.hal should be replaced with a halfile that handles all hardware-specific torch controller logic and makes these connections: + + net E:arc-ok <= indicator_of_arc_status + net E:feedback <= measure_of_torch-voltage + sets E:minimum-vel value_in_units_per_second + +The ngc subroutine files hpid_touchoff.ngc and hpid_sub.ngc should be adapted as-required for hardware-specific usage. + +#---------------------------------------------------------- +Alternate PID calculation + +The defalt PID calc (zo.fnum=0) uses a simple difference for calculation of the error derivative and will exaggerate noise on the input feedback. The noise can be reduced by low pass filtering of the measurement but (see above) too much filtering will destabilize the voltage control servo loop. + +An alternative PID calc (zo.fnum=1) computes the error derivative using a backwards difference and limits the gain at high frequencies. To use this alternative PID calc: + + 1) use zo.fnum=1 + 2) instead of zo.dgain, use zo.kpt for tuning + 3) use zo.nfilt (values typically between 5 and 20) + +Example using sim_pin from the commandline: +$ sim_pin zo.fnum zo.kptd zo.nfilt & + +or in the ini file: +[APPLICATIONS] +APP = sim_pin zo.fnum zo.kptd zo.nfilt diff --git a/configs/sim/axis/external_offsets/hpid_demo.ngc b/configs/sim/axis/external_offsets/hpid_demo.ngc new file mode 100644 index 00000000000..2ee89227831 --- /dev/null +++ b/configs/sim/axis/external_offsets/hpid_demo.ngc @@ -0,0 +1,3 @@ + +o call [10][80][0.080][0.080][0][0][0][0.5][3][0.5][0.100][100][1][0][2][10][25] +m2 diff --git a/configs/sim/axis/external_offsets/hpid_panel.hal b/configs/sim/axis/external_offsets/hpid_panel.hal new file mode 100644 index 00000000000..48e9db1abb8 --- /dev/null +++ b/configs/sim/axis/external_offsets/hpid_panel.hal @@ -0,0 +1,79 @@ +#------------------------------------------------- +# pyvcp displays from signals created in hpid.hal +net E:eoffset-enable => pyvcp.eoffset-enable +net E:feedback => pyvcp.feedback-f +net E:torch-enable => pyvcp.torch-is-on +net E:active => pyvcp.eoffset-active + +#------------------------------------------------- +# panel controls to test dynamic response +# with a perturbing waveform: +loadrt flipflop names=ff1 +loadrt not names=nota +loadrt siggen names=sig +loadrt mux4 names=mux + +setp ff1.data 0 +setp sig.frequency 0.5 +setp sig.amplitude 5.0 +setp sig.offset 0.0 + +addf nota servo-thread +addf ff1 servo-thread +addf sig.update servo-thread +addf mux servo-thread + +# E:is-off <= elsewhere +net E:is-off => ff1.reset + +net P:perturb-start <= pyvcp.perturb-start +net P:perturb-start => ff1.set + +net P:perturb-stop <= pyvcp.perturb-stop +net P:perturb-stop => ff1.clk + +net P:limited <= motion.eoffset-limited +net P:limited => pyvcp.eoffset-limited + +net P:paused <= halui.program.is-paused +net P:paused => pyvcp.paused + +net P:sigenable <= ff1.out +net P:sigenable => pyvcp.perturb-is-on +net P:sigenable => nota.in + +net P:sigreset <= nota.out +net P:sigreset => sig.reset + +net P:mux-in0 <= sig.sine +net P:mux-in0 <= mux.in0 + +net P:mux-in1 <= sig.square +net P:mux-in1 <= mux.in1 + +net P:mux-in2 <= sig.triangle +net P:mux-in2 <= mux.in2 + +net P:mux-in3 <= sig.sawtooth +net P:mux-in3 <= mux.in3 + +net P:sel0 <= pyvcp.sel0 +net P:sel0 => mux.sel0 + +net P:sel1 <= pyvcp.sel1 +net P:sel1 => mux.sel1 + +net P:amplitude <= pyvcp.amplitude +net P:amplitude => sig.amplitude + +net P:frequency <= pyvcp.frequency +net P:frequency => sig.frequency + +net E:perturb <= mux.out +# E:perturb => elsewhere (hpid.hal) + +# E:hold-request => elsewhere (hpid.hal) +net E:hold-request <= pyvcp.hold-request + +# E:minimum-vel => elsewhere (hpid.hal) +net E:minimum-vel <= pyvcp.minimum-vel diff --git a/configs/sim/axis/external_offsets/hpid_panel.xml b/configs/sim/axis/external_offsets/hpid_panel.xml new file mode 100644 index 00000000000..175fc0dfa20 --- /dev/null +++ b/configs/sim/axis/external_offsets/hpid_panel.xml @@ -0,0 +1,229 @@ + + + + + "ridge" + 3 + + + "sunken" + 3 + + + + "feedback-f" + "+11.1f" + "11" + "black" + "gold" + "bold" + + + + + + + "eoffset-limited" + 15 + "red" + "#d9d9d9" + + + + + + + + "eoffset-enable" + 15 + "gold" + "#d9d9d9" + + + + + + + + "eoffset-active" + 15 + "gold" + "#d9d9d9" + + + + + + + + "paused" + 15 + "springgreen" + "#d9d9d9" + + + + + + + + "torch-is-on" + 15 + "cyan" + "#d9d9d9" + + + + + + "sunken" + 3 + + + "minimum-vel" + 0.0 + 3.0 + 4 + 0.0 + 0.1 + "3.1f" + "bold" + + + + + + "sunken" + 2 + + + + "perturb-is-on" + 15 + "red" + "#d9d9d9" + + + + + + + + + + + + "amplitude" + 0.0 + 20.0 + 4 + 5 + 1 + "3.1f" + "bold" + + + + + + + "frequency" + 0.1 + 10.0 + 4 + 0.5 + 0.1 + "3.1f" + "bold" + + + + + + + + "sel1" + "1" + 0 + + + "sel0" + "0" + 0 + + + + + + diff --git a/configs/sim/axis/external_offsets/hpid_sub.ngc b/configs/sim/axis/external_offsets/hpid_sub.ngc new file mode 100644 index 00000000000..696ad5c23fc --- /dev/null +++ b/configs/sim/axis/external_offsets/hpid_sub.ngc @@ -0,0 +1,76 @@ +(info: Test pattern for torch tuning) +;Note: spindle hal pins used to control torch +o sub + # = #1 (=10) +# = #2 (=80) + # = #3 (=0.080) + # = #4 (=0.080) + # = #5 (=0) + + # = #6 (=0) + # = #7 (=0) + # = #8 (=0.5) + # = #9 (=3) + # = #10 (=0.5) + # = #11 (=0.100) + + # = #12 (=100) + # = #13 (=1) + # = #14 (=0) + + # = #15 (= 2) + # = #16 (=10) + # = #17 (=25) + +M111 ;clear notifications + +G20 ;Units: Inches +G40 ;Cancel Cutter Comp +G90 ;Absolute Mode +G64 P# ;Continuous mode + path tolerance + + # = # + # = # + # = 0 + G0 X# Y# Z# +o repeat [#] + # = [# + 1] + # = [#] + # = [# + #] + # = [#] + # = [# + #] + M111 ;clear notifications + G0 X# Y# Z# + + M68 E0 Q# + M65 P0 ;deassert enable-in-b + + ;hpid_touchoff subroutine (typical): + ; probe to surface + ; set Z0 + ; start torch (M3) + ; wait for arc (M66) + ; fail: M2 + ; success: M64 assert enable-in-b + ; dwell for pierce delay (G4) + ; move to cut_height (G1) +o call [#][#][#][#][#][#] + F# + G1 X # Y # + G1 X # Y # + G1 X # Y # + G1 X # Y # + G1 X # Y # + M68 E0 Q0 ;set torch-volts 0 + M5 ;torch off + M65 P0 ;deassert enable-in-b + (debug, torch off) + G92.1 ;Cancel offsets + G0 Z# + (debug, finish ct=#) + G4 P# + # = [# + # + #] + # = [#] +o endrepeat + +o endsub diff --git a/configs/sim/axis/external_offsets/hpid_touchoff.ngc b/configs/sim/axis/external_offsets/hpid_touchoff.ngc new file mode 100644 index 00000000000..cf14d5b1e52 --- /dev/null +++ b/configs/sim/axis/external_offsets/hpid_touchoff.ngc @@ -0,0 +1,47 @@ +;hpid_touchoff (SIMULATION) + +;NOTE: M66 is a queuebuster and fails if eoffset active +; so check for arc-ok must be made beofre +; enabling offsets + +o sub + # = #1 + # = #2 + # = #3 + # = #4 + # = #5 + # = #6 + +(debug, hpid_touchoff BEGIN) +;--------------------------------------- +F# +;NOTE: probe and set Z0 here +;--------------------------------------- +G1 Z# +M3 S1 ;start torch & enable eoffset_pid +(debug, torch on, wait #) + +M66 P0 L1 Q# + ;M66 wait on digital input (arc-ok) + ;P0 motion.digital-in-00 + ;L1 wait low to high + ;Q5 timeout seconds + ; if timeout: #5399 == -1 +# = #5399 +o if [# LT 0] + (debug, arc FAIL, bye) + M2 +o else + M64 P0 + ;M64 set digital output ON + (debug, arc OK, continue) +o endif + +(debug, pierce_delay #) +G4 P# + +F# +G1 Z# +(debug, hpid_touchoff END) + +o endsub diff --git a/configs/sim/axis/external_offsets/jwp_z.ini b/configs/sim/axis/external_offsets/jwp_z.ini new file mode 100644 index 00000000000..a47fe3f9442 --- /dev/null +++ b/configs/sim/axis/external_offsets/jwp_z.ini @@ -0,0 +1,84 @@ +# Notes: +# 1) [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 +# 2) Immediate homing herein +# ([JOINT_*]HOME_SEQUENCE=0, other HOME_* items omitted) +# 3) uncomment [JOINT_0]HOME_SEARCH_VEL,HOME_LATCH_VEL +# to demonstrate non-zero joint.0.motor-offset + + +[HAL] +HALUI = halui +HALFILE = LIB:basic_sim.tcl +POSTGUI_HALFILE = jwp_z_panel.hal + +[EMC] +MACHINE = Jog-While-Pause Z Demo +VERSION = 1.0 + +[DISPLAY] +PYVCP = jwp_z_panel.xml +DISPLAY = axis +POSITION_OFFSET = RELATIVE +POSITION_FEEDBACK = ACTUAL +MAX_LINEAR_VELOCITY = 2 +OPEN_FILE = ./jwp_z.ngc + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[RS274NGC] +USER_M_PATH = . +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 + +[AXIS_X] +MAX_VELOCITY = 2 +MAX_ACCELERATION = 20 +MIN_LIMIT = -1 +MAX_LIMIT = 11 + +[AXIS_Y] +MAX_VELOCITY = 2 +MAX_ACCELERATION = 20 +MIN_LIMIT = -1 +MAX_LIMIT = 5 + +[AXIS_Z] +# Note: low OFFSET_AV_RATIO: +OFFSET_AV_RATIO = 0.05 +MAX_VELOCITY = 2 +MAX_ACCELERATION = 20 +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/jwp_z.ngc b/configs/sim/axis/external_offsets/jwp_z.ngc new file mode 100644 index 00000000000..89e2c1760c1 --- /dev/null +++ b/configs/sim/axis/external_offsets/jwp_z.ngc @@ -0,0 +1,35 @@ +# = 10 +# = 5 +# = 0.5 +# = 100 + +# = [0.25 * #] +# = [0.66 * #] + +# = [0.50 * #] +# = [0.33 * #] + +# = [0.75 * #] +# = [0.33 * #] + +m111 ;clear notifications +g61 +g0 x0 y0 z# +(debug, Enter offsets only when paused, s to continue) +m0 +m111 + +f # +g0 z0 +g1 x # y # +g1 x # y # +g1 x # y # +g1 x # y # + +g0 z# +(debug, Program End: Clear offsets, s to continue) +m0 +g0 x0 y0 + +m111 ;clear notifications +m2 diff --git a/configs/sim/axis/external_offsets/jwp_z.txt b/configs/sim/axis/external_offsets/jwp_z.txt new file mode 100644 index 00000000000..d7577b97b49 --- /dev/null +++ b/configs/sim/axis/external_offsets/jwp_z.txt @@ -0,0 +1,16 @@ +jwp_z.ini + +Usage: + 1) Estop OFF (F1) + 2) Machine ON (F2) + 3) HOME All (Ctrl-Home) + 4) Run program (R) + 5) initial pause: + adjust z offset (+Incr/-Decr) + 6) Resume (S) + 7) Pause (P) + adjust z offset (+Incr/-Decr) + 8) Resume (R) + 9) Final pause + return offset to zero (+Incr/-Decr) + or Clear diff --git a/configs/sim/axis/external_offsets/jwp_z_panel.hal b/configs/sim/axis/external_offsets/jwp_z_panel.hal new file mode 100644 index 00000000000..848151cd8c4 --- /dev/null +++ b/configs/sim/axis/external_offsets/jwp_z_panel.hal @@ -0,0 +1,17 @@ +loadrt updown names=zctr +addf zctr servo-thread + +net E:z-plus <= pyvcp.z-plus => zctr.countup +net E:z-minus <= pyvcp.z-minus => zctr.countdown +net E:z-counts <= zctr.count => axis.z.eoffset-counts +net E:z-scale <= pyvcp.z-scale => axis.z.eoffset-scale +net E:z-offset <= axis.z.eoffset => pyvcp.z-offset-f +net E:limited <= motion.eoffset-limited => pyvcp.limited +net E:active <= motion.eoffset-active => pyvcp.eoffset-active +net E:paused <= halui.program.is-paused => axis.z.eoffset-enable +net E:paused => pyvcp.paused + +net E:clearall <= pyvcp.clearall +net E:clearall => axis.x.eoffset-clear +net E:clearall => axis.y.eoffset-clear +net E:clearall => axis.z.eoffset-clear diff --git a/configs/sim/axis/external_offsets/jwp_z_panel.xml b/configs/sim/axis/external_offsets/jwp_z_panel.xml new file mode 100644 index 00000000000..27d4bd3cf3d --- /dev/null +++ b/configs/sim/axis/external_offsets/jwp_z_panel.xml @@ -0,0 +1,112 @@ + + + + + + "sunken" + 3 + + + + "paused" + 15 + "springgreen" + "#d9d9d9" + + + + + + + + "limited" + 15 + "red" + "#d9d9d9" + + + + + + + + "eoffset-active" + 15 + "gold" + "#d9d9d9" + + + + + + + "z-offset-f" + "+8.4f" + "13" + "black" + "gold" + "bold" + + + + + + "z-scale" + 0.0 + 1.0 + 6 + 0.01 + 0.01 + "8.2f" + "bold" + 1 + + + + + + + + diff --git a/configs/sim/axis/external_offsets/opa.hal b/configs/sim/axis/external_offsets/opa.hal new file mode 100644 index 00000000000..c963ed90689 --- /dev/null +++ b/configs/sim/axis/external_offsets/opa.hal @@ -0,0 +1,47 @@ +loadrt eoffset_per_angle names=xo +setp xo.k 10000 +setp xo.fnum 2 +addf xo servo-thread + +#----------------------------------------------------------- +# required standard connections to xo +net E:angle <= axis.c.pos-cmd +net E:angle => xo.angle + +net E:is-on <= halui.machine.is-on +net E:is-on => xo.is-on + +net E:active <= motion.eoffset-active +net E:active => xo.active + +net E:radius-ref <= motion.analog-out-00 +net E:radius-ref => xo.radius-ref + +#----------------------------------------------------------- +# required standard connections from xo +net E:eoffset-enable <= xo.enable-out +net E:eoffset-enable => axis.x.eoffset-enable + +net E:clear <= xo.clear +net E:clear => axis.x.eoffset-clear + +net E:scale <= xo.kreciprocal +net E:scale => axis.x.eoffset-scale + +net E:kcounts <= xo.kcounts +net E:kcounts => axis.x.eoffset-counts + +#----------------------------------------------------------- +# signals with sink elsewhere (sim: opa_panel.hal) +net E:x-eoffset <= axis.x.eoffset + +# convenience pin, use as required: +net E:is-off <= xo.is-off + +#----------------------------------------------------------- +# signals with source elsewhere (sim: opa_panel.hal) +net E:enable-in => xo.enable-in +net E:fnum => xo.fnum +net E:rfrac => xo.rfrac +net E:fmul => xo.fmul +net E:start-angle => xo.start-angle diff --git a/configs/sim/axis/external_offsets/opa.ini b/configs/sim/axis/external_offsets/opa.ini new file mode 100644 index 00000000000..e1e808ad282 --- /dev/null +++ b/configs/sim/axis/external_offsets/opa.ini @@ -0,0 +1,82 @@ +[APPLICATIONS] +DELAY = 3 +# uncomment to experiment: +# APP = sim_pin ini.x.max_velocity ini.x.max_acceleration + +[HAL] +HALUI = halui +HALFILE = LIB:basic_sim.tcl +HALFILE = opa.hal +POSTGUI_HALFILE = opa_panel.hal + +[EMC] +MACHINE = External Offset per Angle Demo +VERSION = 1.0 + +[DISPLAY] +PYVCP = opa_panel.xml +DISPLAY = axis +OPEN_FILE = opa_demo.ngc +GEOMETRY = CXZ +POSITION_OFFSET = RELATIVE +POSITION_FEEDBACK = ACTUAL +MAX_LINEAR_VELOCITY = 2 +MAX_ANGULAR_VELOCITY = 60 + +TKPKG = Ngcgui 1.0 +NGCGUI_FONT = Helvetica -12 normal +NGCGUI_SUBFILE = ccircle.ngc + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[RS274NGC] +SUBROUTINE_PATH = . +USER_M_PATH = . +PARAMETER_FILE = sim.var + +[EMCIO] +TOOL_TABLE = eoffset.tbl +EMCIO = io +CYCLE_TIME = 0.100 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 + +[TRAJ] +COORDINATES = XCZ +LINEAR_UNITS = inch +ANGULAR_UNITS = degree + +[KINS] +JOINTS = 3 +KINEMATICS = trivkins coordinates=XCZ + +[AXIS_X] +OFFSET_AV_RATIO = 0.5 +MAX_VELOCITY = 2 +# increase MAX_ACCELERATION to improve +# waveform tracking: +MAX_ACCELERATION = 50 + +[AXIS_Z] +MAX_VELOCITY = 2 +MAX_ACCELERATION = 10 + +[AXIS_C] +MAX_VELOCITY = 60 +MAX_ACCELERATION = 600 + +[JOINT_0] +TYPE = LINEAR +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = ANGULAR +HOME_SEQUENCE = 0 diff --git a/configs/sim/axis/external_offsets/opa.txt b/configs/sim/axis/external_offsets/opa.txt new file mode 100644 index 00000000000..209e1d14245 --- /dev/null +++ b/configs/sim/axis/external_offsets/opa.txt @@ -0,0 +1,40 @@ +opa.ini Offset per Angle + +Demonstrates external offsets using the +hal component: eoffset_per_angle. + +A simulation for an XZC machine offsets +the transverse (X) coordinate based on +the rotary coordinate (C) angle. + +($ man eoffset_per_angle for more info) + +Usage: + 1) Estop OFF (F1) + 2) Machine ON (F2) + 3) HOME All (Ctrl-Home) + 4) Run (R) + 5) Enable/Disable offsets using pyvcp + panel On/Off buttons + 6) Select parameters: + Astart ---- starting angle + fmul ------ frequency multipler + rfrac ----- amplitude (radius fraction) + func ------ function + polygon + sine wave + square wave + +Notes: + +1) The effective radius used for computing + external offsets is set using the + motion.analog-out-00 hal pin and + controlled by the sample program or + by MDI ( M68 E0 Q some_radius_value ) + +2) Offset fidelity is dependent upon inifile + settings for [AXIS_X] items: + MAX_VELOCITY + MAX_ACCELERATION + OFFSET_AV_RATIO diff --git a/configs/sim/axis/external_offsets/opa_demo.ngc b/configs/sim/axis/external_offsets/opa_demo.ngc new file mode 100644 index 00000000000..0618a10d8c2 --- /dev/null +++ b/configs/sim/axis/external_offsets/opa_demo.ngc @@ -0,0 +1,13 @@ + # = 1 + # = 2000 + # = 100 + +g92 C 0 +# = #5216 +# = [# MOD 360] +g92.1 +g10l20p1 C # + +g0 x0 z0 +o call [#][#][#] +m2 diff --git a/configs/sim/axis/external_offsets/opa_panel.hal b/configs/sim/axis/external_offsets/opa_panel.hal new file mode 100644 index 00000000000..a61c5954e2c --- /dev/null +++ b/configs/sim/axis/external_offsets/opa_panel.hal @@ -0,0 +1,31 @@ +loadrt flipflop names=ff1 +addf ff1 servo-thread +setp ff1.data 0 + +net P:wave-start <= pyvcp.wave-start +net P:wave-start => ff1.set + +net P:wave-stop <= pyvcp.wave-stop +net P:wave-stop => ff1.clk + +net P:limited <= motion.eoffset-limited +net P:limited => pyvcp.eoffset-limited + +net P:paused <= halui.program.is-paused +net P:paused => pyvcp.paused + +#---------------------------------------------- +# signals created in opa.hal +net E:enable-in <= ff1.out +net E:enable-in => pyvcp.waveon + +net E:fnum <= pyvcp.fnum-i +net E:rfrac <= pyvcp.rfrac +net E:fmul <= pyvcp.fmul +net E:start-angle <= pyvcp.start-angle + +net E:is-off => ff1.reset +net E:eoffset-enable => pyvcp.eoffset-enable +net E:x-eoffset => pyvcp.x-offset-f +net E:radius-ref => pyvcp.radius-ref +net E:active => pyvcp.eoffset-active diff --git a/configs/sim/axis/external_offsets/opa_panel.xml b/configs/sim/axis/external_offsets/opa_panel.xml new file mode 100644 index 00000000000..65ed3320501 --- /dev/null +++ b/configs/sim/axis/external_offsets/opa_panel.xml @@ -0,0 +1,231 @@ + + + + "ridge" + 5 + + + + + "sunken" + 3 + + + + + "x-offset-f" + "+10.4f" + "10" + "black" + "gold" + "bold" + + + + + + + "radius-ref" + "+10.4f" + "10" + "black" + "skyblue" + "bold" + + + + + + + "eoffset-limited" + 15 + "red" + "#d9d9d9" + + + + + + + + "eoffset-enable" + 15 + "gold" + "#d9d9d9" + + + + + + + + "eoffset-active" + 15 + "gold" + "#d9d9d9" + + + + + + + + "paused" + 15 + "springgreen" + "#d9d9d9" + + + + + + + + "waveon" + 15 + "cyan" + "#d9d9d9" + + + + + + + + + + + + "start-angle" + -90.0 + 90.0 + 4 + 0 + 5 + "3.1f" + "bold" + 1 + + + + + + + "fmul" + 1.0 + 20.0 + 4 + 6 + 1 + "3.1f" + "bold" + + + + + + + "rfrac" + -0.9 + 0.9 + 4 + 0.0 + .1 + "3.1f" + "bold" + 1 + + + + + + + + "monospace" + "10" + "fnum" + 1 + VERTICAL + 0 + 0 + 2 + 1 + + + + + + diff --git a/configs/sim/axis/external_offsets/queuebuster.ngc b/configs/sim/axis/external_offsets/queuebuster.ngc new file mode 100644 index 00000000000..95cb6272b75 --- /dev/null +++ b/configs/sim/axis/external_offsets/queuebuster.ngc @@ -0,0 +1,45 @@ +o sub + # = #1 (=9) + # = #2 (=3) + # = #3 (=100) + # = #4 (=100) + # = #5 (=0) + # = #6 (=0) + # = #7 (=0) + # = #8 (=1) + + g20 g61 g90 ;inches, exactpath,absolute + g0 x0y0z0 + f# + o repeat [#] + m111 + g1 x# y0 + + o if [# GT 0] + (debug, m6 queuebuster follows) + t1m6 ;QB toolchange + o endif + + o if [# GT 0] + (debug, m66 queuebuster follows) + m66p0l0 ;QB wait on input l0=imediate + o endif + + o if [# GT 0] + (debug, g38.3 queuebuster follows) + g38.3z-.2 ; QB probe + g1 z0 + o endif + + g1 y# + g1 x0 + g1 x0 y0 + o if [# GT 0] + (debug, Paused, s to continue) + m0 + o endif + o endrepeat +o endsub + +; bad: t0m6 g38.3 +; ok: m66 diff --git a/configs/sim/axis/external_offsets/sim_torch.hal b/configs/sim/axis/external_offsets/sim_torch.hal new file mode 100644 index 00000000000..48f71317dc8 --- /dev/null +++ b/configs/sim/axis/external_offsets/sim_torch.hal @@ -0,0 +1,64 @@ +# Simulate torch voltage measurement with a +# a single-pole lowpass filter + +loadrt scale names=sim:tovolts +loadrt lowpass names=sim:torch +loadrt oneshot names=sim:arcwait + +#--------------------------------------------------- +# scale: 0.1 inch --> 100 volts: +setp sim:tovolts.gain 1000 + +# simulate voltage at arc-ok: +setp sim:tovolts.offset 60 + +# sim:torch.gain (hal param) sets bandwidth +# 1000Hz --> 0.9981 +# 100Hz --> 0.4663 +# 50Hz --> 0.2696 +# 20Hz --> 0.1181 +# 10Hz --> 0.0609 +# 1Hz --> 0.0063 +setp sim:torch.gain 0.2696 + +# simulate time (in seconds) for valid arc: +# for success: 0 < sim:arcwait.width < arc_timeout +setp sim:arcwait.width 1 + +#--------------------------------------------------- +addf sim:tovolts servo-thread +addf sim:torch servo-thread +addf sim:arcwait servo-thread +#--------------------------------------------------- + +# simulate torch voltage from scaled z-eoffset: +# E:z-eoffset <= elsewhere +net E:z-eoffset => sim:tovolts.in + +net S:zscaled <= sim:tovolts.out +net S:zscaled => sim:torch.in + +# trigger oneshot that represents time to make arc +# when the torch start command is issued from gcode: +# E:enable-in-a <= elsewhere +net E:enable-in-a => sim:arcwait.in + +#--------------------------------------------------- +# For real hardware (not sim) replace all above +# with appropriate hal logic for torch-controller. +# +# Use appropriate (non-sim) settings for the +# following defined signals (E:*) + +# arc-ok (read by gcode) +net E:arc-ok <= sim:arcwait.out-not +# E:arc-ok => elsewhere + +net E:feedback <= sim:torch.out +# E:feedback => elsewhere + +# minimum vel for corner lockout (units/sec) +# (use value of zero to ignore) +sets E:minimum-vel 0.0 +# E:minimum-vel <= elsewhere +# E:minimum-vel => elsewhere diff --git a/docs/man/man9/motion.9 b/docs/man/man9/motion.9 index e5b1cba9920..cda9f519d23 100644 --- a/docs/man/man9/motion.9 +++ b/docs/man/man9/motion.9 @@ -46,7 +46,8 @@ When FALSE (the default), the jogwheel operates in position mode. The axis will .TP \fBaxis.\fIL\fB.pos-cmd\fR OUT FLOAT -The axis commanded position. There may be several offsets between the axis and motor coordinates: backlash compensation, screw error compensation, and home offsets. +The axis commanded position. There may be several offsets between the axis and motor coordinates: backlash compensation, screw error compensation, and home offsets. External offsets are +reported separately (axis.\fBL\fR.eoffset). .TP \fBaxis.\fIL\fB.kb-jog-active\fR OUT BIT @@ -70,6 +71,38 @@ 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-clear\fR IN BIT +Clear external offset request + +.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. + +.TP +\fBaxis.\fIL\fB.eoffset-request\fR OUT FLOAT +Debug pin for requested external offset. + .SH JOINT PINS (\fBN\fR is the joint number (\fB0\fR ... \fBnum_joints-1\fR)) @@ -141,6 +174,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 +363,15 @@ 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-active\fR OUT BIT +Indicates external offsets are active (non-zero) + +.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 726ab39d3c3..6ac74e2233a 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 7ce18045f0d..e2dd2571d92 100644 --- a/docs/src/Submakefile +++ b/docs/src/Submakefile @@ -131,6 +131,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 9cab518113c..b1b5aa0ab44 100644 --- a/docs/src/config/ini-config.txt +++ b/docs/src/config/ini-config.txt @@ -985,6 +985,16 @@ The 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..eoffset-enable' + 'axis..eoffset-counts' + 'axis..eoffset-scale' + +See the chapter: <> for +usage information. + [[sec:joint-section]](((INI File, JOINT Section))) === [JOINT_] Section diff --git a/docs/src/index.tmpl b/docs/src/index.tmpl index c9435ad13fc..41c66c701cd 100644 --- a/docs/src/index.tmpl +++ b/docs/src/index.tmpl @@ -268,6 +268,7 @@ into LinuxCNC.
  • Remap: Extending LinuxCNC
  • Moveoff Component
  • Stand Alone Interperter
    • +
  • External Offsets
    • diff --git a/docs/src/motion/external-offsets.txt b/docs/src/motion/external-offsets.txt new file mode 100644 index 00000000000..2ae2c8d76c2 --- /dev/null +++ b/docs/src/motion/external-offsets.txt @@ -0,0 +1,373 @@ +[[cha:external-offsets]] + += External Axis Offsets + +External axis offsets are supported during teleop (world) jogs +and coordinated (gcode) motion. External axis offsets are +enabled on a per-axis basis by ini file settings and controlled +dynamically by hal input pins. The hal interface is similar to +that used for wheel jogging. This type of interface is +typically implemented with a manual-pulse-generator (mpg) +connected to an encoder hal component that counts pulses. + +== 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 conventional (planning) max + velocity and acceleration to preserve [AXIS_L] constraints: + + planning max velocity = (1-r) * MAX_VELOCITY + external offset velocity = ( r) * MAX_VELOCITY + + planning max acceleration = (1-r) * MAX_ACCELERATIOIN + external offset acceleration = ( r) * MAX_ACCELERATION + +== Hal Pins + +=== Per-Axis Motion 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-clear* Input(bit): clear requested offset +. *axis.L.eoffset* Output(float): current external offset +. *axis.L.eoffset-request* Output(float): requested external offset + +=== Other Motion Hal Pins + +. *motion.eoffset-active* Output(bit): non-zero external offsets applied +. *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. + +=== Offset Computation + +At each servo period, the 'axis.L.eoffset-counts' pin is compared to +its value in the prior period. The increase or decrease (positive +or negative delta) of the 'axis.L.eoffset-counts' pin is multiplied +by the current 'axis.L.eoffset-scale' pin value. This product is +accumulated in an internal register and exported to the +'axis.L.eoffset-request' hal pin. The accumulation register is reset +to zero at each machine-on. + +The requested offset value is used to plan the movement for the +offset that is applied to the 'L' coordinate and represented +by the 'axis.L.eoffset' hal pin. The planned motion respects the +allocated velocity and acceleration constraints and may be limited +if the net motion (offset plus teleop jogging or coordinated motion) +reaches a soft limit for the 'L' coordinate. + +For many applications, the 'axis.L.eoffset-scale' pin is constant +making the requested offset equivalent to the product of the +'axis.L.eoffset-counts' and the (constant) 'axis.L.eoffset-scale' pin +values. + +=== 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. + +=== Soft Limits + +External axis offset movements are independently planned with +velocity and acceleration settings specified by the +'[AXIS_L]OFFSET_AV_RATIO'. The offsetting motion is not +coordinated with teleop jogs nor with coordinated (gcode) +motion. During teleop jogging and coordinated (gcode) motion, +axis soft limits ('[AXIS_L]MIN_LIMIT,MAX_LIMIT') restrict +movement of the axis. + +When external offsets are applied and motion reaches a soft limit +(by external offset increases or teleop jogging or coordinated +motion), the hal pin 'motion.eoffset-limited' is asserted and the +axis value is held nominally to the soft limit. This hal pin can +be used by associated hal logic to truncate additional eoffset +counts or to stop the machine (connect to 'halui.machine.off' for +instance). If the axis is moved within the soft limit, the +'motion.eoffset-limited' pin is reset. + +When operating at a soft limit during coordinated motion that +continues to change the planned axis value, the hal output pin +'axis.L.eoffset' will indicate the current offset -- the +distance needed to reach the limit instead of the computed +offset request. This indicated value will change as the +planned axis value changes. + +The hal pin 'axis.L.eoffset-request' indicates the current +requested offset that is the product of the internal counts +register and the eoffset-scale. In general, the 'axis.L.eoffset' +pin value lags the 'axis.L.eoffset-request' value since the +external offset is subject to an acceleration limit. +When operating at a soft limit, additional updates to the +'axis.L.eoffset-counts' will continue to affect the requested +external offset as reflected in the 'axis.L.eoffset-request' hal +pin. + +When teleop jogging with external offsets enabled *and* +non-zero values applied, encountering a soft limit will stop +motion in the offending axis *without a deacceleration interval*. +Similarly, during coordinated motion with external offsets +enabled, reaching a soft limit will stop motion with no +deacceleration phase. For this case, it does not matter if the +offsets are zero. + +When motion is stopped with no deacceleration phase, system +*acceleration limits may be violated* and lead to: 1) a following +error (and/or a thump) for a servo motor system, 2) a loss of +steps for a stepper motor system. In general, it is recommeneded +that external offsets are applied in a manner to avoid +approaching soft limits. + +=== Notes + +External offsets apply to axis coordinate letters (xyzabcuvw). +All joints must be homed before external axis offsets are +honored. + +If an 'axis.L.eoffset-enable' hal pin is reset when its offset is +non-zero, the offset is maintained. The offset may be cleared by: + +. a 'Machine-off/Machine on' toggle +. reactivating the enable pin and incrementing/decrementing the 'axis.L.eoffset-counts' +hal pin to return the offset to zero. +. pulsing the 'axis.L.eoffset-clear' hal pin + +External-offsets are intended for use with 'small' offsets that +are applied within the soft-limit bounds. + +Soft limits are respected for both teleop jogging and coordinated +motion when external offsets are applied. However, when a soft +limit is reached during coordinated motion, reducing the +offending external offset *may not move away* from the soft limit +*if planned motion continues in the same direction*. This +circumstance can occur since the rate of correcting offset +removal (as set by '[AXIS_L]OFFSET_AV_RATIO') may be less than the +opposing planned rate of motion. In such cases, *pausing* (or +stopping) the planned, coordinated motion will allow movement +away from the soft limit when correcting changes are made in the +offending external offset. + +=== Restrictions + +An mdi move may not be started if non-zero external offsets are applied. + +If a non-zero external offset exists when the interpreter synchronizes +its view of the machine's position, motion is terminated. These +synchronizations typically occur when a 'queue buster' gcode is +encountered. 'Queue busters' include: + +. tool changes (M6) +. reading hal pins (M66) +. probe moves (G38.x) + +To avoid this type of termination, all external offsets should be +reduced to zero before execution of a 'queue busting' command. +For operator controlled external offsets, a pause (M0) and +message (debug,Reset External Offsets) may be helpful. + +=== Warning + +The use of external offsets can alter machine motion in a +significant manner. The control of external offsets with hal +components and connections and any associated user interfaces +should be carefully designed and tested before deployment. + +== Related Hal Components + +=== eoffset_per_angle.comp + +Component to compute an external offset from a function based +on a measured angle (rotary coordinate or spindle). See the +man page for details (*$ man eoffset_per_angle*). + +=== eoffset_pid.comp + +Component to manage an external offset using a PID servo loop. +Candidate applications include a plasma cutter torch-height +regulation based on measured torch voltage. Seethe man page for +details (*$ man eoffset_pid*). + +== Testing + +The external axis offset capability is enabled by adding +an '[AXIS_L]' setting for each candidate axis. For example: + +---- +[AXIS_Z] +OFFSET_AV_RATIO = 0.2 +---- + +For testing, it is convenient to simulate a jog wheel interface using the +*sim_pin* gui. For example, in a terminal: + +---- +$ sim_pin axis.z.eoffset-enable axis.z.eoffset-scale axis.z.eoffset-counts +---- + +The use of external offsets is aided by displaying information +related to the current offsets: the current eoffset value and the +requested eoffset value, the axis pos-cmd, and (for an identity +kinematics machine) the corresponding joint motor pos-cmd and +motor-offset. The provided sim configuration (see below) +demonstrates an example pyvcp panel for the axis gui. + +In the absence of a custom display, *halshow* can be started as +an auxiliary application with a custom watch list. + +Example ini file settings to simulate the hal pin +eoffset connections and display eoffset information for the +z axis (for identity kinematics with z==joint2): + +---- +[APPLICATIONS] +APP = sim_pin \ + axis.z.eoffset-enable \ + axis.z.eoffset-scale \ + axis.z.eoffset-counts \ + axis.z.eoffset-clear + +APP = halshow --fformat "%0.5f" ./z.halshow +---- + +Where the file z.halshow (in the configuration directory) is: + +---- +pin+joint.2.motor-pos-cmd +pin+joint.2.motor-offset +pin+axis.z.pos-cmd +pin+axis.z.eoffset +pin+axis.z.eoffset-request +pin+motion.eoffset-limited +---- + +== Examples + +Provided simulation configurations demonstrate the use of +external offsets in order to provide a starting point for user +customization for real hardware + +The sim configurations utilize the ini setting '[HAL]HALFILE = +LIB:basic_sim.tcl' to configure all routine hal connections for +the axes specified in the ini file '[TRAJ]COORDINATES=' setting. +The hal logic needed to demonstrate external offset functionality +and the gui hal pin connections for a pyvcp panel are made in +separate hal files. A non-simulation configuration should +replace the 'LIB:basic_sim.tcl' item HALFILEs appropriate to the +machine. The provided pyvcp files (.hal and .xml) could be a +starting point for application-specific gui interfaces. + +=== eoffsets_demo.ini + +The sim config 'sim/configs/axis/external_offsets/eoffsets_demo.ini' +demonstrates a cartesian XYZ machine with controls to enable external +offsets on any axis. + +Displays are provided to show all important position and offset +values. + +A sim_pin gui provides controls for the axis offset pins: eoffset-scale +& eoffset-counts (via signal e:counts), eoffset-clear +(via signal e:clearall) + +A script (eoffsets_monitor.tcl) is used to set 'axis.L.counts' pins to +zero at Machine-off + +=== jwp_z.ini + +The sim config 'sim/configs/axis/external_offsets/jwp_z.ini' +demonstrates a jog-while-pause capability for a single (Z) coordinate: + +Panel LEDs are provided to show important status items. + +Controls are provided to set the eoffset scale factor and to +increment/decrement/clear the eoffset counts. + +=== dynamic_offsets.ini + +This sim config 'sim/configs/axis/external_offsets/dynamic_offsets.ini' +demonstrates dynamically applied offsets by connecting a sinusoidal waveform +to the z coordinate external offset inputs. + +Panel LEDs are provided to show important status items. + +Controls are provided to alter ini file settings for the Z axis +max velocity and max acceleration. + +Controls are provided to set the waveform generator parameters + +A halscope app is started to show the applied waveform, the +offset response, and the motor cmd response. + +Note: changes to the z coordinate max-acceleration and max-velocity +are not acknowledged while a program is running. + +=== opa.ini (eoffset_per_angle) + +The opa.ini configuration uses the hal component eoffset_per_angle +(*$ man eoffset_per_angle*) to demonstrate an XZC machine with functional +offsets computed from the C coordinate (angle) and applied to +the transvers (X) coordinate. Offset computations are based on +a specified reference radius typically set by a program (or MDI) +M68 command to control a *motion.analog-out-NN* pin. + +Panel LEDs are provided to show important status items. + +Functions are provided for polygons (nsides >= 3), sine waves +and square waves. The functions can be multiplied in frequency +using the fmul pin and modified in amplitude using the +rfrac pin (fraction of reference radius). +and amplitude . + +Controls are provided to start/stop offset waveforms and to +set the function type and its parameters. + +=== hpid.ini (torch height pid demo)) + +The hpid.ini configuration uses the hal component eoffset_pid +(*$ man eoffset_pid*) to demonstrate PID control of the +Z axis for a plasma cutter configured as an XYZ machine. + +Panel LEDs are provided to show important status items. + +Controls are provided to set the minimum velocity for +corner lockout and for temporary holding the current external +offset. + +A signal generator is provided to add a wave form +with variable frequency and amplitude to the requested +torch-voltage. This perturbation is an aid for examining +and tuning system response. + +sim_pin guis provides entry boxes for setting hal pins: +. Ini file settings for Z axis acceleration and velocity limits +. PID tuning settings +. Simulated torch controller settings +. Experimental settings + +A halscope app is started to show important signals for +demonstration and tuning of the PID feedback loop. diff --git a/src/emc/ini/iniaxis.cc b/src/emc/ini/iniaxis.cc index 2246ada8533..2f09025c55f 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 c870b517aba..4a53fa0f63b 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 2e8829fdab5..e8f2c52203a 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 a34e9978720..d2843371414 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -68,6 +68,18 @@ #include "tp_debug.h" #define ABS(x) (((x) < 0) ? -(x) : (x)) +/* +* External offsets behavior for teleop jogging with non-zero eoffset: +* Default behavior will not allow teleop jogs to reach soft limits +* (in direction opposite to applied offset) if external offsets are +* applied. +* +* ALT_EOFFSET_BEHAVIOR: +* Alternate behavior allows teleop jogging to soft limits +* when non-zero external offsets are applied +*/ +#undef ALT_EOFFSET_BEHAVIOR +#define ALT_EOFFSET_BEHAVIOR // Mark strings for translation, but defer translation to userspace #define _(s) (s) @@ -195,6 +207,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"), @@ -578,6 +596,13 @@ void emcmotCommandHandler(void *arg, long period) transition */ /* set the emcmotDebug->coordinating flag to defer transition to controller cycle */ + +#if 0 // disallow in task,rs274ngc not here + if (emcmotStatus->external_offsets_applied) { + reportError("Cannot begin COORD motion with external offsets"); + break; + } +#endif rtapi_print_msg(RTAPI_MSG_DBG, "COORD"); emcmotDebug->coordinating = 1; emcmotDebug->teleoperating = 0; @@ -799,11 +824,37 @@ void emcmotCommandHandler(void *arg, long period) } else { // TELEOP JOG_CONT if (GET_MOTION_ERROR_FLAG()) { break; } - if (emcmotCommand->vel > 0.0) { - axis->teleop_tp.pos_cmd = axis->max_pos_limit; - } else { - axis->teleop_tp.pos_cmd = axis->min_pos_limit; - } +#ifndef ALT_EOFFSET_BEHAVIOR + if (emcmotCommand->vel > 0.0) { + axis->teleop_tp.pos_cmd = axis->max_pos_limit; + } else { + axis->teleop_tp.pos_cmd = axis->min_pos_limit; + } +#else +{ axis_hal_t *axis_data = &(emcmot_hal_data->axis[axis_num]); + if ( axis->ext_offset_tp.enable + && (fabs(*(axis_data->external_offset)) > EOFFSET_EPSILON)) { + /* here: set pos_cmd to a big number so that with combined + * teleop jog plus external offsets the soft limits + * can always be reached + * a fixed epsilon is used here for convenience + * it is not the same as the epsilon used as a stopping + * criterion in control.c + */ + if (emcmotCommand->vel > 0.0) { + axis->teleop_tp.pos_cmd = 1e12; + } else { + axis->teleop_tp.pos_cmd = -1e12; // 1T halscope limit + } + } else { + if (emcmotCommand->vel > 0.0) { + axis->teleop_tp.pos_cmd = axis->max_pos_limit; + } else { + axis->teleop_tp.pos_cmd = axis->min_pos_limit; + } + } +} +#endif axis->teleop_tp.max_vel = fabs(emcmotCommand->vel); axis->teleop_tp.max_acc = axis->acc_limit; axis->kb_ajog_active = 1; @@ -890,13 +941,25 @@ void emcmotCommandHandler(void *arg, long period) } else { tmp1 = axis->teleop_tp.pos_cmd - emcmotCommand->offset; } - /* don't jog past limits */ - if (tmp1 > axis->max_pos_limit) { - break; - } - if (tmp1 < axis->min_pos_limit) { - break; - } +#ifndef ALT_EOFFSET_BEHAVIOR + /* don't jog past limits */ + if (tmp1 > axis->max_pos_limit) { break; } + if (tmp1 < axis->min_pos_limit) { break; } +#else +{ axis_hal_t *axis_data = &(emcmot_hal_data->axis[axis_num]); + // a fixed epsilon is used here for convenience + // it is not the same as the epsilon used as a stopping + // criterion in control.c + if ( axis->ext_offset_tp.enable + && (fabs(*(axis_data->external_offset)) > EOFFSET_EPSILON)) { + // external_offsets: soft limit enforcement is in control.c + } else { + if (tmp1 > axis->max_pos_limit) { break; } + if (tmp1 < axis->min_pos_limit) { break; } + } +} +#endif + axis->teleop_tp.pos_cmd = tmp1; axis->teleop_tp.max_vel = fabs(emcmotCommand->vel); axis->teleop_tp.max_acc = axis->acc_limit; @@ -1009,6 +1072,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); @@ -1763,6 +1827,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: @@ -1775,6 +1840,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 f83fde38fa7..ba1c3a29f0b 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -28,7 +28,9 @@ // 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; +static double ext_offset_epsilon; /* kinematics flags */ KINEMATICS_FORWARD_FLAGS fflags = 0; KINEMATICS_INVERSE_FLAGS iflags = 0; @@ -51,6 +53,43 @@ static unsigned long last_period = 0; /* servo cycle time */ static double servo_period; +extern struct emcmot_status_t *emcmotStatus; + +// *pcmd_p[0] is shorthand for emcmotStatus->carte_pos_cmd.tran.x +// *pcmd_p[1] is shorthand for emcmotStatus->carte_pos_cmd.tran.y +// etc. +static double *pcmd_p[EMCMOT_MAX_AXIS]; + +#define EDEBUG +#undef EDEBUG +#ifdef EDEBUG +#define dprint(format, ...) rtapi_print_msg(RTAPI_MSG_INFO,format, ##__VA_ARGS__) +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"; + rtapi_set_msg_level(RTAPI_MSG_INFO); + 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 +204,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 * ************************************************************************/ @@ -181,6 +228,20 @@ static void update_status(void); */ void emcmotController(void *arg, long period) { + static int do_once = 1; + if (do_once) { + pcmd_p[0] = &(emcmotStatus->carte_pos_cmd.tran.x); + pcmd_p[1] = &(emcmotStatus->carte_pos_cmd.tran.y); + pcmd_p[2] = &(emcmotStatus->carte_pos_cmd.tran.z); + pcmd_p[3] = &(emcmotStatus->carte_pos_cmd.a); + pcmd_p[4] = &(emcmotStatus->carte_pos_cmd.b); + pcmd_p[5] = &(emcmotStatus->carte_pos_cmd.c); + pcmd_p[6] = &(emcmotStatus->carte_pos_cmd.u); + pcmd_p[7] = &(emcmotStatus->carte_pos_cmd.v); + pcmd_p[8] = &(emcmotStatus->carte_pos_cmd.w); + do_once = 0; + } + static long long int last = 0; long long int now = rtapi_get_clocks(); @@ -189,9 +250,11 @@ void emcmotController(void *arg, long period) #ifdef HAVE_CPU_KHZ *(emcmot_hal_data->last_period_ns) = this_run * 1e6 / cpu_khz; #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 +270,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 +284,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 +297,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 +739,9 @@ static void set_operating_mode(void) /* check for disabling */ if (!emcmotDebug->enabling && GET_MOTION_ENABLE_FLAG()) { +#ifdef EDEBUG + dbg_show("dsbl");dbg_disable_ct=dbg_ct; +#endif /* 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 +777,18 @@ 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); +#ifdef EDEBUG + dbg_show("enbl");dbg_enable_ct=dbg_ct; +#endif + 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 +800,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 +827,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 +852,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 +906,7 @@ static void set_operating_mode(void) } else { emcmotStatus->motion_state = EMCMOT_MOTION_FREE; } -} +} //set_operating_mode static void handle_jjogwheels(void) { @@ -1024,7 +1087,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 +1105,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]; + int violated_teleop_limit = 0; /* copy joint position feedback to local array */ for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) { @@ -1171,14 +1235,22 @@ static void get_pos_cmds(long period) axis = &axes[axis_num]; axis->teleop_tp.enable = 0; axis->teleop_tp.curr_vel = 0.0; - } + } // for(axis_num) + /* check joint 0 to see if the interpolators are empty */ while (cubicNeedNextPoint(&(joints[0].cubic))) { /* they're empty, pull next point(s) off Cartesian planner */ /* run coordinated trajectory planning cycle */ + tpRunCycle(&emcmotDebug->coord_tp, period); - /* gt new commanded traj pos */ - tpGetPos(&emcmotDebug->coord_tp, &emcmotStatus->carte_pos_cmd); + /* get new commanded traj pos */ + tpGetPos(&emcmotDebug->coord_tp, &emcmotStatus->carte_pos_cmd); + + if ( update_coord_with_bound() ) { + ext_offset_coord_limit = 1; + } else { + ext_offset_coord_limit = 0; + } /* OUTPUT KINEMATICS - convert to joints in local array */ result = kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions, @@ -1189,7 +1261,8 @@ static void get_pos_cmds(long period) for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) { if(!isfinite(positions[joint_num])) { - reportError(_("kinematicsInverse gave non-finite joint location on joint %d"), joint_num); + reportError(_("kinematicsInverse gave non-finite joint location on joint %d"), + joint_num); SET_MOTION_ERROR_FLAG(1); SET_MOTION_ENABLE_FLAG(0); emcmotDebug->enabling = 0; @@ -1214,7 +1287,7 @@ static void get_pos_cmds(long period) } /* END OF OUTPUT KINS */ - } + } // while /* there is data in the interpolators */ /* run interpolation */ for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) { @@ -1237,40 +1310,50 @@ static void get_pos_cmds(long period) for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { axis = &axes[axis_num]; // teleop_tp.max_vel is always positive - if(axis->teleop_tp.max_vel > axis->vel_limit) + if(axis->teleop_tp.max_vel > axis->vel_limit) { axis->teleop_tp.max_vel = axis->vel_limit; - simple_tp_update(&(axis->teleop_tp), servo_period); - axis->teleop_vel_cmd = axis->teleop_tp.curr_vel; - axis->pos_cmd = axis->teleop_tp.curr_pos; + } + if (update_teleop_with_check(axis_num,&(axis->teleop_tp) )) { + violated_teleop_limit = 1; + ext_offset_teleop_limit = 1; + } else { + axis->teleop_vel_cmd = axis->teleop_tp.curr_vel; + axis->pos_cmd = axis->teleop_tp.curr_pos; + } if(!axis->teleop_tp.active) { axis->kb_ajog_active = 0; axis->wheel_ajog_active = 0; } + + if (axis->ext_offset_tp.enable) { + if (update_teleop_with_check(axis_num,&(axis->ext_offset_tp)) ) { + violated_teleop_limit = 1; + ext_offset_teleop_limit = 1; + } + } + } + if (!violated_teleop_limit) { + ext_offset_teleop_limit = 0; + ext_offset_coord_limit = 0; //in case was set in prior coord motion } - emcmotStatus->carte_pos_cmd.tran.x = (&axes[0])->teleop_tp.curr_pos; - emcmotStatus->carte_pos_cmd.tran.y = (&axes[1])->teleop_tp.curr_pos; - emcmotStatus->carte_pos_cmd.tran.z = (&axes[2])->teleop_tp.curr_pos; - emcmotStatus->carte_pos_cmd.a = (&axes[3])->teleop_tp.curr_pos; - emcmotStatus->carte_pos_cmd.b = (&axes[4])->teleop_tp.curr_pos; - emcmotStatus->carte_pos_cmd.c = (&axes[5])->teleop_tp.curr_pos; - emcmotStatus->carte_pos_cmd.u = (&axes[6])->teleop_tp.curr_pos; - emcmotStatus->carte_pos_cmd.v = (&axes[7])->teleop_tp.curr_pos; - emcmotStatus->carte_pos_cmd.w = (&axes[8])->teleop_tp.curr_pos; + sync_carte_pos_to_teleop_tp(+1); // teleop /* the next position then gets run through the inverse kins, to compute the next positions of the joints */ /* OUTPUT KINEMATICS - convert to joints in local array */ result = kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions, &iflags, &fflags); + /* copy to joint structures and spline them up */ if(result == 0) { for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) { if(!isfinite(positions[joint_num])) { - reportError(_("kinematicsInverse gave non-finite joint location on joint %d"), joint_num); + reportError(_("kinematicsInverse gave non-finite joint location on joint %d"), + joint_num); SET_MOTION_ERROR_FLAG(1); SET_MOTION_ENABLE_FLAG(0); emcmotDebug->enabling = 0; @@ -1387,7 +1470,12 @@ static void get_pos_cmds(long period) axis->teleop_tp.curr_vel = 0.0; } } -} + if (ext_offset_teleop_limit || ext_offset_coord_limit) { + *(emcmot_hal_data->eoffset_limited) = 1; + } else { + *(emcmot_hal_data->eoffset_limited) = 0; + } +} // get_pos_cmds() /* NOTES: These notes are just my understanding of how things work. @@ -1831,24 +1919,17 @@ static void output_to_hal(void) /* point to HAL data */ axis_data = &(emcmot_hal_data->axis[axis_num]); /* write to HAL pins */ - - *(axis_data->teleop_vel_cmd) = axis->teleop_vel_cmd; - *(axis_data->teleop_pos_cmd) = axis->teleop_tp.pos_cmd; - *(axis_data->teleop_vel_lim) = axis->teleop_tp.max_vel; - *(axis_data->teleop_tp_enable) = axis->teleop_tp.enable; - *(axis_data->kb_ajog_active) = axis->kb_ajog_active; - *(axis_data->wheel_ajog_active) = axis->wheel_ajog_active; + *(axis_data->teleop_vel_cmd) = axis->teleop_vel_cmd; + *(axis_data->teleop_pos_cmd) = axis->teleop_tp.pos_cmd; + *(axis_data->teleop_vel_lim) = axis->teleop_tp.max_vel; + *(axis_data->teleop_tp_enable) = axis->teleop_tp.enable; + *(axis_data->kb_ajog_active) = axis->kb_ajog_active; + *(axis_data->wheel_ajog_active) = axis->wheel_ajog_active; + + // hal pins: axis.L.pos-cmd reported without applied offsets: + *(axis_data->pos_cmd) = *pcmd_p[axis_num] + - axis->ext_offset_tp.curr_pos; } - *(emcmot_hal_data->axis[0].pos_cmd) = emcmotStatus->carte_pos_cmd.tran.x; - *(emcmot_hal_data->axis[1].pos_cmd) = emcmotStatus->carte_pos_cmd.tran.y; - *(emcmot_hal_data->axis[2].pos_cmd) = emcmotStatus->carte_pos_cmd.tran.z; - *(emcmot_hal_data->axis[3].pos_cmd) = emcmotStatus->carte_pos_cmd.a; - *(emcmot_hal_data->axis[4].pos_cmd) = emcmotStatus->carte_pos_cmd.b; - *(emcmot_hal_data->axis[5].pos_cmd) = emcmotStatus->carte_pos_cmd.c; - *(emcmot_hal_data->axis[6].pos_cmd) = emcmotStatus->carte_pos_cmd.u; - *(emcmot_hal_data->axis[7].pos_cmd) = emcmotStatus->carte_pos_cmd.v; - *(emcmot_hal_data->axis[8].pos_cmd) = emcmotStatus->carte_pos_cmd.w; - } static void update_status(void) @@ -1902,7 +1983,17 @@ static void update_status(void) axis_status->max_pos_limit = axis->max_pos_limit; axis_status->min_pos_limit = axis->min_pos_limit; } - + emcmotStatus->eoffset_pose.tran.x = (&axes[0])->ext_offset_tp.curr_pos; + emcmotStatus->eoffset_pose.tran.y = (&axes[1])->ext_offset_tp.curr_pos; + emcmotStatus->eoffset_pose.tran.z = (&axes[2])->ext_offset_tp.curr_pos; + emcmotStatus->eoffset_pose.a = (&axes[3])->ext_offset_tp.curr_pos; + emcmotStatus->eoffset_pose.b = (&axes[4])->ext_offset_tp.curr_pos; + emcmotStatus->eoffset_pose.c = (&axes[5])->ext_offset_tp.curr_pos; + emcmotStatus->eoffset_pose.u = (&axes[6])->ext_offset_tp.curr_pos; + emcmotStatus->eoffset_pose.v = (&axes[7])->ext_offset_tp.curr_pos; + emcmotStatus->eoffset_pose.w = (&axes[8])->ext_offset_tp.curr_pos; + + emcmotStatus->external_offsets_applied = *(emcmot_hal_data->eoffset_active); for (dio = 0; dio < emcmotConfig->numDIO; dio++) { emcmotStatus->synch_di[dio] = *(emcmot_hal_data->synch_di[dio]); @@ -1942,3 +2033,232 @@ static void update_status(void) } #endif } + +static void sync_teleop_tp_to_carte_pos(int extfactor) +{ + int axis_num; + emcmot_axis_t *axis; + + // expect extfactor = -1 || 0 || +1 + for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { + axis = &axes[axis_num]; + axis->teleop_tp.curr_pos = *pcmd_p[axis_num] + + extfactor * axis->ext_offset_tp.curr_pos; + } +} //sync_teleop_tp_to_carte_pos() + +static void sync_carte_pos_to_teleop_tp(int extfactor) +{ + int axis_num; + emcmot_axis_t *axis; + + // expect extfactor = -1 || 0 || +1 + for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { + axis = &axes[axis_num]; + *pcmd_p[axis_num] = axis->teleop_tp.curr_pos + + extfactor * axis->ext_offset_tp.curr_pos; + } +} // sync_carte_pos_to_teleop_tp() + +static void apply_ext_offsets_to_carte_pos(int extfactor) +{ + int axis_num; + emcmot_axis_t *axis; + + // expect extfactor = -1 || 0 || +1 + for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { + axis = &axes[axis_num]; + *pcmd_p[axis_num] = *pcmd_p[axis_num] + + extfactor * axis->ext_offset_tp.curr_pos; + } +} // apply_ext_offsets_to_carte_pos() + +static void initialize_external_offsets() +{ + int axis_num; + emcmot_axis_t *axis; + axis_hal_t *axis_data; + for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { + axis = &axes[axis_num]; + axis_data = &(emcmot_hal_data->axis[axis_num]); + + *(axis_data->external_offset) = 0; + *(axis_data->external_offset_requested) = 0; + axis->ext_offset_tp.pos_cmd = 0; + axis->ext_offset_tp.curr_pos = 0; + axis->ext_offset_tp.curr_vel = 0; + } +} // initialize_external_offsets() + +static void plan_external_offsets(void) +{ + static int first_pass = 1; + int axis_num; + emcmot_axis_t *axis; + axis_hal_t *axis_data; + int new_eoffset_counts, delta; + static int last_eoffset_enable[EMCMOT_MAX_AXIS]; + + *(emcmot_hal_data->eoffset_active) = 0; //set if any enabled + + for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { + axis = &axes[axis_num]; + // coord,teleop updates done in get_pos_cmds() + axis->ext_offset_tp.max_vel = axis->ext_offset_vel_limit; + axis->ext_offset_tp.max_acc = axis->ext_offset_acc_limit; + + axis_data = &(emcmot_hal_data->axis[axis_num]); + + new_eoffset_counts = *(axis_data->eoffset_counts); + delta = new_eoffset_counts - axis->old_eoffset_counts; + axis->old_eoffset_counts = new_eoffset_counts; + + *(axis_data->external_offset) = axis->ext_offset_tp.curr_pos; + axis->ext_offset_tp.enable = 1; + if ( first_pass ) { + *(axis_data->external_offset) = 0; + continue; + } + + // Use stopping criterion of simple_tp.c: + ext_offset_epsilon = TINY_DP(axis->ext_offset_tp.max_acc,servo_period); + if (fabs(*(axis_data->external_offset)) > ext_offset_epsilon) { + *(emcmot_hal_data->eoffset_active) = 1; + } + if ( !*(axis_data->eoffset_enable) ) { + axis->ext_offset_tp.enable = 0; + // Detect disabling of eoffsets: + // At very high accel, simple planner may terminate with + // a larger position value than occurs at more realistic accels. + if ( last_eoffset_enable[axis_num] + && (fabs(*(axis_data->external_offset)) > ext_offset_epsilon) + && GET_MOTION_ENABLE_FLAG() + && axis->ext_offset_tp.enable + ) { +#if 1 + // to stdout only: + rtapi_print_msg(RTAPI_MSG_NONE, + "*** Axis_%c External Offset=%.4g eps=%.4g\n" + "*** External Offset disabled while NON-zero\n" + "*** To clear: re-enable & zero or use Machine-Off\n", + "XYZABCUVW"[axis_num], + *(axis_data->external_offset), + ext_offset_epsilon); +#else + // as error message: + reportError("Axis_%c External Offset=%.4g eps=%.4g\n" + "External Offset disabled while NON-zero\n" + "To clear: re-enable & zero or use Machine-Off", + "XYZABCUVW"[axis_num], + *(axis_data->external_offset), + ext_offset_epsilon); +#endif + } + last_eoffset_enable[axis_num] = 0; + continue; // Note: if not eoffset_enable + // then planner disabled and no pos_cmd updates + // useful for eoffset_pid hold + } + last_eoffset_enable[axis_num] = 1; + if (*(axis_data->eoffset_clear)) { + axis->ext_offset_tp.pos_cmd = 0; + *(axis_data->external_offset_requested) = 0; + continue; + } + if ( delta == 0 ) { continue; } + if ( !checkAllHomed() ) { continue; } + if ( !GET_MOTION_ENABLE_FLAG() ) { continue; } + + axis->ext_offset_tp.pos_cmd += delta * *(axis_data->eoffset_scale); + *(axis_data->external_offset_requested) = axis->ext_offset_tp.pos_cmd; + } // for axis_num + first_pass = 0; +} // plan_external_offsets() + +static int update_teleop_with_check(int axis_num,simple_tp_t *the_tp) +{ + // 'the_tp' is the planner to update + // the tests herein apply to the sum of the offsets for both + // planners (teleop_tp and ext_offset_tp) + double save_curr_pos; + emcmot_axis_t *axis = &axes[axis_num]; + + save_curr_pos = the_tp->curr_pos; + simple_tp_update(the_tp, servo_period ); + + //workaround: axis letters not in [TRAJ]COORDINATES + // have min_pos_limit == max_pos_lim == 0 + if ( (0 == axis->max_pos_limit) && (0 == axis->min_pos_limit) ) { + return 0; + } + if ( (axis->ext_offset_tp.curr_pos + axis->teleop_tp.curr_pos) + >= axis->max_pos_limit) { + // positive error, restore save_curr_pos + the_tp->curr_pos = save_curr_pos; + the_tp->curr_vel = 0; + return 1; + } + if ( (axis->ext_offset_tp.curr_pos + axis->teleop_tp.curr_pos) + <= axis->min_pos_limit) { + // negative error, restore save_curr_pos + the_tp->curr_pos = save_curr_pos; + the_tp->curr_vel = 0; + return 1; + } + return 0; +} // update_teleop_with_check() + +static int update_coord_with_bound(void) +{ + int axis_num; + int ans = 0; + emcmot_axis_t *axis; + double save_pos_cmd[EMCMOT_MAX_AXIS]; + double save_offset_cmd[EMCMOT_MAX_AXIS]; + + for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { + axis = &axes[axis_num]; + save_pos_cmd[axis_num] = *pcmd_p[axis_num]; + save_offset_cmd[axis_num] = axis->ext_offset_tp.pos_cmd; + simple_tp_update(&(axis->ext_offset_tp), servo_period ); + } + apply_ext_offsets_to_carte_pos(+1); // add external offsets + + for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { + axis = &axes[axis_num]; + //workaround: axis letters not in [TRAJ]COORDINATES + // have min_pos_limit == max_pos_lim == 0 + if ( (0 == axis->max_pos_limit) && (0 == axis->min_pos_limit) ) { + continue; + } + if (axis->ext_offset_tp.curr_pos == 0) { + continue; // don't claim violation if no offset + } + + if (*pcmd_p[axis_num] >= axis->max_pos_limit) { + // hold carte_pos_cmd at the limit: + *pcmd_p[axis_num] = axis->max_pos_limit; + // stop growth of offsetting position: + axis->ext_offset_tp.curr_pos = axis->max_pos_limit + - save_pos_cmd[axis_num]; + if (axis->ext_offset_tp.pos_cmd > save_offset_cmd[axis_num]) { + axis->ext_offset_tp.pos_cmd = save_offset_cmd[axis_num]; + } + axis->ext_offset_tp.curr_vel = 0; + ans++; + continue; + } + if (*pcmd_p[axis_num] <= axis->min_pos_limit) { + *pcmd_p[axis_num] = axis->min_pos_limit; + axis->ext_offset_tp.curr_pos = axis->min_pos_limit + - save_pos_cmd[axis_num]; + if (axis->ext_offset_tp.pos_cmd < save_offset_cmd[axis_num]) { + axis->ext_offset_tp.pos_cmd = save_offset_cmd[axis_num]; + } + axis->ext_offset_tp.curr_vel = 0; + ans++; + } + } + if (ans > 0) { return 1; } + return 0; +} // update_coord_with_bound() diff --git a/src/emc/motion/mot_priv.h b/src/emc/motion/mot_priv.h index f9ef3fa78db..3fa940965d9 100644 --- a/src/emc/motion/mot_priv.h +++ b/src/emc/motion/mot_priv.h @@ -91,6 +91,13 @@ typedef struct { hal_bit_t *ajog_vel_mode; /* RPI: true for "velocity mode" jogwheel */ hal_bit_t *kb_ajog_active; /* RPI: executing keyboard jog */ hal_bit_t *wheel_ajog_active;/* RPI: executing handwheel jog */ + + hal_bit_t *eoffset_enable; + hal_bit_t *eoffset_clear; + hal_s32_t *eoffset_counts; + hal_float_t *eoffset_scale; + hal_float_t *external_offset; + hal_float_t *external_offset_requested; } axis_hal_t; /* machine data */ @@ -192,6 +199,8 @@ typedef struct { joint_hal_t joint[EMCMOT_MAX_JOINTS]; /* data for each joint */ axis_hal_t axis[EMCMOT_MAX_AXIS]; /* data for each axis */ + hal_bit_t *eoffset_active; /* ext offsets active */ + hal_bit_t *eoffset_limited; /* ext offsets exceed limit */ } emcmot_hal_data_t; /*********************************************************************** @@ -347,4 +356,6 @@ extern void reportError(const char *fmt, ...) __attribute((format(printf,1,2))); #if defined(__KERNEL__) #define HAVE_CPU_KHZ #endif + +#define EOFFSET_EPSILON 1e-8 #endif /* MOT_PRIV_H */ diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index d423edfe588..53603132e7f 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -485,12 +485,31 @@ static int init_hal_io(void) mot_comp_id, "axis.%c.teleop-vel-lim", c)) != 0) goto error; if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->axis[n].teleop_tp_enable), mot_comp_id, "axis.%c.teleop-tp-enable",c)) != 0) goto error; + if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->axis[n].eoffset_enable), + mot_comp_id, "axis.%c.eoffset-enable", c)) != 0) return retval; + if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->axis[n].eoffset_clear), + mot_comp_id, "axis.%c.eoffset-clear", c)) != 0) return retval; + + if ((retval = hal_pin_s32_newf(HAL_IN, &(emcmot_hal_data->axis[n].eoffset_counts), + mot_comp_id, "axis.%c.eoffset-counts", c)) != 0) return retval; + if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->axis[n].eoffset_scale), + mot_comp_id, "axis.%c.eoffset-scale", c)) != 0) return retval; + + if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->axis[n].external_offset), + mot_comp_id, "axis.%c.eoffset", c)) != 0) return retval; + if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->axis[n].external_offset_requested), + mot_comp_id, "axis.%c.eoffset-request", c)) != 0) return retval; + retval = export_axis(c, axis_data); if (retval != 0) { rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: axis %c pin/param export failed\n"), c); return -1; } } + if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->eoffset_limited), mot_comp_id, + "motion.eoffset-limited")) < 0) goto error; + if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->eoffset_active), mot_comp_id, + "motion.eoffset-active")) < 0) goto error; /* Done! */ rtapi_print_msg(RTAPI_MSG_INFO, diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index a59ac0cfe72..4c01da0cfe9 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -268,6 +268,8 @@ extern "C" { double arcBlendRampFreq; double arcBlendTangentKinkRatio; double maxFeedScale; + double ext_offset_vel; /* velocity for an external axis offset */ + double ext_offset_acc; /* acceleration for an external axis offset */ } emcmot_command_t; /*! \todo FIXME - these packed bits might be replaced with chars @@ -613,6 +615,11 @@ Suggestion: Split this in to an Error and a Status flag register.. int kb_ajog_active; /* non-zero during a keyboard jog */ int wheel_ajog_active; /* non-zero during a wheel jog */ int locking_joint; /* locking_joint number, -1 ==> notused*/ + + double ext_offset_vel_limit; /* upper limit of axis speed for ext offset */ + double ext_offset_acc_limit; /* upper limit of axis accel for ext offset */ + int old_eoffset_counts; + simple_tp_t ext_offset_tp;/* planner for external coordinate offsets*/ } emcmot_axis_t; typedef struct { @@ -720,7 +727,9 @@ Suggestion: Split this in to an Error and a Status flag register.. EmcPose tool_offset; int atspeed_next_feed; /* at next feed move, wait for spindle to be at speed */ int spindle_is_atspeed; /* hal input */ - unsigned char tail; /* flag count for mutex detect */ + unsigned char tail; /* flag count for mutex detect */ + int external_offsets_applied; + EmcPose eoffset_pose; } emcmot_status_t; diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index b84a0eb4c73..609623b04b1 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -969,4 +969,10 @@ extern void PLUGIN_CALL(int len, const char *call); // same for IoTask context extern void IO_PLUGIN_CALL(int len, const char *call); +extern int GET_EXTERNAL_OFFSET_APPLIED(); +extern EmcPose GET_EXTERNAL_OFFSETS(); + +#undef STOP_ON_SYNCH_IF_EXTERNAL_OFFSETS +#define STOP_ON_SYNCH_IF_EXTERNAL_OFFSETS + #endif /* ifndef CANON_HH */ diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index ab6b5388e93..f1659729e49 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -337,8 +337,8 @@ extern int emcOperatorDisplay(int id, const char *fmt, ...) __attribute__((forma extern int emcAxisSetUnits(int axis, double units); extern int emcAxisSetMinPositionLimit(int axis, double limit); extern int emcAxisSetMaxPositionLimit(int axis, double limit); -extern int emcAxisSetMaxVelocity(int axis, double vel); -extern int emcAxisSetMaxAcceleration(int axis, double acc); +extern int emcAxisSetMaxVelocity(int axis, double vel, double ext_offset_vel); +extern int emcAxisSetMaxAcceleration(int axis, double acc, double ext_offset_acc); extern double emcAxisGetMaxVelocity(int axis); extern double emcAxisGetMaxAcceleration(int axis); extern int emcAxisSetLockingJoint(int axis,int joint); @@ -529,6 +529,8 @@ int emcSetupArcBlends(int arcBlendEnable, double arcBlendRampFreq, double arcBlendTangentKinkRatio); int emcSetProbeErrorInhibit(int j_inhibit, int h_inhibit); +int emcGetExternalOffsetApplied(void); +EmcPose emcGetExternalOffsets(void); extern int emcUpdate(EMC_STAT * stat); // full EMC status diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index f0652c63585..6dda52401a8 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -1211,6 +1211,8 @@ class EMC_MOTION_STAT:public EMC_MOTION_STAT_MSG { double analog_output[EMCMOT_MAX_AIO]; //motion analog outputs queried by interp int debug; // copy of EMC_DEBUG global int on_soft_limit; + int external_offsets_applied; + EmcPose eoffset_pose; }; // declarations for EMC_TASK classes diff --git a/src/emc/rs274ngc/canonmodule.cc b/src/emc/rs274ngc/canonmodule.cc index dac4cb702c1..e21a2377776 100644 --- a/src/emc/rs274ngc/canonmodule.cc +++ b/src/emc/rs274ngc/canonmodule.cc @@ -242,4 +242,7 @@ BOOST_PYTHON_MODULE(emccanon) { // def("enqueue_ARC_FEED", &enqueue_ARC_FEED); // def("enqueue_M_USER_COMMAND ", &enqueue_M_USER_COMMAND); // def("enqueue_START_CHANGE", & enqueue_START_CHANGE); + def("GET_EXTERNAL_OFFSET_APPLIED",&GET_EXTERNAL_OFFSET_APPLIED); + def("GET_EXTERNAL_OFFSETS",&GET_EXTERNAL_OFFSETS); + } diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index e35778b12e5..aed6d490e7c 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -575,6 +575,21 @@ int GET_EXTERNAL_SPINDLE_OVERRIDE_ENABLE() {return 1;} int GET_EXTERNAL_ADAPTIVE_FEED_ENABLE() {return 0;} int GET_EXTERNAL_FEED_HOLD_ENABLE() {return 1;} +int GET_EXTERNAL_OFFSET_APPLIED() {return 0;} +EmcPose GET_EXTERNAL_OFFSETS() { + EmcPose e; + e.tran.x = 0; + e.tran.y = 0; + e.tran.z = 0; + e.a = 0; + e.b = 0; + e.c = 0; + e.u = 0; + e.v = 0; + e.w = 0; + return e; +}; + int GET_EXTERNAL_AXIS_MASK() { if(interp_error) return 7; PyObject *result = @@ -1030,5 +1045,4 @@ initgcode(void) { PyObject_SetAttrString(m, "MIN_ERROR", PyInt_FromLong(INTERP_MIN_ERROR)); } - // vim:ts=8:sts=4:sw=4:et: diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index 20cf5981d93..e0bd56e89b9 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -1959,18 +1959,39 @@ int Interp::synch() { char file_name[LINELEN]; - - _setup.control_mode = GET_EXTERNAL_MOTION_CONTROL_MODE(); + _setup.current_x = GET_EXTERNAL_POSITION_X(); + _setup.current_y = GET_EXTERNAL_POSITION_Y(); + _setup.current_z = GET_EXTERNAL_POSITION_Z(); _setup.AA_current = GET_EXTERNAL_POSITION_A(); _setup.BB_current = GET_EXTERNAL_POSITION_B(); _setup.CC_current = GET_EXTERNAL_POSITION_C(); + _setup.u_current = GET_EXTERNAL_POSITION_U(); + _setup.v_current = GET_EXTERNAL_POSITION_V(); + _setup.w_current = GET_EXTERNAL_POSITION_W(); + + if (GET_EXTERNAL_OFFSET_APPLIED() ) { +#ifdef STOP_ON_SYNCH_IF_EXTERNAL_OFFSETS + return INTERP_ERROR; // synch() return error --> stop +#else + // Removing an external offset applied in motion works + // here _unless_ there is a loop (repeat,while,do-while) + // with queuebusters. + // Below is for experimental usage only: + EmcPose e = GET_EXTERNAL_OFFSETS(); + _setup.current_x = GET_EXTERNAL_POSITION_X() - e.tran.x; + _setup.current_y = GET_EXTERNAL_POSITION_Y() - e.tran.y; + _setup.current_z = GET_EXTERNAL_POSITION_Z() - e.tran.z; + _setup.AA_current = GET_EXTERNAL_POSITION_A() - e.a; + _setup.BB_current = GET_EXTERNAL_POSITION_B() - e.b; + _setup.CC_current = GET_EXTERNAL_POSITION_C() - e.c; + _setup.u_current = GET_EXTERNAL_POSITION_U() - e.u; + _setup.v_current = GET_EXTERNAL_POSITION_V() - e.v; + _setup.w_current = GET_EXTERNAL_POSITION_W() - e.w; +#endif + } + + _setup.control_mode = GET_EXTERNAL_MOTION_CONTROL_MODE(); _setup.current_pocket = GET_EXTERNAL_TOOL_SLOT(); - _setup.current_x = GET_EXTERNAL_POSITION_X(); - _setup.current_y = GET_EXTERNAL_POSITION_Y(); - _setup.current_z = GET_EXTERNAL_POSITION_Z(); - _setup.u_current = GET_EXTERNAL_POSITION_U(); - _setup.v_current = GET_EXTERNAL_POSITION_V(); - _setup.w_current = GET_EXTERNAL_POSITION_W(); _setup.feed_rate = GET_EXTERNAL_FEED_RATE(); _setup.flood = GET_EXTERNAL_FLOOD(); _setup.length_units = GET_EXTERNAL_LENGTH_UNIT_TYPE(); diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index c2775a81df2..3fdfb712787 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -1150,6 +1150,24 @@ int GET_EXTERNAL_TC_REASON() return _toolchanger_reason; } +int GET_EXTERNAL_OFFSET_APPLIED() { + return 0; +}; + +EmcPose GET_EXTERNAL_OFFSETS() { + EmcPose e; + e.tran.x = 0; + e.tran.y = 0; + e.tran.z = 0; + e.a = 0; + e.b = 0; + e.c = 0; + e.u = 0; + e.v = 0; + e.w = 0; + return e; +}; + /* Sends error message */ void CANON_ERROR(const char *fmt, ...) { diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 112505fc7e6..74547db3ae4 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -2943,6 +2943,14 @@ int GET_EXTERNAL_AXIS_MASK() { return emcStatus->motion.traj.axis_mask; } +int GET_EXTERNAL_OFFSET_APPLIED(void) { + return emcGetExternalOffsetApplied(); +} + +EmcPose GET_EXTERNAL_OFFSETS() { + return emcGetExternalOffsets(); +} + CANON_PLANE GET_EXTERNAL_PLANE() { return canon.activePlane; diff --git a/src/emc/task/emctask.cc b/src/emc/task/emctask.cc index 2b0dbbab018..bb910f2bd82 100644 --- a/src/emc/task/emctask.cc +++ b/src/emc/task/emctask.cc @@ -35,7 +35,6 @@ #include "taskclass.hh" #include "motion.h" - #define USER_DEFINED_FUNCTION_MAX_DIRS 5 #define MAX_M_DIRS (USER_DEFINED_FUNCTION_MAX_DIRS+1) //note:the +1 is for the PROGRAM_PREFIX or default directory==nc_files @@ -194,9 +193,19 @@ int emcTaskAbort() stepping = 0; steppingWait = 0; +#ifdef STOP_ON_SYNCH_IF_EXTERNAL_OFFSETS + if (GET_EXTERNAL_OFFSET_APPLIED()) { + emcStatus->task.execState = EMC_TASK_EXEC_DONE; + } else { + // now queue up command to resynch interpreter + EMC_TASK_PLAN_SYNCH taskPlanSynchCmd; + emcTaskQueueCommand(&taskPlanSynchCmd); + } +#else // now queue up command to resynch interpreter EMC_TASK_PLAN_SYNCH taskPlanSynchCmd; emcTaskQueueCommand(&taskPlanSynchCmd); +#endif // without emcTaskPlanClose(), a new run command resumes at // aborted line-- feature that may be considered later @@ -496,9 +505,13 @@ int emcTaskPlanSetBlockDelete(bool state) return 0; } + int emcTaskPlanSynch() { int retval = interp.synch(); + if (retval == INTERP_ERROR) { + emcTaskAbort(); + } if (emc_debug & EMC_DEBUG_INTERP) { rcs_print("emcTaskPlanSynch() returned %d\n", retval); diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 7431ecc03ca..e44ec9a6b52 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -60,7 +60,6 @@ #include #include "usrmotintf.h" - #if 0 // Enable this to niftily trap floating point exceptions for debugging #include @@ -84,6 +83,8 @@ fpu_control_t __fpu_control = _FPU_IEEE & ~(_FPU_MASK_IM | _FPU_MASK_ZM | _FPU_M #include "motion.h" // EMCMOT_ORIENT_* #include "inihal.hh" +static int stop_if_eoffsets_at_synch = 0; + static emcmot_config_t emcmotConfig; /* time after which the user interface is declared dead @@ -550,6 +551,8 @@ void readahead_reading(void) emcTaskPlanSetWait(); // and resynch interp WM emcTaskQueueCommand(&taskPlanSynchCmd); + + stop_if_eoffsets_at_synch = 1; } else if (execRetval != 0) { // end of file emcStatus->task.interpState = @@ -969,8 +972,14 @@ static int emcTaskPlan(void) emcTaskQueueCommand(&taskPlanSynchCmd); break; - // otherwise we can't handle it + case EMC_TASK_PLAN_RUN_TYPE: + if (GET_EXTERNAL_OFFSET_APPLIED()) { + // err here, fewer err reports + retval = -1; + } + break; + // otherwise we can't handle it default: emcOperatorError(0, _("can't do that (%s:%d) in manual mode"), emc_symbol_lookup(type),(int) type); @@ -1381,6 +1390,13 @@ static int emcTaskPlan(void) break; case EMC_TASK_PLAN_EXECUTE_TYPE: + // mdi with pre-existing external offset: + // too many issues (loops, sub calls, queue busters) + // just deny + if (GET_EXTERNAL_OFFSET_APPLIED()) { + emcOperatorError(0,"Cannot start MDI with External Offsets"); + break; + } // If there are no queued MDI commands, no commands in // interp_list, and waitFlag isn't set, then this new // incoming MDI command can just be issued directly. @@ -2108,6 +2124,14 @@ static int emcTaskIssueCommand(NMLmsg * cmd) // now queue up command to resynch interpreter emcTaskQueueCommand(&taskPlanSynchCmd); } +#if 0 + if ( GET_EXTERNAL_OFFSET_APPLIED() + && ( (mode_msg->mode == EMC_TASK_MODE_AUTO) + || (mode_msg->mode == EMC_TASK_MODE_MDI) ) ) { + fprintf(stderr,"Starting with EXTERNAL OFFSETS applied\n"); + } +#endif + retval = emcTaskSetMode(mode_msg->mode); } break; @@ -2526,8 +2550,16 @@ static int emcTaskExecute(void) stepping = 0; steppingWait = 0; - // now queue up command to resynch interpreter - emcTaskQueueCommand(&taskPlanSynchCmd); + if (GET_EXTERNAL_OFFSET_APPLIED()) { + if ( stop_if_eoffsets_at_synch + || emcStatus->task.mode == EMC_TASK_MODE_MDI) { + emcOperatorError(0,"Stopping at synch() with External Offsets"); + } + } else { + // now queue up command to resynch interpreter + emcTaskQueueCommand(&taskPlanSynchCmd); + } + stop_if_eoffsets_at_synch = 0; retval = -1; break; diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index ff005ea2d28..1cc8e815363 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -463,7 +463,7 @@ int emcAxisSetMaxPositionLimit(int axis, double limit) return retval; } -int emcAxisSetMaxVelocity(int axis, double vel) +int emcAxisSetMaxVelocity(int axis, double vel,double ext_offset_vel) { CATCH_NAN(std::isnan(vel)); @@ -480,6 +480,7 @@ int emcAxisSetMaxVelocity(int axis, double vel) emcmotCommand.command = EMCMOT_SET_AXIS_VEL_LIMIT; emcmotCommand.axis = axis; emcmotCommand.vel = vel; + emcmotCommand.ext_offset_vel = ext_offset_vel; int retval = usrmotWriteEmcmotCommand(&emcmotCommand); if (emc_debug & EMC_DEBUG_CONFIG) { @@ -488,7 +489,7 @@ int emcAxisSetMaxVelocity(int axis, double vel) return retval; } -int emcAxisSetMaxAcceleration(int axis, double acc) +int emcAxisSetMaxAcceleration(int axis, double acc,double ext_offset_acc) { CATCH_NAN(std::isnan(acc)); @@ -505,6 +506,7 @@ int emcAxisSetMaxAcceleration(int axis, double acc) emcmotCommand.command = EMCMOT_SET_AXIS_ACC_LIMIT; emcmotCommand.axis = axis; emcmotCommand.acc = acc; + emcmotCommand.ext_offset_acc = ext_offset_acc; int retval = usrmotWriteEmcmotCommand(&emcmotCommand); if (emc_debug & EMC_DEBUG_CONFIG) { @@ -1831,7 +1833,6 @@ int emcMotionUpdate(EMC_MOTION_STAT * stat) if (0 != usrmotReadEmcmotStatus(&emcmotStatus)) { return -1; } - new_config = 0; if (emcmotStatus.config_num != emcmotConfig.config_num) { if (0 != usrmotReadEmcmotConfig(&emcmotConfig)) { @@ -1945,3 +1946,11 @@ int emcSetProbeErrorInhibit(int j_inhibit, int h_inhibit) { emcmotCommand.probe_home_err_inhibit = h_inhibit; return usrmotWriteEmcmotCommand(&emcmotCommand); } + +int emcGetExternalOffsetApplied(void) { + return emcmotStatus.external_offsets_applied; +} + +EmcPose emcGetExternalOffsets(void) { + return emcmotStatus.eoffset_pose; +} diff --git a/src/hal/components/eoffset_per_angle.comp b/src/hal/components/eoffset_per_angle.comp new file mode 100644 index 00000000000..11a69fb5643 --- /dev/null +++ b/src/hal/components/eoffset_per_angle.comp @@ -0,0 +1,298 @@ +component eoffset_per_angle "Compute External Offset Per Angle"; +//" vim +description +""" + +An offset is computed (from one of several functions) based on +an input angle in degrees. The angle could be a rotary +coordinate value or a spindle angle. + +The computed offset is represented as an s32 \\fBkcounts\\fR output +pin that is a compatible input to external offset pins like +\\fBaxis.L.eoffset-counts\\fR where \\fBL\\fR is the coordinate +letter. Scaling of the s32 \\fBkcounts\\fR is controlled by the +input (\\fBk\\fR) -- its reciprocal value is presented on an output pin +(\\fBkreciprocal\\fR) for connection to \\fBaxis.L.eoffset-scale\\fR. +The default value for \\fBk\\fR should be suitable for most uses. + +The built-in functions use pins \\fBfmul\\fR and \\fBrfrac\\fR +to control the output frequency (or number of polygon sides) +and amplitude respectively. The \\fBrfrac\\fR pin controls +the offset amplitude as a fraction of the \\fBradius-ref\\fR pin. + +Three built-in functions: + \\fBf0\\fR polygon (requires number of sides >= 3) + \\fBf1\\fR sinusoid + \\fBf2\\fR square wave + +\\fBNOTES:\\fR + +\\fBradius-ref\\fR: +The computed offsets are based on the \\fBradius-ref\\fR pin +value. This pin may be set to a constant radius value or +controlled by a user interface or by g code program (using +\\fBM68\\fR and a \\fBmotion.analog-out-NN pin for instance). + +\\fBStopping\\fR: +When the \\fBenable-in\\fR pin is deasserted, the offset is +returned to zero respecting the allocated acceleration +and velocity limits. The allocations for coordinate \\fBL\\fR +are typically controlled by an ini file setting: +\\fB[AXIS_L]OFFSET_AV_RATIO\\fR. + +\\fBNOTES\\fR: +If unsupported parameters are supplied to a function (for instance +a polygon with fewer than three sides), the current offset +will be returned to zero (respecting velocity and acceleration +constraints). After correcting the offending parameter, the +\\fBenable-in\\fR pin must be toggled to resume offset +computations. + +\\fBEXAMPLE:\\fR +An example simulation configuration is provided at: +\\fBconfigs/sim/axis/external_offsets/opa.ini\\fR. A +simulated XZC machine uses the \\fBC\\fR coordinate angle to +offset the transverse \\fBX\\fR coordinate according to +the selected \\fBfnum\\fR function. +"""; + +//" quote char for vim highlighting + +pin in bit active = 0 "From: motion.eoffset-active"; +pin in bit is_on = 0 "From: halui.machine.is-on"; + +pin in bit enable_in = 0 "Enable Input"; +pin in float radius_ref = 1 "Radius reference (see notes)"; +pin in float angle = 0 "Input angle (in degrees)"; +pin in float start_angle = 0 "Start angle (in degrees)"; +pin in s32 fnum = 0 "Function selector (default 0)"; +pin in float rfrac = 0.1 "Offset amplitude (+/- r fraction)"; +pin in float fmul = 6 "Offset frequency multiplier"; +pin in u32 k = 10000 "Scaling Factor (if 0, use 10000)"; + +pin out bit is_off "invert is_on (for convenience)"; + +pin out bit enable_out "To: axis.L.eoffset-enable"; +pin out bit clear "To: axis.L.eoffset-clear"; +pin out s32 kcounts "To: axis.L.eoffset-counts"; +pin out float kreciprocal "To: axis.L.eoffset-scale (1/k)"; + +pin out float eoffset_dbg "offset (debug pin--use kcounts & kreciprocal)"; +pin out u32 state_dbg "state (debug pin)"; + +function _; +license "GPL"; +;; + +#include + +#define FINISH_DELAY 0 +#define TO_RAD M_PI/180 + +static int run_ct = 0; +static int delay_ct = 0; + +struct ofunc_data { + double adeg; + double astart; + double r; + double rfraction; + double fmultiplier; + double ovalue; +}; + +typedef int ofunc(struct ofunc_data*); +static ofunc func0,func1,func2; + +typedef enum { + OFF, + READY, + RUNNING, + STOPPING, + FINISH, +} state; + +#define OPA_DEBUG +#undef OPA_DEBUG +#ifdef OPA_DEBUG + #define LVL RTAPI_MSG_INFO + #define dprint(msg,n) do { \ + rtapi_set_msg_level(LVL); \ + rtapi_print_msg(LVL,"%20s %5d\n",msg,n); \ + } while (0) +#else + #define dprint(msg,n) +#endif + +FUNCTION(_) { + static state thestate = OFF; + static int messaged = 0; + static int err_stop = 0; + int kfactor; + + static struct ofunc_data the_data; + struct ofunc_data* data = &the_data; + ofunc* thefunc; + + run_ct++; + state_dbg = thestate; + if (k == 0) {kfactor = 10000;} + kreciprocal = 1/((float)kfactor); + + is_off = !is_on; // convenience pin + + if (is_off) { + // note: the external_offsets implementation defines + // axis.L.eoffset as zero when machine is off + err_stop = 0; + enable_out = 0; + kcounts = 0; eoffset_dbg = 0; + messaged = 0; + thestate = OFF; + return; + } + + switch (thestate) { + case OFF: + // require an enable_in 0-->1 transition to advance to READY + if (enable_in) { + if (!messaged) { + rtapi_print_msg(RTAPI_MSG_ERR, + "eoffset_per_angle: active enable-in not honored at start"); + messaged = 1; + } + return; + } + err_stop = 0; + messaged = 1; + kcounts = 0; + thestate = READY; + dprint("OFF->READY",kcounts); + return; + break; + case READY: + if (!enable_in) {return;} + kcounts = 0; eoffset_dbg = 0; + enable_out = 0; + delay_ct = 0; + thestate = RUNNING; + dprint("READY->RUNNING",kcounts); + return; + break; + case RUNNING: + if (enable_in) { + enable_out = 1; + thestate = RUNNING; + } else { + /* + ** When the enable_in pin is deasserted, kcounts are set to + ** zero and the simple trajectory planner removes the offset to + ** within its stopping criterion. Under some conditions, a + ** residual offset may remain. Connecting the clear pin to + ** axis.L.eoffset-clear forces the axis->ext_offset_tp.pos_cmd + ** to zero to remove any residual with no modifications to + ** simple_tp.c + */ + clear = 1; + kcounts = 0; eoffset_dbg = 0; + thestate = STOPPING; + delay_ct = run_ct; + dprint("RUNNING->STOPPING",kcounts); + return; + } + break; + case STOPPING: + if (active) { + thestate = STOPPING; + } else { + // !active ==> stopping criterion met + delay_ct = run_ct; + thestate = FINISH; + dprint("STOPPING->FINISH",kcounts); + } + return; + break; + case FINISH: + // provision for delay if needed + if (run_ct < (FINISH_DELAY + delay_ct) ) { + thestate = FINISH; + } else { + enable_out = 0; + if (err_stop) { + thestate = OFF; + dprint("FINISH->OFF",kcounts); + } else { + thestate = READY; + dprint("FINISH->READY",kcounts); + } + } + clear = 0; + return; + break; + } //switch (thestate) + + switch (fnum) { + case 0: thefunc = func0; break; + case 1: thefunc = func1; break; + case 2: thefunc = func2; break; + default: thefunc = func0; break; + } + data->adeg = angle; + data->astart = start_angle; + data->r = radius_ref; + data->rfraction = rfrac; + data->fmultiplier = fmul; + + if (thefunc(data) ) { + // thefunc returned nonzero (problem) + err_stop = 1; + data->ovalue = 0; + kcounts = 0; eoffset_dbg = 0; + thestate = STOPPING; + rtapi_print_msg(RTAPI_MSG_ERR, + "eoffset_per_angle stopping:func%d problem\n",fnum); + return; + } + + kcounts = kfactor * data->ovalue; + eoffset_dbg = kcounts * kreciprocal; // debug pin + return; +} + +static int func0 (struct ofunc_data* d) +{ + // polygon (fmultiplier > 2) + double uangle,divisor; + + if (d->fmultiplier <= 2) { + rtapi_print_msg(RTAPI_MSG_ERR, + "func0 bad fmultiplier: %g\n",d->fmultiplier); + return -1; + } + + divisor = 360/d->fmultiplier; + uangle = d->adeg + divisor/2 - d->astart; + d->ovalue = (1 + d->rfraction) + * d->r/cos((divisor/2 - fmod(uangle,divisor))*TO_RAD) - d->r; + return 0; +} + +static int func1 (struct ofunc_data* d) +{ + // sin() -90 ==> start at zero amplitude + d->adeg = d->fmultiplier * (d->adeg - d->astart) + -90; + d->ovalue = (0.5 * d->rfraction * d->r) * (1 + sin(d->adeg * TO_RAD)); + return 0; +} + +static int func2 (struct ofunc_data* d) +{ + // square() -90 ==> start at zero amplitude + // useful for looking at affects of ini settings + // max vel & accel and offset_av_ratio + double value = -1; + d->adeg = d->fmultiplier * (d->adeg - d->astart) + -90; + if (sin(d->adeg * TO_RAD) >= 0) {value = 1;} + d->ovalue = (0.5 * d->rfraction * d->r) * (1 + value); + return 0; +} diff --git a/src/hal/components/eoffset_pid.comp b/src/hal/components/eoffset_pid.comp new file mode 100644 index 00000000000..3fe41c7d813 --- /dev/null +++ b/src/hal/components/eoffset_pid.comp @@ -0,0 +1,476 @@ +component eoffset_pid "PID control of external offset"; + +//" vim +description +""" +The \\fBeoffset_pid\\fR hal component implements a state machine +and PID (Proportional, Integral, Derivative) controller for +managing the external offset for a single axis. A candidate +application is torch-height-control for a plasma cutter. + +The component receives a \\fBcommand\\fR input representing a +requested torch voltage and a \\fBfeedback\\fR input representing +measured torch voltage. A conventional PID computation uses +inputs \\fBpgain\\fR, \\fBigain\\fR, and \\fBdgain\\fR to +generate an external offset control presented as an s32 +\\fBkcounts\\fR pin. + +Scaling is controlled by a single input pin +\\fBk\\fR; a reciprocal output is provided at the +\\fBkreciprocal\\fR pin. The default value for \\fBk\\fR +should be suitable for most configurations. + +Three enabling inputs \\fBenable-in-a\\fR, \\fBenable-in-b\\fR, +and \\fBenable-in-c\\fR are 'anded' internally to enable +offset computation by connection to the \\fBaxis.L.eoffset-enable\\fR +pin for the controlled axis. Disabling externel offsets +causes any external offset to be reduced to zero (respecting +velocity and acceleration constraints) so these pins are not +suitable for short-term control of external offsets. + +Short term holding of the applied external offset is available using +pins provided for corner lockout (\\fBminimal-vel\\fR and +\\fBcurrent-vel\\fR). The pin \\fBminimum-vel\\fR is used to reduce +excessive height corrections that may occur when torch voltage increases +due to reduction in velocity on corners. Current offset values are +sustained when the \\fBcurrent-vel\\fR pin value is less than the +\\fBminimum-vel\\fR pin value. To disable this function, set +\\fBminimum-vel\\fR to zero. A separate pin, \\fBhold-request\\fR, +is available to hold the current external offset for short intervals. + +A number of pins require mandatory connections as listed +in the pin descriptions. + +\\fBNOTES\\fR + +When used to regulate an axis offset, the \\fBeoffset_pid\\fR +component servo loop is an outer loop to the \\fBLinuxCNC\\fR +motion loop. Since the inner loop effectively contains +an integrator, use of the \\fBigain\\fR pin is not +expected to be helpful and some amount of \\fBdgain\\fR may +be required for stability. + +The use of the \\fBeoffset_pid\\fR component as a feedback loop +enclosing the motion/external offset facilities of LinuxCNC is +influenced by many factors that can affect loop stability and hence the +effectiveness of the control. The factors include all the inputs to the +component itself plus a) velocity and acceleration settings of the +controlled axis in LinuxCNC, b) settings for torch timing and voltage +control, and importantly c) torch voltage measurement, filtering, and +noise control. + + +\\fBRESTRICTIONS\\fR + +Gcode commands that require synchronization of the interpreter +and motion modules (\\fBqueuebusters\\fR) result in immediate +\\fBtermination\\fR of motion if applied external offsets are +nonzero. Commands that read hal pins (\\fBM66\\fR) and probe +commands (\\fBG38.x\\fR) require this synchronization. Consequently, +reading hal pins and probe moves must be completed before +enabling external offsets by the 'anding' of the enabling +input pins. + +See the Documents chapter for \\fBExternal Axis Offsets\\fR for +more information on these and other restrictions. + + +\\fBEXPERIMENTAL derivative limit function (fnum=1)\\fR + +The function specified with fnum=1 is an experimental +pid derivateive gain calculation based on: + +"Control System Design", Karl Johan Astrom, 2002 +Eqn 6.2: limit high frequency gain of derivative term +Eqn 6.17: derivative estimation using backward difference. + +For this function, use the \\fBkptd\\fR pin for derivative gain +and \\fBnfilt\\fR to modify high frequency response for +computation of derivative gain. + +This function may be deleted in the future. + +\\fBDEBUG PINS:\\fR + +Pins named \\fB*.dbg-*\\fR are for debugging and may be +deleted in the future. + +\\fBEXAMPLE CONFIGURATION:\\fR + +An example simulation configuration for a plasma +torch height control by pid is provided at: +\\fBconfigs/sim/axis/external_offsets/hpid.ini\\fR. + +"""; + +//" quote char for vim highlighting + +pin in bit enable_in_a = 0 "Enable Input a"; +pin in bit enable_in_b = 0 "Enable Input b"; +pin in bit enable_in_c = 0 "Enable Input c"; +pin in float minimum_vel = 0 "units/sec (cornerlock aid)"; + +pin in float command = 0 "Command Input"; +pin in float feedback = 0 "Feedback Input"; + +pin in float pgain = 10 "Proportional gain"; +pin in float igain = 0 "Integral gain"; +pin in float dgain = 0.2 "Derivative gain"; +pin in bit hold_request = 0 "Hold request"; + +pin in u32 k = 10000 "Scaling Factor (if 0, use 10000)"; +pin in s32 fnum = 0 "Function selector (default 0 == PID)"; + +pin in bit active = 0 "From: \\fBmotion.eoffset-active\\fR"; +pin in bit is_on = 0 "From: \\fBhalui.machine.is-on\\fR"; +pin in float current_vel = 0 "From: \\fBmotion.current-vel\\fR"; + +pin out bit enable_out "To: \\fBaxis.L.eoffset-enable\\fR"; +pin out bit clear "To: \\fBaxis.L.eoffset-clear\\fR"; +pin out s32 kcounts "To: \\fBaxis.L.eoffset-counts\\fR"; +pin out float kreciprocal "To: \\fBaxis.L.eoffset-scale\\fR (1/k)"; + +// convenience pins (eliminate some external hal components) +pin out bit is_off "invert is_on (for convenience)"; + +// experimental (func1) +pin in float kptd = 0.5 "(experimental fnum=1)typ value 0.1-5"; +pin in float nfilt = 15 "(experimental fnum=1)typ value 5-30"; +pin in float olimit = 10000000 "(experimental output limit)"; + +pin out float dbg_eoffset "eoffset (use kcounts & kreciprocal)"; +pin out u32 dbg_state "state machine current state"; +pin out float dbg_error "command-request following error"; +pin out bit dbg_holding "holding current offset"; + +function _; +license "GPL"; +;; + +#include + +#define FINISH_DELAY 0 +#define TO_RAD M_PI/180 + +static int run_ct = 0; +static int delay_ct = 0; + + +static double periodrecip; +static double the_period; + +struct ofunc_data { + double p_gain; + double i_gain; + double d_gain; + double cmd; + double fb; + double prev_cmd; + double prev_fb; + + //computed items: + double err; + double err_d; + double err_i; + double ovalue; + + //experimental (func1) + double n_filt; + double kptd_gain; + double prev_diff; + double diff; +}; + +typedef int ofunc(struct ofunc_data*); +static ofunc func0; +static ofunc func1; + +typedef enum { + OFF, + READY, + STARTING, + RUNNING, + STOPPING, + FINISH, +} state; + +#define HPID_DEBUG +#undef HPID_DEBUG +#ifdef HPID_DEBUG + #define LVL RTAPI_MSG_INFO + #define dprint(msg,n) do { \ + rtapi_set_msg_level(LVL); \ + rtapi_print_msg(LVL,"%20s %5d\n",msg,n); \ + } while (0) +#else + #define dprint(msg,n) +#endif + +FUNCTION(_) { + static struct ofunc_data the_data; + struct ofunc_data* data = &the_data; + static state the_state = OFF; + static int do_once = 1; + static int messaged = 0; + static int err_stop = 0; + static int hold = 0; + static int old_hold = 0; + + double the_feedback; + int kfactor; + int enable_in; + ofunc* thefunc; + + if (do_once) { + the_period = period * 1e-9; + periodrecip = 1/the_period; + data->err = 0; + data->err_i = 0; + data->err_d = 0; + data->ovalue = 0; + data->diff = 0; // experimental func1: + data->diff = 0; // experimental func1: + do_once = 0; + } + + run_ct++; + if (k == 0) { + kfactor = 10000; + } else { + kfactor = k; + } + + kreciprocal = 1/((float)kfactor); + dbg_state = the_state; + is_off = !is_on; // convenience pin + the_feedback = feedback; + + if (is_off) { + // note: the external_offsets implementation defines + // axis.L.eoffset as zero when machine is off + err_stop = 0; + enable_out = 0; + clear = 0; + kcounts = 0; dbg_eoffset = 0; + messaged = 0; + the_state = OFF; + return; + } + + enable_in = enable_in_a && enable_in_b && enable_in_c; + + if ( hold_request + || ( (minimum_vel > 0) && (minimum_vel > current_vel) ) + ) { + hold = 1; + } else { + hold = 0; + } + dbg_holding = 0; //report only in some states + + switch (the_state) { + case OFF: + // require an enable_in 0-->1 transition to advance to READY + if (enable_in) { + if (!messaged) { + rtapi_print_msg(RTAPI_MSG_ERR, + "eoffset_pid: active enable-in not honored at start"); + messaged = 1; + } + return; + } + err_stop = 0; + messaged = 1; + kcounts = 0; + the_state = READY; + dprint("OFF->READY",kcounts); + return; + break; + case READY: + if (!enable_in) {return;} + kcounts = 0; dbg_eoffset = 0; + old_hold = 0; + enable_out = 0; + delay_ct = 0; + data->prev_cmd = 0; + data->prev_fb = 0; + data->prev_cmd = command; + data->prev_fb = command; + data->prev_diff = 0; //experminental func1 + the_state = STARTING; + dprint("READY->STARTING",kcounts); + return; + break; + case STARTING: + // hold not honored here in order to get to the next state + if (enable_in) { + enable_out = 1; + the_state = RUNNING; + dprint("STARTING->RUNNING",kcounts); + break; + } else { + enable_out = 0; + the_state = READY; + return; + } + case RUNNING: + dbg_holding = hold; // report as true this state only + if (enable_in) { + if (hold) { + old_hold = hold; + data->err = 0; + dbg_error = 0; + kcounts = 0; // expects integral action in inner loop + enable_out = 0; // relies on motion/control.c holding if: + // axis.L.eoffset-enable == 0 + the_state = RUNNING; + return; + } + if (!hold && old_hold) { + // end of hold interval + old_hold = hold; + the_feedback = command; + data->prev_cmd = command; + data->prev_fb = command; + data->err_i = 0; + data->prev_diff = 0; //experminental func1 + } + enable_out = 1; + the_state = RUNNING; + } else { + /* + ** When the enable_in pin is deasserted, kcounts are set to + ** zero and the simple trajectory planner removes the offset to + ** within its stopping criterion. Under some conditions, a + ** residual offset may remain. Connecting the clear pin to + ** axis.L.eoffset-clear forces the axis->ext_offset_tp.pos_cmd + ** to zero to remove any residual with no modifications to + ** simple_tp.c + */ + enable_out = 1; // if may have been holding + clear = 1; + kcounts = 0; dbg_eoffset = 0; + delay_ct = run_ct; + the_state = STOPPING; + dprint("RUNNING->STOPPING",kcounts); + return; + } + break; + case STOPPING: + if (active) { + the_state = STOPPING; + } else { + // !active ==> stopping criterion met + delay_ct = run_ct; + the_state = FINISH; + dprint("STOPPING->FINISH",kcounts); + } + return; + break; + case FINISH: + // provision for delay if needed + if (run_ct < (FINISH_DELAY + delay_ct) ) { + the_state = FINISH; + } else { + enable_out = 0; + if (err_stop) { + the_state = OFF; + dprint("FINISH->OFF",kcounts); + } else { + the_state = READY; + dprint("FINISH->READY",kcounts); + } + } + clear = 0; + return; + break; + } //switch (the_state) + + switch (fnum) { + case 0: thefunc = func0; break; + case 1: thefunc = func1; break; + default: thefunc = func0; break; + } + // current values: + data->p_gain = pgain; + data->i_gain = igain; + data->d_gain = dgain; + data->cmd = command; + data->fb = the_feedback; + data->n_filt = nfilt; // for experimental func1: + data->kptd_gain = kptd; // for experimental func1: + + // calculations: + if (thefunc(data) ) { + // thefunc returned nonzero (problem) + err_stop = 1; + data->ovalue = 0; + kcounts = 0; dbg_eoffset = 0; + the_state = STOPPING; + rtapi_print_msg(RTAPI_MSG_ERR, + "eoffset_pid: stopping:func%d problem\n",fnum); + return; + } + + // items for next iteration: + data->prev_cmd = command; + data->prev_fb = the_feedback; + data->prev_diff = data->diff; //experminental func1 + + // results: + if (data->ovalue > olimit) { data->ovalue = olimit; } + if (data->ovalue < -olimit) { data->ovalue = -olimit; } + kcounts = kfactor * data->ovalue; // offset counts + dbg_eoffset = kcounts * kreciprocal; // debug pin + dbg_error = data->err; // debug pin +} // _() + +static int func0 (struct ofunc_data* d) +{ + // PID + if ( (d->p_gain < 0) + || (d->i_gain < 0) + || (d->d_gain < 0) ) { + rtapi_print_msg(RTAPI_MSG_ERR, + "eoffset_pid: negative gain\n"); + return -1; + } + d->err = d->cmd - d->fb; + d->err_d = ( (d->cmd - d->fb) - (d->prev_cmd - d->prev_fb) ) + * periodrecip; + d->err_i += d->err * the_period; + d->ovalue = d->p_gain * d->err + + d->i_gain * d->err_i + + d->d_gain * d->err_d; + return 0; +} + +static int func1 (struct ofunc_data* d) +{ + // PID with backward difference + // and limit to derivative freq response + if ( (d->p_gain <= 0) // is divisor + || (d->i_gain < 0) + || (d->d_gain < 0) + || (d->kptd_gain < 0) + ) { + rtapi_print_msg(RTAPI_MSG_ERR, + "eoffset_pid: bad gain setting\n"); + return -1; + } + d->err = d->cmd - d->fb; + + //ref: Astrom 6.17: + d->diff = (d->kptd_gain/d->p_gain) + / (d->kptd_gain/d->p_gain + d->n_filt * the_period) + * ( d->prev_diff + - d->p_gain * d->n_filt * (d->fb - d->prev_fb) + ); + + d->err_i += d->err * the_period; + d->ovalue = d->p_gain * d->err + + d->i_gain * d->err_i + + d->diff; //gain term included + + return 0; +} diff --git a/tests/interp/compile/use-rs274.cc b/tests/interp/compile/use-rs274.cc index db36608fb2c..ae3b88659cb 100644 --- a/tests/interp/compile/use-rs274.cc +++ b/tests/interp/compile/use-rs274.cc @@ -250,3 +250,5 @@ void PLUGIN_CALL(int len, const char *call) {} void IO_PLUGIN_CALL(int len, const char *call) {} USER_DEFINED_FUNCTION_TYPE USER_DEFINED_FUNCTION[USER_DEFINED_FUNCTION_NUM]; +int GET_EXTERNAL_OFFSET_APPLIED() {}; +EmcPose GET_EXTERNAL_OFFSETS(){};