debounce probe pin and remove python2 use
This commit is contained in:
philipp
parent
750fc17fd7
commit
55d86b685a
18
3040T.hal
18
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
|
||||
|
||||
|
|
|
|||
12
3040T.ini
12
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<rectangle_probe> 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
|
||||
|
|
|
|||
185
compensation.py
185
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()
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -7,7 +7,7 @@ O<tool-change> 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 )
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@ o<touchoff-z-to-Bead> 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 )
|
||||
|
|
|
|||
Loading…
Reference in a new issue