From 55d86b685a1b364ae3edb4d708b5d72afd3e0bb5 Mon Sep 17 00:00:00 2001 From: philipp Date: Tue, 8 Jul 2025 09:47:25 +0200 Subject: [PATCH] debounce probe pin and remove python2 use --- 3040T.hal | 18 ++-- 3040T.ini | 12 +-- compensation.py | 185 ++++++++++++++++++++--------------------- custom_postgui.hal | 16 ++-- tool-change.ngc | 3 +- touchoff-z-to-bead.ngc | 3 +- 6 files changed, 126 insertions(+), 111 deletions(-) diff --git a/3040T.hal b/3040T.hal index 4f1ae78..a431985 100644 --- a/3040T.hal +++ b/3040T.hal @@ -9,22 +9,24 @@ loadrt timedelay count=2 loadrt or2 count=3 loadrt xor2 count=2 loadrt mux_generic config="bb2" +loadrt dsmono count=1 +loadrt timedelta count=1 addf parport.0.read base-thread addf stepgen.make-pulses base-thread addf parport.0.write base-thread addf parport.0.reset base-thread addf mux-gen.00 base-thread +addf timedelta.0 base-thread addf stepgen.capture-position servo-thread addf motion-command-handler servo-thread addf motion-controller servo-thread addf stepgen.update-freq servo-thread +addf dsmono.0 servo-thread #Pin Assignments -net probe-in => motion.probe-input - net estop-out => parport.0.pin-01-out net xstep => parport.0.pin-02-out setp parport.0.pin-02-out-reset 1 @@ -43,7 +45,13 @@ net estop-ext <= parport.0.pin-10-in net min-home-x <= parport.0.pin-11-in-not net min-home-y <= parport.0.pin-12-in-not net max-home-z <= parport.0.pin-13-in-not -net probe-in <= parport.0.pin-15-in-not + +#Probe +setp dsmono.0.deadtime-ns 50000000 +net probe-in <= parport.0.pin-15-in-not +net probe-in => dsmono.0.in +net probe-debounced <= dsmono.0.out +net probe-debounced => motion.probe-input #X-Axis setp stepgen.0.position-scale [JOINT_0]SCALE @@ -139,6 +147,7 @@ net amp-enable halui.machine.is-on => cncextension.wr0 #Main Spindle Control net spindle-main-fwd spindle.0.forward net spindle-main-fwd => cncextension.ch0 +net spindle-main-fwd => cncextension.ch2 net spindle-main-fwd => cncextension.opt addf timedelay.0 servo-thread @@ -210,7 +219,7 @@ net zpos-abs-cmd joint.2.pos-cmd => collisionavoid.zaxis net collision-stop collisionavoid.stop => halui.estop.activate #Autolevel -loadusr -Wn compensation python2 compensation.py probe-results.txt cubic +loadusr -Wn compensation python3 compensation.py probe-results.txt cubic net xpos-abs-cmd => compensation.x-pos net ypos-abs-cmd => compensation.y-pos net zpos-abs-cmd => compensation.z-pos @@ -218,4 +227,3 @@ net eoffset-enable compensation.enable-out => axis.z.eoffset-enable net eoffset-scale compensation.scale => axis.z.eoffset-scale net eoffset-counts compensation.counts => axis.z.eoffset-counts net eoffset-clear compensation.clear => axis.z.eoffset-clear - diff --git a/3040T.ini b/3040T.ini index 909412b..f73c25f 100644 --- a/3040T.ini +++ b/3040T.ini @@ -52,7 +52,7 @@ FEATURES=12 [EMCMOT] EMCMOT = motmod COMM_TIMEOUT = 1.0 -BASE_PERIOD = 35000 +BASE_PERIOD = 45000 SERVO_PERIOD = 1000000 [TASK] @@ -69,8 +69,9 @@ MDI_COMMAND = M600 MDI_COMMAND = G0 X0 Y0 MDI_COMMAND = G53 G0 X0 Y0 Z0 MDI_COMMAND = G53 g0 z0 +MDI_COMMAND = G53 g0 z-40 MDI_COMMAND = G10 L20 p0 x0 y0 z0 -MDI_COMMAND = G10 L20 p0 x36.575 y-38.364 +MDI_COMMAND = G10 L20 p0 x-36.257 y-49.488 MDI_COMMAND = G10 L2 p0 x261.3 y286.9 MDI_COMMAND = M400 MDI_COMMAND = o call @@ -109,7 +110,6 @@ STEPGEN_MAXACCEL = 1875.0 SCALE = -400.0 FERROR = 1 MIN_FERROR = .25 -MIN_FERROR = 10.25 MIN_LIMIT = -0.001 MAX_LIMIT = 290.0 HOME_OFFSET = -1.000000 @@ -146,15 +146,15 @@ HOME_SEQUENCE = 1 MIN_LIMIT = -75 MAX_LIMIT = 0.001 MAX_VELOCITY = 50 -MAX_ACCELERATION = 2000.0 +MAX_ACCELERATION = 1500.0 OFFSET_AV_RATIO = 0.2 [JOINT_2] TYPE = LINEAR HOME = 0.0 MAX_VELOCITY = 50 -MAX_ACCELERATION = 2000.0 -STEPGEN_MAXACCEL = 2500.0 +MAX_ACCELERATION = 1500.0 +STEPGEN_MAXACCEL = 1875.0 SCALE = -400.0 FERROR = 1 MIN_FERROR = .25 diff --git a/compensation.py b/compensation.py index 1438840..fb3a885 100644 --- a/compensation.py +++ b/compensation.py @@ -1,5 +1,6 @@ #!/usr/bin/env python2 """Copyright (C) 2020 Scott Alford, scottalford75@gmail.com +Copyright (C) 2022-2024 Carl Klemm, carl@uvos.xyz This program is free software; you can redistribute it and/or modify it under the terms of the GNU 2 General Public License as published by @@ -16,47 +17,50 @@ along with this program; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA """ -update = 0.025 # this is how often the z external offset value is updated based on current x & y position -safeOffset = 20 - import sys -import os.path, time +import os.path import numpy as np from scipy.interpolate import griddata from enum import Enum, unique - import linuxcnc +import hal +import time + +# this is how often the z external offset value is updated based on current x & y position +update = 0.025 +safeOffset = 20 + @unique class States(Enum): - START = 1 - IDLE = 2 - LOADMAP = 3 - RUNNING = 4 - RESET = 5 - STOP = 6 + START = 1 + IDLE = 2 + LOADMAP = 3 + RUNNING = 4 + RESET = 5 + STOP = 6 -class Compensation : - def __init__(self) : +class Compensation: + def __init__(self): self.comp = {} - if len(sys.argv)<2: - print "ERROR! No input file name specified!" + if len(sys.argv) < 2: + print("ERROR! No input file name specified!") sys.exit() self.filename = sys.argv[1] self.method = sys.argv[2] - + # default to cubic if not specified - if self.method == "" : self.methond = "cubic" + if self.method == "": + self.methond = "cubic" - - def loadMap(self) : + def loadMap(self): # data coordinates and values self.data = np.loadtxt(self.filename, dtype=float, delimiter=" ", usecols=(0, 1, 2)) - self.x_data = np.around(self.data[:,0],1) - self.y_data = np.around(self.data[:,1],1) - self.z_data = self.data[:,2] + self.x_data = np.around(self.data[:, 0], 1) + self.y_data = np.around(self.data[:, 1], 1) + self.z_data = self.data[:, 2] # get the x and y, min and max values from the data self.xMin = int(np.min(self.x_data)) @@ -64,51 +68,45 @@ class Compensation : self.yMin = int(np.min(self.y_data)) self.yMax = int(np.max(self.y_data)) - print " xMin = ", self.xMin - print " xMax = ", self.xMax - print " yMin = ", self.yMin - print " yMax = ", self.yMax + print(" xMin = ", self.xMin) + print(" xMax = ", self.xMax) + print(" yMin = ", self.yMin) + print(" yMax = ", self.yMax) # target grid to interpolate to, 1 grid per mm - self.xSteps = (self.xMax-self.xMin)+1 - self.ySteps = (self.yMax-self.yMin)+1 + self.xSteps = (self.xMax - self.xMin) + 1 + self.ySteps = (self.yMax - self.yMin) + 1 self.x = np.linspace(self.xMin, self.xMax, self.xSteps) self.y = np.linspace(self.yMin, self.yMax, self.ySteps) - self.xi,self.yi = np.meshgrid(self.x,self.y) - print " xSteps = ", self.xSteps - print " ySteps = ", self.ySteps - print " x = ", self.x + self.xi, self.yi = np.meshgrid(self.x, self.y) + print(" xSteps = ", self.xSteps) + print(" ySteps = ", self.ySteps) + print(" x = ", self.x) # interpolate, zi has all the offset values but need to be transposed - self.zi = griddata((self.x_data,self.y_data),self.z_data,(self.xi,self.yi),method=self.method) + self.zi = griddata((self.x_data, self.y_data), self.z_data, (self.xi, self.yi), method=self.method) self.zi = np.transpose(self.zi) - - def compensate(self) : + def compensate(self): # get our nearest integer position self.xpos = int(round(self.h['x-pos'])) self.ypos = int(round(self.h['y-pos'])) - + zo = safeOffset - - if self.xpos >= self.xMin and self.xpos <= self.xMax and self.ypos >= self.yMin and self.ypos <= self.yMax : + + if self.xpos >= self.xMin and self.xpos <= self.xMax and self.ypos >= self.yMin and self.ypos <= self.yMax: self.Xn = self.xpos - self.xMin self.Yn = self.ypos - self.yMin - - # get the nearest compensation offset and convert to counts (s32) with a scale (float) - # Requested offset == counts * scale - zo = self.zi[self.Xn,self.Yn] - if np.isnan(zo) : + + zo = self.zi[self.Xn, self.Yn] + if np.isnan(zo): zo = safeOffset self.scale = 0.001 compensation = int(zo / self.scale) return compensation - - def run(self) : - import hal, time - + def run(self): self.h = hal.component("compensation") self.h.newpin("enable-in", hal.HAL_BIT, hal.HAL_IN) self.h.newpin("enable-out", hal.HAL_BIT, hal.HAL_OUT) @@ -120,113 +118,114 @@ class Compensation : self.h.newpin("z-pos", hal.HAL_FLOAT, hal.HAL_IN) self.h.newpin("fade-height", hal.HAL_FLOAT, hal.HAL_IN) self.h.ready() - + s = linuxcnc.stat() - + currentState = States.START prevState = States.STOP try: while True: time.sleep(update) - + # get linuxcnc task_state status for machine on / off transitions s.poll() - - if currentState == States.START : - if currentState != prevState : + + if currentState == States.START: + if currentState != prevState: print("\nCompensation entering START state") prevState = currentState - + # do start-up tasks print(" %s last modified: %s" % (self.filename, time.ctime(os.path.getmtime(self.filename)))) - + prevMapTime = 0 - + self.h["counts"] = 0 - + # transition to IDLE state currentState = States.IDLE - - elif currentState == States.IDLE : - if currentState != prevState : + + elif currentState == States.IDLE: + if currentState != prevState: print("\nCompensation entering IDLE state") prevState = currentState - + # stay in IDLE state until compensation is enabled - if self.h["enable-in"] : + if self.h["enable-in"]: currentState = States.LOADMAP - - elif currentState == States.LOADMAP : - if currentState != prevState : + + elif currentState == States.LOADMAP: + if currentState != prevState: print("\nCompensation entering LOADMAP state") prevState = currentState - + mapTime = os.path.getmtime(self.filename) - + if mapTime != prevMapTime: self.loadMap() print(" Compensation map loaded") prevMapTime = mapTime - + # transition to RUNNING state currentState = States.RUNNING - - elif currentState == States.RUNNING : - if currentState != prevState : + + elif currentState == States.RUNNING: + if currentState != prevState: print("\nCompensation entering RUNNING state") prevState = currentState - - if self.h["enable-in"] : + + if self.h["enable-in"]: # enable external offsets self.h["enable-out"] = 1 - + fadeHeight = self.h["fade-height"] zPos = self.h["z-pos"] - - if fadeHeight == 0 : + + if fadeHeight == 0: compScale = 1 elif zPos < fadeHeight: - compScale = (fadeHeight - zPos)/fadeHeight - if compScale > 1 : + compScale = (fadeHeight - zPos) / fadeHeight + if compScale > 1: compScale = 1 - else : + else: compScale = 0 - - if s.task_state == linuxcnc.STATE_ON : + + if s.task_state == linuxcnc.STATE_ON: # get the compensation if machine power is on, else set to 0 - # otherwise we loose compensation eoffset if machine power is cycled + # otherwise we loose compensation eoffset if machine power is cycled # when copensation is enable compensation = self.compensate() self.h["counts"] = compensation * compScale self.h["scale"] = self.scale - else : + else: self.h["counts"] = 0 - - else : + + else: # transition to RESET state currentState = States.RESET - - elif currentState == States.RESET : - if currentState != prevState : + + elif currentState == States.RESET: + if currentState != prevState: print("\nCompensation entering RESET state") prevState = currentState # reset the eoffsets counts register so we don't accumulate self.h["counts"] = 0 # toggle the clear output - self.h["clear"] = 1; + self.h["clear"] = 1 time.sleep(0.1) - self.h["clear"] = 0; + self.h["clear"] = 0 # disable external offsets - #self.h["enable-out"] = 0 - + # self.h["enable-out"] = 0 + # transition to IDLE state currentState = States.IDLE except KeyboardInterrupt: - raise SystemExit + raise SystemExit + comp = Compensation() comp.run() diff --git a/custom_postgui.hal b/custom_postgui.hal index 5e14e50..818bbd3 100644 --- a/custom_postgui.hal +++ b/custom_postgui.hal @@ -30,10 +30,11 @@ net m600 halui.mdi-command-00 <= pyvcp.m600-btn net g0x0y0 halui.mdi-command-01 <= pyvcp.x0y0-btn net g53g0x0y0z0 halui.mdi-command-02 <= pyvcp.g53g0x0y0z0-btn net g53g0z0 halui.mdi-command-03 <= pyvcp.g53g0z0-btn -net g10l20x0y0z0 halui.mdi-command-04 <= or2.2.out -net g10l20x20y20 halui.mdi-command-05 <= pyvcp.microscope-btn -net g53g10l20pin halui.mdi-command-06 <= pyvcp.touchoffpin-btn -net touchofzpin halui.mdi-command-07 <= pyvcp.touchofz-btn +net microscopeHightBtn halui.mdi-command-04 <= pyvcp.g53g0z-40-btn +net g10l20x0y0z0 halui.mdi-command-05 <= or2.2.out +net g10l20x20y20 halui.mdi-command-06 <= pyvcp.microscope-btn +net g53g10l20pin halui.mdi-command-07 <= pyvcp.touchoffpin-btn +net touchofzpin halui.mdi-command-08 <= pyvcp.touchofz-btn #Collisionavoid @@ -43,10 +44,15 @@ setp pyvcp.chuck-chkbtn.changepin 1 #Autolevel net levelpin pyvcp.level-chkbtn => compensation.enable-in -net runalprobe halui.mdi-command-08 <= pyvcp.alprobe-btn +net runalprobe halui.mdi-command-09 <= pyvcp.alprobe-btn #Autolevel spinbox display net alprsx pyvcp.alprsx => pyvcp.alprsxDisp net alprsy pyvcp.alprsy => pyvcp.alprsyDisp net alintervl pyvcp.alintervl => pyvcp.alintervlDisp net alheight pyvcp.alheight => pyvcp.alheightDisp + +net jitter <= timedelta.0.jitter +net jitter => pyvcp.jitter-disp +net jitter-reset <= pyvcp.jitter-rst-btn +net jitter-reset => timedelta.0.reset diff --git a/tool-change.ngc b/tool-change.ngc index 33cd7cd..453c365 100644 --- a/tool-change.ngc +++ b/tool-change.ngc @@ -7,7 +7,7 @@ O SUB #<_ProbeY> = 24.5 ( machine Y coordinate of switch/touch-off plate ) #<_ProbeFastZ> = -20 ( machine Z coord to move to before starting probe, longest tool should not touch switch at this Z ) #<_ProbeMinZ> = -70 ( machine Z coord to stop probe, shortest tool must touch switch at this Z, must be > min Z ) -#<_ProbeRetract> = 0.2 ( small distance to retract before approaching switch/touch-off plate second time ) +#<_ProbeRetract> = 0.5 ( small distance to retract before approaching switch/touch-off plate second time ) #<_ProbeFastFeed> = 3000 ( feed rate for moving to _ProbeFastZ ) #<_ProbeFeed1> = 750 ( feed rate for touching switch/touch-off plate first time ) #<_ProbeFeed2> = 3 ( feed rate for touching switch/touch-off plate second time ) @@ -75,6 +75,7 @@ O102 ELSE M[7 + #<_MistOnDuringProbe> - 1] ( turn on mist/coolant ) O104 ENDIF G38.2 Z[#<_ProbeMinZ> - #<_ProbeFastZ>] F[#<_ProbeFeed1>] ( trip switch slowly ) + G4 P0.1 G0 Z[#<_ProbeRetract>] ( go up slightly ) G38.2 Z[#<_ProbeRetract>*-1.25] F[#<_ProbeFeed2>] ( trip switch very slowly ) M9 ( turn off mist ) diff --git a/touchoff-z-to-bead.ngc b/touchoff-z-to-bead.ngc index 283c1a1..1830be8 100644 --- a/touchoff-z-to-bead.ngc +++ b/touchoff-z-to-bead.ngc @@ -4,7 +4,7 @@ o sub #<_ProbeY> = 24.5 ( machine Y coordinate of switch/touch-off plate ) #<_ProbeFastZ> = -20 ( machine Z coord to move to before starting probe, longest tool should not touch switch at this Z ) #<_ProbeMinZ> = -70 ( machine Z coord to stop probe, shortest tool must touch switch at this Z, must be > min Z ) -#<_ProbeRetract> = 0.2 ( small distance to retract before approaching switch/touch-off plate second time ) +#<_ProbeRetract> = 0.5 ( small distance to retract before approaching switch/touch-off plate second time ) #<_ProbeFastFeed> = 3000 ( feed rate for moving to _ProbeFastZ ) #<_ProbeFeed1> = 750 ( feed rate for touching switch/touch-off plate first time ) #<_ProbeFeed2> = 3 ( feed rate for touching switch/touch-off plate second time ) @@ -25,6 +25,7 @@ G53 G1 F[#<_ProbeFastFeed>] Z[#<_ProbeFastZ>] ( move tool closer to switch -- w G54 G1 F[#<_ProbeFeed1>] G91 ( use relative positioning ) G38.2 Z[#<_ProbeMinZ> - #<_ProbeFastZ>] F[#<_ProbeFeed1>] ( trip switch slowly ) +G4 P0.1 G0 Z[#<_ProbeRetract>] ( go up slightly ) G38.2 Z[#<_ProbeRetract>*-1.25] F[#<_ProbeFeed2>] ( trip switch very slowly ) G90 ( use absolute positioning )