def insert_z_probe(self):
gz1 = config.SIZE_Z
gz2 = -2
gz3 = 3
if config.Z_INVERT:
gz1 *= -1
gz2 *= -1
gz3 *= -1
self.__add_action(action.MCUCmd("M996", sender=self.table_sender))
self.__add_action(action.MCUCmd("M998", sender=self.table_sender))
self.__add_action(
linear.LinearMovement(delta=euclid3.Vector3(0, 0, gz1),
feed=config.MAXFEED,
acc=config.ACCELERATION,
sender=self.table_sender))
self.__add_action(action.MCUCmd("M998", sender=self.table_sender))
self.__add_action(
linear.LinearMovement(delta=euclid3.Vector3(0, 0, gz2),
feed=config.MAXFEED,
acc=config.ACCELERATION,
sender=self.table_sender))
self.__add_action(action.MCUCmd("M998", sender=self.table_sender))
self.__add_action(
linear.LinearMovement(delta=euclid3.Vector3(0, 0, gz3),
feed=config.PRECISE_FEED,
acc=config.ACCELERATION,
sender=self.table_sender))
self.__add_action(action.MCUCmd("M998", sender=self.table_sender))
self.__add_action(action.MCUCmd("M995", sender=self.table_sender))
def __init__(self, location=euclid.Vector3(0.0, 0.0, 0.0), group="default"):
# if a list or a tuple is passed in, convert it to a Vector3
if type(location).__name__=='list' or type(location).__name__=='tuple':
location = euclid.Vector3(location[0], location[1], location[2])
self.location = location
self.group = group
def __fill_max_feed_01(self, actions):
# set maximal feed0 and feed1
for i in range(len(actions)):
action = actions[i][0]
dir0 = actions[i][1]["dir0"]
if i > 0:
prevdir = actions[i - 1][1]["dir1"]
prevmf = actions[i - 1][0].max_feed
else:
prevdir = euclid3.Vector3(0, 0, 0)
prevmf = 0
action.max_feed0 = min(
[self.__max_feed_jerk(prevdir, dir0), action.max_feed, prevmf])
dir1 = actions[i][1]["dir1"]
if i < len(actions) - 1:
nextdir = actions[i + 1][1]["dir0"]
nextmf = actions[i + 1][0].max_feed
else:
nextdir = euclid3.Vector3(0, 0, 0)
nextmf = 0
action.max_feed1 = min(
[self.__max_feed_jerk(nextdir, dir1), action.max_feed, nextmf])
def __process_move(self, frame):
self.table_state.process_frame(frame)
pos = arguments.Positioning(frame)
if pos is not None and self.table_state.modals.units is positioning.Configuration.UnitsGroup.inches:
pos.as_inches()
feed = arguments.Feed(frame)
stop = arguments.ExactStop(frame)
tool = arguments.Tool(frame)
no_motion = False
for cmd in frame.commands:
if cmd.type == "G":
if cmd.value == 74:
self.program.insert_homing()
self.table_state.pos.x = 0
self.table_state.pos.y = 0
self.table_state.pos.z = 0
self.program.insert_reset_coordinates(
x=self.table_state.pos.x,
y=self.table_state.pos.y,
z=self.table_state.pos.z)
elif cmd.value == 30:
self.program.insert_z_probe()
self.table_state.pos.z = 0
self.program.insert_reset_coordinates(
z=self.table_state.pos.z)
elif cmd.value == 92:
# set offset registers
self.__set_coordinates(x=pos.X, y=pos.Y, z=pos.Z)
cs = self.table_state.modals.coord_system
csstr = str(cs)
offset = self.table_state.modals.offsets[cs]
offset = euclid3.Vector3(offset.x, offset.y, offset.z)
self.program.insert_coordinate_system_change(csstr, offset)
no_motion = True
elif cmd.value >= 53 and cmd.value <= 59:
cs = self.table_state.modals.coord_system
csstr = str(cs)
offset = self.table_state.modals.offsets[cs]
offset = euclid3.Vector3(offset.x, offset.y, offset.z)
self.program.insert_coordinate_system_change(csstr, offset)
elif cmd.type == "M":
if cmd.value == 6 and tool.tool != None:
self.program.insert_select_tool(tool.tool,
self.tool_select_cb)
self.table_state.select_tool(tool.tool)
if feed.feed != None:
self.__set_feed(feed.feed)
if pos.is_moving and not no_motion:
self.table_state.pos = self.program.insert_move(
pos, self.table_state)
#print("Pos = ", self.table_state.pos)
if stop.exact_stop is True:
self.program.insert_stop()
def __init__(self):
# global coordinates
self.pos = euclid3.Vector3(0, 0, 0)
# modals
self.modals = Configuration()
self.mstack = []
def import_mesh(output_model,
mesh_name,
blender_object,
use_vertex_groups=True):
blender_mesh = blender_object.data
log.info("Processing mesh: ", mesh_name)
output_mesh = output_model.addMesh(mesh_name)
for vertex in blender_mesh.vertices:
group = "group"
if use_vertex_groups and len(vertex.groups) == 1:
group = blender_object.vertex_groups[vertex.groups[0].group].name
position = euclid.Vector3(vertex.co.x, vertex.co.y, vertex.co.z)
output_mesh.addVertex(position, group)
for polygon in blender_mesh.polygons:
uvlist = None
if len(blender_mesh.uv_layers) > 0:
uv_data = blender_mesh.uv_layers[0].data
uvlist = [
uv_data[polygon.loop_start + i].uv
for i in range(0, polygon.loop_total)
]
# Here we need to specify normals per-polygon, as opposed
# to per-vertex.
normals = [
blender_mesh.vertices[vertex].normal.normalized() * 0.95
for vertex in polygon.vertices
]
material = blender_mesh.materials[polygon.material_index].name
smooth_shading = polygon.use_smooth
output_mesh.addPolygon(polygon.vertices, uvlist, normals, material,
smooth_shading)
def __init__(self, d):
self.delta = euclid3.Vector3(d, 0, 0)
self.feed = 600
self.feed0 = 0
self.feed1 = 0
self.is_moving = True
self.exact_stop = False
def update_current_coordinate_system(self, cs, shift=None):
self.coordinate_system = cs
if cs == "G53" or shift is None:
self.__local_shift = euclid3.Vector3()
else:
self.__local_shift = shift
print("LOCAL SHIFT = ", shift)
self.current_crds = self.global_crds - self.__local_shift
def __convert_hw(self, hw):
hwc = euclid3.Vector3(hw["x"], hw["y"], hw["z"])
if config.X_INVERT:
hwc.x = -hwc.x
if config.Y_INVERT:
hwc.y = -hwc.y
if config.Z_INVERT:
hwc.z = -hwc.z
return hwc
def bounding_sphere(self):
# returns the center of the object, and the magnitude of the furthest
# point from that center.
all_points = []
all_points.extend(self.vertices)
midpoint = sum([point.location
for point in all_points],
euclid.Vector3()) / len(all_points)
radius = max( abs( abs(point.location - midpoint) ) for point in all_points)
return midpoint, radius
def read(self, filename):
with open(filename) as fp:
for line in fp.readlines():
self.process_command(self.remove_comments(line))
# ok, now we have the obj read in, convert it to a model
object = model.Model()
#add the materials to the model
for k in self.materials.keys():
object.addMaterial(k, self.materials[k]["ambient"],
self.materials[k]["specular"],
self.materials[k]["diffuse"])
# First, add the vertecies
for point in self.v:
object.addVertex(euclid.Vector3(point['x'], point['y'],
point['z']))
# for each polygon in the model, add the appropriate
# data to the model
for face in self.f:
# build a list of vertecies for this face
points = []
uvlist = []
normals = []
for point in face["points"]:
# todo: make sure this data is valid!
points.append(point['v'])
# do we have a uvlist?
uvlist = []
if (point['vt'] != None):
uvlist.append((
self.vt[point['vt']]['x'],
self.vt[point['vt']]['y'],
))
if len(uvlist) == 0:
uvlist = None
#similarly, do we have a Normals list?
if (point['vn'] != None):
normals.append((
self.vn[point['vn']]['x'],
self.vn[point['vn']]['y'],
self.vn[point['vn']]['z'],
))
if len(normals) == 0:
normals = None
#object.polygons.append(model.Model.Polygon(points, uvlist))
object.addPoly(points, uvlist, normals,
face["material"]) # todo: vertex normals here
return object
def WorkReset(self):
print("RESET")
act = system.TableReset(sender=self.table_sender)
act.run()
self.reset = True
self.table_sender.reset()
self.spindle_sender.stop()
self.state = None
self.builder = None
if self.program is not self.user_program and self.program is not self.empty_program:
self.program.dispose()
self.program = self.empty_program
self.hw_coordinates = euclid3.Vector3(0, 0, 0)
def addPolygon(self, vertex_list=None, uvlist=None, vertex_normals=None, material=None, smooth=True):
face_normal = self.face_normal(vertex_list)
#if this is a 2-point polygon, turn it into a triangle; this will draw
#on hardware as a perfect line segment
if abs(self.vertices[vertex_list[0]].location - self.vertices[vertex_list[1]].location) < 0.01:
log.debug("Encountered LINE SEGMENT variant 1")
vertex_list = [vertex_list[0], vertex_list[2], vertex_list[2]]
vertex_normals = [vertex_normals[0], vertex_normals[2], vertex_normals[2]]
face_normal = euclid.Vector3(vertex_normals[0][0],vertex_normals[0][1],vertex_normals[0][2]) #pick one at random
elif abs(self.vertices[vertex_list[1]].location - self.vertices[vertex_list[2]].location) < 0.01:
log.debug("Encountered LINE SEGMENT variant 2")
vertex_list = [vertex_list[0], vertex_list[1], vertex_list[1]]
vertex_normals = [vertex_normals[0], vertex_normals[1], vertex_normals[1]]
face_normal = euclid.Vector3(vertex_normals[0][0],vertex_normals[0][1],vertex_normals[0][2])
elif abs(self.vertices[vertex_list[2]].location - self.vertices[vertex_list[0]].location) < 0.01:
log.debug("Encountered LINE SEGMENT variant 3")
vertex_list = [vertex_list[0], vertex_list[1], vertex_list[1]]
vertex_normals = [vertex_normals[0], vertex_normals[1], vertex_normals[1]]
face_normal = euclid.Vector3(vertex_normals[0][0],vertex_normals[0][1],vertex_normals[0][2])
self.polygons.append(Model.Polygon(vertex_list, uvlist, material,
face_normal, vertex_normals, self, smooth))
def find_geometry(source, target, ccw, axis, **kwargs):
d, h = HelixMovement.__get_d_h(target - source, axis)
# Now we don't support 'h'!
if abs(h) > 1e-4:
raise Exception("Arc don't support 'h'!")
if "r" in kwargs.keys():
radius = kwargs["r"]
start_to_center, tan0, tan1, arc_angle = HelixMovement.__find_geometry_from_r(
d, radius, ccw)
else:
if axis == HelixMovement.Axis.xy:
start_to_center = euclid3.Vector2(kwargs["i"], kwargs["j"])
elif axis == HelixMovement.Axis.yz:
start_to_center = euclid3.Vector2(kwargs["j"], kwargs["k"])
else:
start_to_center = euclid3.Vector2(kwargs["k"], kwargs["i"])
radius, tan0, tan1, arc_angle = HelixMovement.__find_geometry_from_ijk(
d, start_to_center, ccw)
if axis == HelixMovement.Axis.xy:
dir_0 = euclid3.Vector3(tan0.x, tan0.y, 0)
dir_1 = euclid3.Vector3(tan1.x, tan1.y, 0)
center = source + euclid3.Vector3(start_to_center[0],
start_to_center[1], 0)
elif axis == HelixMovement.Axis.yz:
dir_0 = euclid3.Vector3(0, tan0.x, tan0.y)
dir_1 = euclid3.Vector3(0, tan1.x, tan1.y)
center = source + euclid3.Vector3(0, start_to_center[0],
start_to_center[1])
elif axis == HelixMovement.Axis.zx:
dir_0 = euclid3.Vector3(tan0.y, 0, tan0.x)
dir_1 = euclid3.Vector3(tan1.y, 0, tan1.x)
center = source + euclid3.Vector3(start_to_center[1], 0,
start_to_center[0])
return center, dir_0, dir_1, arc_angle
def __init__(self, table_sender, spindle_sender):
print("Creating machine...")
self.registers = {"tools": {}}
self.is_running = False
self.is_finished = False
self.table_sender = table_sender
self.spindle_sender = spindle_sender
self.running = event.EventEmitter()
self.paused = event.EventEmitter()
self.finished = event.EventEmitter()
self.error = event.EventEmitter()
self.line_selected = event.EventEmitter()
self.tool_selected = event.EventEmitter()
self.on_coordinates = event.EventEmitter()
self.reset = False
self.action_rdy = threading.Event()
self.__table_reseted_ev = threading.Event()
self.current_wait = None
self.c_actions = []
self.nc_action = None
self.opt = Optimizer(common.config.JERKING, common.config.ACCELERATION,
common.config.MAXFEED)
# loaded program
self.user_program = None
self.user_frames = []
# actual program
self.program = None
self.state = None
self.builder = None
# special programs
self.empty_program = pr.Program(self.table_sender, self.spindle_sender)
# states
self.previous_state = None
self.work_init(self.empty_program)
self.table_sender.mcu_reseted += self.__table_reseted
# current states
self.runtime_state = runtimestate.RuntimeState({
"x": 0,
"y": 0,
"z": 0
}, euclid3.Vector3())
print("done")
def get_euler(self):
q = euclid3.Quaternion(self.q0, self.q1, self.q2, self.q3)
euler = q.get_euler()
return euclid3.Vector3(euler[2] * 57.29577951, euler[0] * 57.29577951,
euler[1] * 57.29577951)
def get_gyro(self):
return euclid3.Vector3(self.gx, self.gy, self.gz)
def get_acc(self):
return euclid3.Vector3(self.ax, self.ay, self.az)
def addVertex(self, location=euclid.Vector3(0.0, 0.0, 0.0), group="default"):
self.vertices.append(Model.Vertex(location, group))
if group not in self.model.groups:
self.model.groups.append(group)
def insert_homing(self):
gz1 = -config.SIZE_Z
gz2 = 2
gz3 = -3
if config.Z_INVERT:
gz1 *= -1
gz2 *= -1
gz3 *= -1
gx1 = -config.SIZE_X
gx2 = 2
gx3 = -3
if config.X_INVERT:
gx1 *= -1
gx2 *= -1
gx3 *= -1
gy1 = -config.SIZE_Y
gy2 = 2
gy3 = -3
if config.Y_INVERT:
gy1 *= -1
gy2 *= -1
gy3 *= -1
self.__add_action(action.MCUCmd("M998", sender=self.table_sender))
self.__add_action(
linear.LinearMovement(delta=euclid3.Vector3(0, 0, gz1),
feed=config.MAXFEED,
acc=config.ACCELERATION,
sender=self.table_sender))
self.__add_action(action.MCUCmd("M998", sender=self.table_sender))
self.__add_action(
linear.LinearMovement(delta=euclid3.Vector3(0, 0, gz2),
feed=config.MAXFEED,
acc=config.ACCELERATION,
sender=self.table_sender))
self.__add_action(action.MCUCmd("M998", sender=self.table_sender))
self.__add_action(
linear.LinearMovement(delta=euclid3.Vector3(0, 0, gz3),
feed=config.PRECISE_FEED,
acc=config.ACCELERATION,
sender=self.table_sender))
self.__add_action(action.MCUCmd("M998", sender=self.table_sender))
self.__add_action(
linear.LinearMovement(delta=euclid3.Vector3(gx1, 0, 0),
feed=config.MAXFEED,
acc=config.ACCELERATION,
sender=self.table_sender))
self.__add_action(action.MCUCmd("M998", sender=self.table_sender))
self.__add_action(
linear.LinearMovement(delta=euclid3.Vector3(gx2, 0, 0),
feed=config.MAXFEED,
acc=config.ACCELERATION,
sender=self.table_sender))
self.__add_action(action.MCUCmd("M998", sender=self.table_sender))
self.__add_action(
linear.LinearMovement(delta=euclid3.Vector3(gx3, 0, 0),
feed=config.PRECISE_FEED,
acc=config.ACCELERATION,
sender=self.table_sender))
self.__add_action(action.MCUCmd("M998", sender=self.table_sender))
self.__add_action(
linear.LinearMovement(delta=euclid3.Vector3(0, gy1, 0),
feed=config.MAXFEED,
acc=config.ACCELERATION,
sender=self.table_sender))
self.__add_action(action.MCUCmd("M998", sender=self.table_sender))
self.__add_action(
linear.LinearMovement(delta=euclid3.Vector3(0, gy2, 0),
feed=config.MAXFEED,
acc=config.ACCELERATION,
sender=self.table_sender))
self.__add_action(action.MCUCmd("M998", sender=self.table_sender))
self.__add_action(
linear.LinearMovement(delta=euclid3.Vector3(0, gy3, 0),
feed=config.PRECISE_FEED,
acc=config.ACCELERATION,
sender=self.table_sender))
self.__add_action(action.MCUCmd("M998", sender=self.table_sender))
self.__add_action(action.MCUCmd("M997", sender=self.table_sender))
def insert_move(self, pos, table_state):
print("*** Insert move ", pos.X, pos.Y, pos.Z)
#traceback.print_stack()
#print(table_state.positioning)
if table_state.modals.positioning == positioning.Configuration.PositioningGroup.absolute:
cs = table_state.modals.offsets[table_state.modals.coord_system]
origpos = table_state.pos
origpos = cs.global2local(origpos.x, origpos.y, origpos.z)
newpos = origpos
if pos.X != None:
newpos = euclid3.Vector3(pos.X, newpos.y, newpos.z)
if pos.Y != None:
newpos = euclid3.Vector3(newpos.x, pos.Y, newpos.z)
if pos.Z != None:
newpos = euclid3.Vector3(newpos.x, newpos.y, pos.Z)
delta = newpos - origpos
else:
delta = euclid3.Vector3()
if pos.X != None:
delta.x = pos.X
if pos.Y != None:
delta.y = pos.Y
if pos.Z != None:
delta.z = pos.Z
source = table_state.pos
target = source + delta
offset = 0
# Normal linear move
if table_state.modals.motion == positioning.Configuration.MotionGroup.line:
self.__insert_payload_movement(source, target, offset, table_state)
# Fast linear move
elif table_state.modals.motion == positioning.Configuration.MotionGroup.fast_move:
self.__insert_fast_movement(source, target, offset, table_state)
# Arc movement
elif table_state.modals.motion == positioning.Configuration.MotionGroup.round_cw or \
table_state.modals.motion == positioning.Configuration.MotionGroup.round_ccw:
if pos.R != None:
self.__insert_arc_R(source, target, offset, pos.R, table_state)
else:
ci = 0
cj = 0
ck = 0
if pos.I != None:
ci = pos.I
if pos.J != None:
cj = pos.J
if pos.K != None:
ck = pos.K
self.__insert_arc_IJK(source, target, offset, ci, cj, ck,
table_state)
else:
raise Exception("Not implemented %s motion state" %
table_state.modals.motion)
return target
def update_global_coordinates(self, hw, glob):
self.hw_crds = euclid3.Vector3(hw["x"], hw["y"], hw["z"])
self.__hwc_crds = self.__convert_hw(hw)
self.global_crds = glob
self.__shift = self.global_crds - self.__hwc_crds
def global2local(self, x, y, z):
return euclid3.Vector3(x - self.x, y - self.y, z - self.z)
def update_coordinates(self, hw):
self.hw_crds = euclid3.Vector3(hw["x"], hw["y"], hw["z"])
self.__hwc_crds = self.__convert_hw(hw)
self.global_crds = self.__hwc_crds + self.__shift
self.current_crds = self.global_crds - self.__local_shift
def get_acc_earth(self):
q = euclid3.Quaternion(self.q0, self.q1, self.q2, self.q3)
a = euclid3.Vector3(self.ax, self.ay, self.az)
e = q * a
return euclid3.Vector3(e.x, e.y, e.z)
def __init__(self, hw, glob):
self.coordinate_system = "G53"
self.__local_shift = euclid3.Vector3()
self.update_global_coordinates(hw, glob)
def local2global(self, x, y, z):
return euclid3.Vector3(x + self.x, y + self.y, z + self.z)
def process_mesh(self, object, mesh):
# Import polygon and vertex data.
log.debug("Polygons: %d", mesh.GetPolygonCount())
log.debug("Verticies: %d", len(mesh.GetControlPoints()))
#this list contains all the points in the model; polygons will
#index into this list
vertex_list = mesh.GetControlPoints()
#add the verticies to the model
for i in range(len(vertex_list)):
object.addVertex(
euclid.Vector3(vertex_list[i][0], vertex_list[i][1],
vertex_list[i][2]))
#do something about materials
self.process_materials(object, mesh)
material_map = mesh.GetLayer(0).GetMaterials().GetIndexArray()
log.debug("Mesh Global Transform:")
log.debug(fbx_to_euclid(mesh.GetNode().EvaluateGlobalTransform()))
#exit()
#well ... that explains a lot.
self.mesh_global = fbx_to_euclid(
mesh.GetNode().EvaluateGlobalTransform())
object.global_matrix = self.mesh_global
for face in range(mesh.GetPolygonCount()):
#this importer only supports triangles and
#quads, so we need to throw out any weird
#sizes here
vertex_count = mesh.GetPolygonSize(face)
if vertex_count >= 3 and vertex_count <= 4:
points = []
normals = []
uvlist = []
for v in range(vertex_count):
points.append(mesh.GetPolygonVertex(face, v))
#figure out if there's normal data?
#TODO: Why does this need to loop over layer data? Investigate!
for l in range(mesh.GetLayerCount()):
normal_data = mesh.GetLayer(l).GetNormals()
if normal_data:
normal = normal_data.GetDirectArray().GetAt(v)
#print(normal)
normals.append((normal[0], normal[1], normal[2]))
uv_data = mesh.GetLayer(l).GetUVs()
if uv_data:
if uv_data.GetMappingMode(
) == FbxLayerElement.eByControlPoint:
log.warn(
"eByControlPoint not supported for UVs!")
elif uv_data.GetMappingMode(
) == FbxLayerElement.eByPolygonVertex:
uv_index = mesh.GetTextureUVIndex(face, v)
uv = uv_data.GetDirectArray().GetAt(uv_index)
#print("UVs: ", uv)
uvlist.append((uv[0], uv[1]))
#todo: not discard UV coordinates here
if len(uvlist) == 0:
uvlist = None
object.addPoly(
points, uvlist, normals,
mesh.GetNode().GetMaterial(
material_map.GetAt(face)).GetName())
self.process_clusters(object, mesh)
