Import LinuxCNC source tree
This commit is contained in:
Wang Dequan
285
lib/python/linuxcnc_util.py
Normal file
285
lib/python/linuxcnc_util.py
Normal file
@@ -0,0 +1,285 @@
|
||||
"""
|
||||
LinuxCNC User Interface helper functions
|
||||
"""
|
||||
|
||||
import linuxcnc
|
||||
|
||||
import sys
|
||||
import time
|
||||
import math
|
||||
|
||||
|
||||
class LinuxCNC_Exception(Exception):
|
||||
pass
|
||||
|
||||
|
||||
class LinuxCNC:
|
||||
"""
|
||||
This class implements the main interface to LinuxCNC.
|
||||
"""
|
||||
|
||||
def __init__(self, command=None, status=None, error=None):
|
||||
"""init docs"""
|
||||
self.command = command
|
||||
self.status = status
|
||||
self.error = error
|
||||
|
||||
if not self.command:
|
||||
self.command = linuxcnc.command()
|
||||
|
||||
if not self.status:
|
||||
self.status = linuxcnc.stat()
|
||||
|
||||
if not self.error:
|
||||
self.error = linuxcnc.error_channel()
|
||||
|
||||
|
||||
def wait_for_linuxcnc_startup(self, timeout=10.0):
|
||||
|
||||
"""Poll the Status buffer waiting for it to look initialized,
|
||||
rather than just allocated (all-zero). Returns on success, throws
|
||||
RuntimeError on failure."""
|
||||
|
||||
start_time = time.time()
|
||||
while time.time() - start_time < timeout:
|
||||
self.status.poll()
|
||||
if (self.status.angular_units == 0.0) \
|
||||
or (self.status.axis_mask == 0) \
|
||||
or (self.status.cycle_time == 0.0) \
|
||||
or (self.status.exec_state != linuxcnc.EXEC_DONE) \
|
||||
or (self.status.interp_state != linuxcnc.INTERP_IDLE) \
|
||||
or (self.status.inpos == False) \
|
||||
or (self.status.linear_units == 0.0) \
|
||||
or (self.status.max_acceleration == 0.0) \
|
||||
or (self.status.max_velocity == 0.0) \
|
||||
or (self.status.program_units == 0.0) \
|
||||
or (self.status.rapidrate == 0.0) \
|
||||
or (self.status.state != linuxcnc.RCS_DONE) \
|
||||
or (self.status.task_state != linuxcnc.STATE_ESTOP):
|
||||
time.sleep(0.1)
|
||||
else:
|
||||
# looks good
|
||||
return
|
||||
|
||||
# timeout, throw an exception
|
||||
raise RuntimeError
|
||||
|
||||
|
||||
def all_joints_homed(self, joints):
|
||||
"""Arguments:
|
||||
|
||||
'joints' is an array of booleans, indicating the joints to
|
||||
check for homed-ness.
|
||||
|
||||
Returns True if all the specified joints are homed, False if
|
||||
any are unhomed."""
|
||||
|
||||
self.status.poll()
|
||||
for i in range(0,9):
|
||||
if joints[i] and not self.status.homed[i]:
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def wait_for_home(self, joints, timeout=10.0):
|
||||
"""Arguments:
|
||||
|
||||
'joints' is an array of booleans, indicating the joints that
|
||||
are homing.
|
||||
|
||||
'timeout' is a float, the number of seconds to wait for
|
||||
homing before giving up.
|
||||
|
||||
Returns if all the specified joints homed before the timeout,
|
||||
raises LinuxCNC_Exception if the timeout expired first."""
|
||||
|
||||
start_time = time.time()
|
||||
while (time.time() - start_time) < timeout:
|
||||
if self.all_joints_homed(joints):
|
||||
return
|
||||
time.sleep(0.1)
|
||||
|
||||
raise LinuxCNC_Exception("timeout waiting for homing to complete:\nstatus.homed:" + str(self.status.homed) + "\nstatus.position:" + str(self.status.position))
|
||||
|
||||
|
||||
def wait_for_axis_to_stop(self, axis_letter, timeout=10.0):
|
||||
"""Arguments:
|
||||
'axis_letter' is a single character from the set [xyzabcuvw]
|
||||
indicating the axis to wait for stillness on.
|
||||
|
||||
'timeout' is a float, the number of seconds to wait for the
|
||||
axis to stop before giving up.
|
||||
|
||||
Returns if the specified axis stops moving before the timeout
|
||||
expires. Raises LinuxCNC_Exception if the timeout expires before
|
||||
the axis stops.
|
||||
"""
|
||||
|
||||
axis_letter = axis_letter.lower()
|
||||
axis_index = 'xyzabcuvw'.index(axis_letter)
|
||||
print("waiting for axis", axis_letter, "to stop")
|
||||
self.status.poll()
|
||||
start_time = time.time()
|
||||
prev_pos = self.status.position[axis_index]
|
||||
while (time.time() - start_time) < timeout:
|
||||
time.sleep(0.1)
|
||||
self.status.poll()
|
||||
new_pos = self.status.position[axis_index]
|
||||
if new_pos == prev_pos:
|
||||
return
|
||||
prev_pos = new_pos
|
||||
raise LinuxCNC_Exception("axis %s didn't stop jogging!\n" % axis_letter + "axis %s is at %.3f %.3f seconds after reaching target (prev_pos=%.3f)\n" % (axis_letter, self.status.position[axis_index], timeout, prev_pos))
|
||||
|
||||
|
||||
def jog_axis(self, axis_letter, target, vel=5.0, timeout=10.0):
|
||||
"""Arguments:
|
||||
|
||||
'axis_letter' is a single character from the set [xyzabcuvw]
|
||||
indicating an axis.
|
||||
|
||||
'target' is a float indicating the location to jog to.
|
||||
|
||||
'vel' is a float indicating the jog speed.
|
||||
|
||||
'timeout' is a float, the number of seconds to wait for the
|
||||
jog to reach the target before giving up.
|
||||
|
||||
This function uses the linuxcnc.command.jog() function to do
|
||||
a continuous jog of the specified axis towards the target, at
|
||||
the specified speed. Stops the jog when the target is reached.
|
||||
Will overshoot some. The function returns after the axis has
|
||||
stopped moving.
|
||||
|
||||
If the axis does not reach the target before the timeout, or if
|
||||
any other axis moved, raises LinuxCNC_Exeption.
|
||||
"""
|
||||
|
||||
self.status.poll()
|
||||
axis_letter = axis_letter.lower()
|
||||
axis_index = 'xyzabcuvw'.index(axis_letter)
|
||||
start_pos = self.status.position
|
||||
|
||||
print("jogging axis %s from %.3f to %.3f" % (axis_letter, start_pos[axis_index], target))
|
||||
|
||||
if self.status.position[axis_index] < target:
|
||||
vel = abs(vel)
|
||||
done = lambda pos: pos > target
|
||||
else:
|
||||
vel = -1.0 * abs(vel)
|
||||
done = lambda pos: pos < target
|
||||
|
||||
# the 0 here means "jog axis, not joint"
|
||||
self.command.jog(linuxcnc.JOG_CONTINUOUS, 0, axis_index, vel)
|
||||
|
||||
start = time.time()
|
||||
while not done(self.status.position[axis_index]) and ((time.time() - start) < timeout):
|
||||
time.sleep(0.1)
|
||||
self.status.poll()
|
||||
|
||||
# the 0 here means "jog axis, not joint"
|
||||
self.command.jog(linuxcnc.JOG_STOP, 0, axis_index)
|
||||
|
||||
if not done(self.status.position[axis_index]):
|
||||
raise LinuxCNC_Exception("failed to jog axis %s to %.3f\n" % (axis_letter, target) + "timed out at %.3f after %.3f seconds" % (self.status.position[axis_index], timeout))
|
||||
|
||||
print(" jogged axis %d past target %.3f" % (axis_index, target))
|
||||
|
||||
self.wait_for_axis_to_stop(axis_letter)
|
||||
|
||||
success = True
|
||||
for i in range(0, 9):
|
||||
if i == axis_index:
|
||||
continue;
|
||||
if start_pos[i] != self.status.position[i]:
|
||||
raise LinuxCNC_Exception("axis %s moved from %.3f to %.3f but should not have!" % ('xyzabcuvw'[i], start_pos[i], self.status.position[i]))
|
||||
|
||||
|
||||
def wait_for_axis_to_stop_at(self, axis_letter, target, timeout=10.0, tolerance = 0.0001):
|
||||
"""
|
||||
Arguments:
|
||||
'axis_letter' is a single character from the set [xyzabcuvw]
|
||||
indicating the axis to wait for stillness on.
|
||||
|
||||
'target' is a float, where the specified axis is trying to
|
||||
move to.
|
||||
|
||||
'timeout' is a float, the number of seconds to wait for the
|
||||
axis to stop before giving up.
|
||||
|
||||
'tolerance' is a float, how close to the target the axis
|
||||
has to be before we consider it there.
|
||||
|
||||
This function polls the LinuxCNC Status buffer and monitors the
|
||||
position field, waiting for the specified axis to come to rest
|
||||
at the specified target. If this does not happen before the
|
||||
timeout, the function raises LinuxCNC_Exception.
|
||||
"""
|
||||
|
||||
axis_letter = axis_letter.lower()
|
||||
axis_index = 'xyzabcuvw'.index(axis_letter)
|
||||
|
||||
self.status.poll()
|
||||
start = time.time()
|
||||
|
||||
while ((time.time() - start) < timeout):
|
||||
prev_pos = self.status.position[axis_index]
|
||||
self.status.poll()
|
||||
vel = self.status.position[axis_index] - prev_pos
|
||||
error = math.fabs(self.status.position[axis_index] - target)
|
||||
if (error < tolerance) and (vel == 0):
|
||||
print("axis %s stopped at %.3f" % (axis_letter, target))
|
||||
return
|
||||
time.sleep(0.1)
|
||||
raise LinuxCNC_Exception("timeout waiting for axis %s to stop at %.3f (pos=%.3f, vel=%.3f)" % (axis_letter, target, self.status.position[axis_index], vel))
|
||||
|
||||
|
||||
def wait_for_interp_state(self, target_state, timeout=10.0):
|
||||
"""
|
||||
Arguments:
|
||||
'target_state' is the Interpreter state to wait for,
|
||||
one of linuxcnc.INTERP_IDLE, linuxcnc.INTERP_PAUSED,
|
||||
linuxcnc.INTERP_READING, or linuxcnc.INTERP_WAITING.
|
||||
|
||||
'timeout' is a float, the number of seconds to wait for the
|
||||
Interpreter to reach the target state.
|
||||
|
||||
This function polls the LinuxCNC Status buffer waiting for the
|
||||
specified Interpreter state. If the timeout expires before
|
||||
the Interpreter reaches the specified status, it raises
|
||||
LinuxCNC_Exception.
|
||||
"""
|
||||
|
||||
start_time = time.time()
|
||||
while (time.time() - start_time) < timeout:
|
||||
self.status.poll()
|
||||
if self.status.interp_state == target_state:
|
||||
return
|
||||
time.sleep(0.001)
|
||||
|
||||
if self.status.interp_state != target_state:
|
||||
raise LinuxCNC_Exception("interpreter state %d did not reach target state %d" % (self.state.interp_state, target_state))
|
||||
|
||||
|
||||
def wait_for_tool_in_spindle(self, expected_tool, timeout=10.0):
|
||||
"""
|
||||
Arguments:
|
||||
'expected_tool', integer, the tool we're waiting to find in
|
||||
the spindle.
|
||||
|
||||
'timeout', float, the number of seconds to wait for the
|
||||
specified tool to show up in the spindle.
|
||||
|
||||
This function polls the LinuxCNC Status buffer, waiting for the
|
||||
specified tool to appear in the spindle. If the timeout expires
|
||||
before the tool appears, it raises LinuxCNC_Exception.
|
||||
"""
|
||||
|
||||
start_time = time.time()
|
||||
while (time.time() - start_time) < timeout:
|
||||
time.sleep(0.1)
|
||||
self.status.poll()
|
||||
if self.status.tool_in_spindle == expected_tool:
|
||||
print("the Stat buffer's toolInSpindle reached the value of %d after %f seconds" % (expected_tool, time.time() - start_time))
|
||||
return
|
||||
raise LinuxCNC_Exception("the Stat buffer's toolInSpindle value is %d, expected %d" % (self.status.tool_in_spindle, expected_tool))
|
||||
|
||||
Reference in New Issue
Block a user