Multiaxis kinematics and limits

.gitignore

@@ -21,6 +21,10 @@ share/desktop-directories/linuxcnc-cnc.directory
 share/desktop-directories/linuxcnc-ref.directory
 share/desktop-directories/linuxcnc-doc.directory
 share/linuxcnc/mesa_modbus.c.tmpl
+share/linuxcnc/switchkins.c
+share/linuxcnc/switchkins_setup.c
+share/linuxcnc/kins_util.c
+share/linuxcnc/kins_single.c
 src/modules.order
 /configs/*/emc.nml
 !/configs/common/emc.nml

configs/sim/axis/vismach/5axis/bridgemill/5axis.ini

@@ -4,8 +4,13 @@ MACHINE = Sim-5Axis Bridge Mill (xyzbcw)
   DEBUG = 0
 
 [DISPLAY]
-         GEOMETRY = XYZCBW
-        OPEN_FILE = ./5axisgui.ngc
+# GEOMETRY tells the AXIS live plot how to guess the tool position from
+# the nine axis values: letters translate, ABC rotate the point.  W is
+# deliberately absent: the plot would add the W value as a Z offset after
+# the rotations, while the kinematics spends W along the tool axis, so
+# the guess never matches.
+         GEOMETRY = XYZCB
+        OPEN_FILE = ./g532-fused-orient-move.ngc
        INCREMENTS = 10 mm, 1 mm, .1 mm
          JOG_AXES = XYZC
           DISPLAY = axis
@@ -41,7 +46,6 @@ CYCLE_TIME = 0.010
   HALUI = halui
 HALFILE = LIB:basic_sim.tcl
 HALFILE = 5axisgui.hal
-HALCMD  = net :kinstype-select <= motion.analog-out-03 => motion.switchkins-type
 POSTGUI_HALFILE = 5axis_postgui.hal
 
 [HALUI]

configs/sim/axis/vismach/5axis/bridgemill/5axisgui.ngc

@@ -1,3 +1,17 @@
+; 5axisgui.ngc - the historical demo program, unchanged.
+; It drills a sphere pattern with W words.  W was never a physical
+; quill: no motor is connected to its joint, the kinematics folds
+; the word into the XYZ slides, and the head sliding along the tool
+; axis is what plunges the rigidly mounted tool.  Watch the slides
+; in vismach do the stroke.
+;
+; The quirk, then and now: the controller keeps W as a separate
+; world coordinate, so the programmed XYZ point does not move and
+; the preview shows nothing of the stroke; only the W DRO tracks it.
+; Drilling the controller can see, check and preview is what the
+; tilted work plane family is for (g532-fused-orient-move.ngc,
+; g536-orient-then-move.ngc).
+
         #<r>  =   60 ; sphere radius
   #<rdelta>   =    5 ; safe distance
 #<drilldepth> =   -5
@@ -13,6 +27,7 @@
 #<bdelta> = [90/#<bctmax>]
 
 g49
+g13.1 ; kinematics 0 (TCP), in case a previous run left a switch active
 t#<toolno>m6g43
 
 g53 g0 x0y0z#<zstart>b0c0 w0

configs/sim/axis/vismach/5axis/bridgemill/README

@@ -1,19 +1,59 @@
-This is a simulation of an XYZBCWY 5 axis bridge mill.
+This is a simulation of an XYZBCWY 5 axis bridge mill with a
+tilting head (B, C) and a W axis.
 
-Example demo:
+W is not a physical axis of the machine and never was: no motor is
+connected to its joint.  The kinematics folds a W word into the
+XYZ joints, so the head slides along the tool axis and the
+rigidly mounted tool goes with it: that is the whole trick, and it
+is what vismach shows.  There is no quill to draw because there is
+no quill.
+
+The quirk: the controller keeps W as a separate world coordinate,
+so a W word leaves the programmed XYZ point untouched and the
+preview cannot show the stroke; the W DRO tracks it.  It also
+means the planner does not see the tool-axis motion, so for
+drilling the controller can check and preview, use the tilted work
+plane family.
+
+Because the W joint is virtual, a point-to-point move on it
+(g53.5/g53.7 j5=) moves no motor on a real machine while the
+controller believes the world moved, the opposite of a W word.
+Do not drill that way.
+
+Demo programs:
+
+ g532-fused-orient-move.ngc -- drill a sphere pattern with tilted work
+                             planes (g68.2), using g53.2 to solve each
+                             orientation without moving (STAY), then
+                             one fused g0 that turns the head and
+                             travels to the hole at the same time
+                             (TCP motion)
+ g536-orient-then-move.ngc -- the same pattern with g53.6 (Heidenhain
+                             MOVE style): reorient about the fixed
+                             tip, then travel in the tilted plane
+ 5axisgui.ngc --------------- the historical demo program, unchanged:
+                             it drills the same pattern with W words,
+                             the slides doing the stroke while the
+                             preview stays blind to it
+
+The tool table provides tool 100 (length 100).  Load it with
+t100m6g43 so the vismach tool and the kinematics pivot length
+(pivotsum: 250 + tool length) match.
+
+Example MDI:
 
  1) $ linuxcnc 5axis.ini
  2) F1 ---------- Estop off
     F2 ---------- Machine on
     CTRL-HOME --- home all
     F5 ---------- MDI tab
  3) orient vismach gui as required
- 4) g0w10   ; retract w
+ 4) g0w10   ; head slides up the tool axis, tip with it
  5) g43h100 ; tool offset (100)
  6) g0b45   ; tilt 45 deg wrt z
  7) g0c30   ; rotate 30 deg in xy
- 8) g0w-10  ; simulate drill
- 9) g0w10   ; retract drill
+ 8) g0w-10  ; head slides back, tip plunges 10 along the tool axis
+ 9) g0w10   ; and back
 10) etc
 
 Note: Motion for the W coordinate is incorporated

configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc

@@ -0,0 +1,50 @@
+; g532-fused-orient-move.ngc - drill a sphere pattern with tilted work planes:
+; g68.2 tilts the plane onto each hole normal, g53.2 solves the head
+; orientation without moving (STAY), and one fused g0 turns the head
+; and travels to the hole at the same time (TCP motion).  See README
+; for the other demos.
+
+        #<r>  =   60 ; sphere radius
+  #<rdelta>   =    5 ; safe distance
+#<drilldepth> =   -5
+   #<zsafe>   =   20 ; clearance above the ball for start and stop
+    #<bctmax> =    8
+    #<cctmax> =   16
+    #<toolno> =  100
+    #<frate>  = 1000
+
+#<bct>    = 0
+#<cct>    = 0
+#<cdelta> = [360/#<cctmax>]
+#<bdelta> = [90/#<bctmax>]
+
+g49
+g13.1 ; kinematics 0 (TCP), in case a previous run left a switch active
+t#<toolno>m6g43
+
+g53 g0 x0 y0 z0 b0 c0
+g10 l20 p0 x0 y0 z[#<r>+#<rdelta>+#<zsafe>] b0 c0 ; ball center at program origin, we park above it
+f#<frate>
+o100 while [#<bct> lt #<bctmax>]
+       #<b> = [[#<bctmax>-1-#<bct>]*#<bdelta>] ; top ring first, the way in stays outside the ball
+       #<cct> = 0
+o200   while [#<cct> lt #<cctmax>]
+o210      if [[#<bct> mod 2] eq 0]
+              #<c> = [#<cct> * #<cdelta>]
+o210      else
+              #<c> = [360 - [1+ #<cct>] * #<cdelta>]
+o210      endif
+          g68.2 p1 j[90-#<b>] k#<c>  ; plane Z is the sphere radius at b, c
+          g53.2                      ; solve the orientation, stay put
+          g0 x0 y0 z[#<r>+#<rdelta>] b#<_orient_b> c#<_orient_c> ; one TCP turn and travel
+          g1 z[#<r>+#<drilldepth>]
+          g0 z[#<r>+#<rdelta>]
+          #<cct> = [#<cct>+1]
+o200   endwhile
+       #<bct> = [#<bct>+1]
+o100 endwhile
+
+g69
+g53 g0 z0         ; up, clear of the ball
+g53 g0 x0 y0 b0 c0
+m2

configs/sim/axis/vismach/5axis/bridgemill/g536-orient-then-move.ngc

@@ -0,0 +1,49 @@
+; g536-orient-then-move.ngc - same sphere as g532-fused-orient-move.ngc, but with g53.6
+; (Heidenhain MOVE style): reorient about the fixed tip, a TCP move,
+; then travel to the next hole in the tilted plane with a separate g0.
+; g532-fused-orient-move.ngc fuses the turn and the travel into one move instead.
+
+        #<r>  =   60 ; sphere radius
+  #<rdelta>   =    5 ; safe distance
+#<drilldepth> =   -5
+   #<zsafe>   =   20 ; clearance above the ball for start and stop
+    #<bctmax> =    8
+    #<cctmax> =   16
+    #<toolno> =  100
+    #<frate>  = 1000
+
+#<bct>    = 0
+#<cct>    = 0
+#<cdelta> = [360/#<cctmax>]
+#<bdelta> = [90/#<bctmax>]
+
+g49
+g13.1 ; kinematics 0 (TCP), in case a previous run left a switch active
+t#<toolno>m6g43
+
+g53 g0 x0 y0 z0 b0 c0
+g10 l20 p0 x0 y0 z[#<r>+#<rdelta>+#<zsafe>] b0 c0 ; ball center at program origin, we park above it
+f#<frate>
+o100 while [#<bct> lt #<bctmax>]
+       #<b> = [[#<bctmax>-1-#<bct>]*#<bdelta>] ; top ring first, the way in stays outside the ball
+       #<cct> = 0
+o200   while [#<cct> lt #<cctmax>]
+o210      if [[#<bct> mod 2] eq 0]
+              #<c> = [#<cct> * #<cdelta>]
+o210      else
+              #<c> = [360 - [1+ #<cct>] * #<cdelta>]
+o210      endif
+          g68.2 p1 j[90-#<b>] k#<c>  ; plane Z is the sphere radius at b, c
+          g53.6                      ; reorient about the tip, a TCP move
+          g0 x0 y0 z[#<r>+#<rdelta>]
+          g1 z[#<r>+#<drilldepth>]
+          g0 z[#<r>+#<rdelta>]
+          #<cct> = [#<cct>+1]
+o200   endwhile
+       #<bct> = [#<bct>+1]
+o100 endwhile
+
+g69
+g53 g0 z0         ; up, clear of the ball
+g53 g0 x0 y0 b0 c0
+m2

configs/sim/axis/vismach/5axis/bridgemill/remap_subs/428remap.ngc

@@ -1,24 +1,13 @@
 ;M428 by remap: select kinstype=0 genhexkins
 o<428remap>sub
   #<kinstype> = 0
-  #<SWITCHKINS_PIN> = 3  ; set N as required: motion.analog-out-0N
 
-o1 if [exists [#<_hal[motion.switchkins-type]>]]
-o1 else
-     (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1)
-     (debug,STOP)
-     M2
-o1 endif
-
-   M68  E#<SWITCHKINS_PIN> Q#<kinstype> ; set kinstype value
-   M66  E0 L0                           ; force synch
+   G13.1                                ; back to kinstype 0, syncs interp and motion
 
-o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #<kinstype>]]
-     (debug,M428: Wrong motion.switchkins-type)
-     (debug,or missing hal net to analog-out-0x)
+o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #<kinstype>]]
+     (debug,M428: Wrong kinematics type)
      (debug,STOP)
      M2
-o2 else
 o2 endif
 
 o<428remap>endsub

configs/sim/axis/vismach/5axis/bridgemill/remap_subs/429remap.ngc

@@ -1,24 +1,13 @@
 ;M429 by remap: select kinstype==1 Identity kinematics
 o<429remap>sub
   #<kinstype> = 1
-  #<SWITCHKINS_PIN> = 3  ; set N as required: motion.analog-out-0N
 
-o1 if [exists [#<_hal[motion.switchkins-type]>]]
-o1 else
-     (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1)
-     (debug,STOP)
-     M2
-o1 endif
-
-   M68  E#<SWITCHKINS_PIN> Q#<kinstype> ; set kinstype value
-   M66  E0 L0                           ; force synch
+   G12.1 P#<kinstype>                   ; select kinstype, syncs interp and motion
 
-o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #<kinstype>]]
-     (debug,M429:Wrong motion.switchkins-type)
-     (debug,or missing hal net to analog-out-0x)
+o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #<kinstype>]]
+     (debug,M429:Wrong kinematics type)
      (debug,STOP)
      M2
-o2 else
 o2 endif
 
 o<429remap>endsub

configs/sim/axis/vismach/5axis/bridgemill/remap_subs/430remap.ngc

@@ -1,24 +1,13 @@
 ;M430 by remap: select kinstype==2 userk kins
 o<430remap>sub
   #<kinstype> = 2
-  #<SWITCHKINS_PIN> = 3  ; set N as required: motion.analog-out-0N
 
-o1 if [exists [#<_hal[motion.switchkins-type]>]]
-o1 else
-     (debug,M30:Missing [RS274NGC]HAL_PIN_VARS=1)
-     (debug,STOP)
-     M2
-o1 endif
-
-   M68  E#<SWITCHKINS_PIN> Q#<kinstype> ; set kinstype value
-   M66  E0 L0                           ; force synch
+   G12.1 P#<kinstype>                   ; select kinstype, syncs interp and motion
 
-o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #<kinstype>]]
-     (debug,M430:Wrong motion.switchkins-type)
-     (debug,or missing hal net to analog-out-0x)
+o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #<kinstype>]]
+     (debug,M430:Wrong kinematics type)
      (debug,STOP)
      M2
-o2 else
 o2 endif
 
 o<430remap>endsub

configs/sim/axis/vismach/5axis/table-dual-rotary/README

@@ -28,9 +28,6 @@ For proper tool-path preview RELOAD THE CGODE after startup and after changing o
 ***********************************************
 Note: IMPORTANT ini file requirements:
 
-[HAL]
-HALCMD = net :kinstype-select <= motion.analog-out-0N => motion.switchkins-type
-
 [RS274NGC]
 SUBROUTINE_PATH = ./remap_subs
           REMAP = M428  modalgroup=10  ngc=428remap

configs/sim/axis/vismach/5axis/table-dual-rotary/remap_subs/428remap.ngc

@@ -1,24 +1,13 @@
-;M428 by remap: kinstype==1 (xyzac,xyzbc) (note: sparm=identityfirst)
+;M428 by remap: kinstype==1 (xyzab-tdr kinematics)
 o<428remap>sub
-  #<kinstype> = 1        ; xyzac,xyzbc
-  #<SWITCHKINS_PIN> = 3  ; set N as required: motion.analog-out-0N
+  #<kinstype> = 1        ; xyzab-tdr
 
-o1 if [exists [#<_hal[motion.switchkins-type]>]]
-o1 else
-     (debug,M428:Missing [RS274NGC]HAL_PIN_VARS=1)
-     (debug,STOP)
-     M2
-o1 endif
-
-   M68  E#<SWITCHKINS_PIN> Q#<kinstype> ; set kinstype value
-   M66  E0 L0                           ; force synch
+   G12.1 P#<kinstype>                   ; select kinstype, syncs interp and motion
 
-o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #<kinstype>]]
-     (debug,M428: Wrong motion.switchkins-type)
-     (debug,or missing hal net to analog-out-0x)
+o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #<kinstype>]]
+     (debug,M428: Wrong kinematics type)
      (debug,STOP)
      M2
-o2 else
 o2 endif
 
 o<428remap>endsub

configs/sim/axis/vismach/5axis/table-dual-rotary/remap_subs/429remap.ngc

@@ -1,24 +1,13 @@
 ;M429 by remap: kinstype==0 Identity kinematics
 o<429remap>sub
   #<kinstype> = 0
-  #<SWITCHKINS_PIN> = 3  ; set N as required: motion.analog-out-0N
 
-o1 if [exists [#<_hal[motion.switchkins-type]>]]
-o1 else
-     (debug,M429:Missing [RS274NGC]HAL_PIN_VARS=1)
-     (debug,STOP)
-     M2
-o1 endif
-
-   M68  E#<SWITCHKINS_PIN> Q#<kinstype> ; set kinstype value
-   M66  E0 L0                           ; force synch
+   G13.1                                ; back to identity kinematics, syncs interp and motion
 
-o2 if [[#<_task> EQ 1] AND [#<_hal[motion.switchkins-type]> NE #<kinstype>]]
-     (debug,M429:Wrong motion.switchkins-type)
-     (debug,or missing hal net to analog-out-0x)
+o2 if [[#<_task> EQ 1] AND [#<_kins_type> NE #<kinstype>]]
+     (debug,M429: Wrong kinematics type)
      (debug,STOP)
      M2
-o2 else
 o2 endif
 
 o<429remap>endsub